|
|
@@ -0,0 +1,592 @@
|
|
|
+/**
|
|
|
+ * Copyright (c) 2014, James Nutaro
|
|
|
+ * All rights reserved.
|
|
|
+ *
|
|
|
+ * Redistribution and use in source and binary forms, with or without
|
|
|
+ * modification, are permitted provided that the following conditions are met:
|
|
|
+ *
|
|
|
+ * 1. Redistributions of source code must retain the above copyright notice, this
|
|
|
+ * list of conditions and the following disclaimer.
|
|
|
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
|
|
|
+ * this list of conditions and the following disclaimer in the documentation
|
|
|
+ * and/or other materials provided with the distribution.
|
|
|
+ *
|
|
|
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
|
|
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
|
|
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
|
|
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
|
|
|
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
|
|
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
|
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
|
|
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
|
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
|
|
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
|
+ *
|
|
|
+ * The views and conclusions contained in the software and documentation are those
|
|
|
+ * of the authors and should not be interpreted as representing official policies,
|
|
|
+ * either expressed or implied, of the FreeBSD Project.
|
|
|
+ *
|
|
|
+ * Bugs, comments, and questions can be sent to nutaro@gmail.com
|
|
|
+ */
|
|
|
+#ifndef _adevs_fmi_h_
|
|
|
+#define _adevs_fmi_h_
|
|
|
+#include <algorithm>
|
|
|
+#include <cmath>
|
|
|
+#include <iostream>
|
|
|
+#include <cstdlib>
|
|
|
+#include <cstdio>
|
|
|
+#include <cassert>
|
|
|
+#include "adevs_hybrid.h"
|
|
|
+#include "fmi2Functions.h"
|
|
|
+#include "fmi2FunctionTypes.h"
|
|
|
+#include "fmi2TypesPlatform.h"
|
|
|
+
|
|
|
+// Functions for loading DLL and so files
|
|
|
+#ifdef _WIN32
|
|
|
+#include <windows.h>
|
|
|
+#define OPEN_LIB(name) LoadLibrary(name)
|
|
|
+#define GET_FUNC(hndl,name) GetProcAddress(hndl,name)
|
|
|
+#define CLOSE_LIB(hndl) FreeLibrary(hndl)
|
|
|
+#else
|
|
|
+#include <dlfcn.h>
|
|
|
+#define OPEN_LIB(name) dlopen(name,RTLD_LAZY)
|
|
|
+#define GET_FUNC(hndl,name) dlsym(hndl,name)
|
|
|
+#define CLOSE_LIB(hndl) dlclose(hndl)
|
|
|
+#endif
|
|
|
+
|
|
|
+namespace adevs
|
|
|
+{
|
|
|
+
|
|
|
+/**
|
|
|
+ * Load an FMI wrapped continuous system model for use in a
|
|
|
+ * discrete event simulation. The FMI can then be attached
|
|
|
+ * to any of the ODE solvers and event detectors in adevs
|
|
|
+ * for simulation with the Hybrid class. This FMI loader
|
|
|
+ * does not automatically extract model information from the
|
|
|
+ * description XML, and so that information must be provided
|
|
|
+ * explicitly by the end-user, but you probably need to know
|
|
|
+ * this information regardless if you are using the FMI inside
|
|
|
+ * of a larger discrete event simulation.
|
|
|
+ */
|
|
|
+template <typename X> class FMI:
|
|
|
+ public ode_system<X>
|
|
|
+{
|
|
|
+ public:
|
|
|
+ /**
|
|
|
+ * This constructs a wrapper around an FMI. The constructor
|
|
|
+ * must be provided with the FMI's GUID, the number of state variables,
|
|
|
+ * number of event indicators, and the path to the .so file
|
|
|
+ * that contains the FMI functions for this model.
|
|
|
+ */
|
|
|
+ FMI(const char* modelname,
|
|
|
+ const char* guid,
|
|
|
+ const char* resource_location,
|
|
|
+ int num_state_variables,
|
|
|
+ int num_event_indicators,
|
|
|
+ const char* shared_lib_name,
|
|
|
+ const double tolerance = 1E-8,
|
|
|
+ int num_extra_event_indicators = 0,
|
|
|
+ double start_time = 0.0);
|
|
|
+ /// Copy the initial state of the model to q
|
|
|
+ virtual void init(double* q);
|
|
|
+ /// Compute the derivative for state q and put it in dq
|
|
|
+ virtual void der_func(const double* q, double* dq);
|
|
|
+ /// Compute the state event functions for state q and put them in z
|
|
|
+ virtual void state_event_func(const double* q, double* z);
|
|
|
+ /// Compute the time event function using state q
|
|
|
+ virtual double time_event_func(const double* q);
|
|
|
+ /**
|
|
|
+ * This method is invoked immediately following an update of the
|
|
|
+ * continuous state variables and signal to the FMI the end
|
|
|
+ * of an integration state.
|
|
|
+ */
|
|
|
+ virtual void postStep(double* q);
|
|
|
+ /**
|
|
|
+ * Like the postStep() method, but this is called at the end
|
|
|
+ * of a trial step made by the numerical integrator. Trial steps
|
|
|
+ * are used to locate state events and to estimate numerical
|
|
|
+ * errors. The state vector q passed to this method may not be
|
|
|
+ * the final vector assigned to the model at the end of
|
|
|
+ * the current integration step. The get that value use
|
|
|
+ * the postStep() method.
|
|
|
+ */
|
|
|
+ virtual void postTrialStep(double* q);
|
|
|
+ /**
|
|
|
+ * The internal transition function. This function will process all events
|
|
|
+ * required by the FMI. Any derived class should call this method for the
|
|
|
+ * parent class, then set or get any variables as appropriate, and then
|
|
|
+ * call the base class method again to account for these changes.
|
|
|
+ */
|
|
|
+ virtual void internal_event(double* q,
|
|
|
+ const bool* state_event);
|
|
|
+ /**
|
|
|
+ * The external transition See the notes on the internal_event function for
|
|
|
+ * derived classes.
|
|
|
+ */
|
|
|
+ virtual void external_event(double* q, double e,
|
|
|
+ const Bag<X>& xb);
|
|
|
+ /**
|
|
|
+ * The confluent transition function. See the notes on the internal_event function for
|
|
|
+ * derived classes.
|
|
|
+ */
|
|
|
+ virtual void confluent_event(double *q, const bool* state_event,
|
|
|
+ const Bag<X>& xb);
|
|
|
+ /**
|
|
|
+ * The output function. This can read variables for the FMI, but should
|
|
|
+ * not make any modifications to those variables.
|
|
|
+ */
|
|
|
+ virtual void output_func(const double *q, const bool* state_event,
|
|
|
+ Bag<X>& yb);
|
|
|
+ /**
|
|
|
+ * Garbage collection function. This works just like the Atomic gc_output method.
|
|
|
+ * The default implementation does nothing.
|
|
|
+ */
|
|
|
+ virtual void gc_output(Bag<X>& gb);
|
|
|
+ /// Destructor
|
|
|
+ virtual ~FMI();
|
|
|
+ /// Get the current time
|
|
|
+ double get_time() const { return t_now; }
|
|
|
+ /// Get the value of a real variable
|
|
|
+ double get_real(int k);
|
|
|
+ /// Set the value of a real variable
|
|
|
+ void set_real(int k, double val);
|
|
|
+ /// Get the value of an integer variable
|
|
|
+ int get_int(int k);
|
|
|
+ /// Set the value of an integer variable
|
|
|
+ void set_int(int k, int val);
|
|
|
+ /// Get the value of a boolean variable
|
|
|
+ bool get_bool(int k);
|
|
|
+ /// Set the value of a boolean variable
|
|
|
+ void set_bool(int k, bool val);
|
|
|
+ /// Get the value of a string variable
|
|
|
+ std::string get_string(int k);
|
|
|
+ /// Set the value of a string variable
|
|
|
+ void set_string(int k, std::string& val);
|
|
|
+ private:
|
|
|
+ // Reference to the FMI
|
|
|
+ fmi2Component c;
|
|
|
+ // Pointer to the FMI interface
|
|
|
+ fmi2Component (*_fmi2Instantiate)(fmi2String, fmi2Type,
|
|
|
+ fmi2String, fmi2String, const fmi2CallbackFunctions*,
|
|
|
+ fmi2Boolean, fmi2Boolean);
|
|
|
+ void (*_fmi2FreeInstance)(fmi2Component);
|
|
|
+ fmi2Status (*_fmi2SetupExperiment)(fmi2Component, fmi2Boolean,
|
|
|
+ fmi2Real, fmi2Real, fmi2Boolean, fmi2Real);
|
|
|
+ fmi2Status (*_fmi2EnterInitializationMode)(fmi2Component);
|
|
|
+ fmi2Status (*_fmi2ExitInitializationMode)(fmi2Component);
|
|
|
+ fmi2Status (*_fmi2GetReal)(fmi2Component, const fmi2ValueReference*, size_t, fmi2Real*);
|
|
|
+ fmi2Status (*_fmi2GetInteger)(fmi2Component, const fmi2ValueReference*, size_t, fmi2Integer*);
|
|
|
+ fmi2Status (*_fmi2GetBoolean)(fmi2Component, const fmi2ValueReference*, size_t, fmi2Boolean*);
|
|
|
+ fmi2Status (*_fmi2GetString)(fmi2Component, const fmi2ValueReference*, size_t, fmi2String*);
|
|
|
+ fmi2Status (*_fmi2SetReal)(fmi2Component, const fmi2ValueReference*, size_t, const fmi2Real*);
|
|
|
+ fmi2Status (*_fmi2SetInteger)(fmi2Component, const fmi2ValueReference*, size_t, const fmi2Integer*);
|
|
|
+ fmi2Status (*_fmi2SetBoolean)(fmi2Component, const fmi2ValueReference*, size_t, const fmi2Boolean*);
|
|
|
+ fmi2Status (*_fmi2SetString)(fmi2Component, const fmi2ValueReference*, size_t, const fmi2String*);
|
|
|
+ fmi2Status (*_fmi2EnterEventMode)(fmi2Component);
|
|
|
+ fmi2Status (*_fmi2NewDiscreteStates)(fmi2Component,fmi2EventInfo*);
|
|
|
+ fmi2Status (*_fmi2EnterContinuousTimeMode)(fmi2Component);
|
|
|
+ fmi2Status (*_fmi2CompletedIntegratorStep)(fmi2Component, fmi2Boolean, fmi2Boolean*, fmi2Boolean*);
|
|
|
+ fmi2Status (*_fmi2SetTime)(fmi2Component, fmi2Real);
|
|
|
+ fmi2Status (*_fmi2SetContinuousStates)(fmi2Component, const fmi2Real*, size_t);
|
|
|
+ fmi2Status (*_fmi2GetDerivatives)(fmi2Component, fmi2Real*, size_t);
|
|
|
+ fmi2Status (*_fmi2GetEventIndicators)(fmi2Component, fmi2Real*, size_t);
|
|
|
+ fmi2Status (*_fmi2GetContinuousStates)(fmi2Component, fmi2Real*, size_t);
|
|
|
+
|
|
|
+ // Instant of the next time event
|
|
|
+ double next_time_event;
|
|
|
+ // Current time
|
|
|
+ double t_now;
|
|
|
+ // so library handle
|
|
|
+ #ifdef _WIN32
|
|
|
+ HINSTANCE so_hndl;
|
|
|
+ #else
|
|
|
+ void* so_hndl;
|
|
|
+ #endif
|
|
|
+ // Are we in continuous time mode?
|
|
|
+ bool cont_time_mode;
|
|
|
+ // Number of event indicators that are not governed by the FMI
|
|
|
+ int num_extra_event_indicators;
|
|
|
+ // Start time of the simulation
|
|
|
+ double start_time;
|
|
|
+
|
|
|
+ static void fmilogger(
|
|
|
+ fmi2ComponentEnvironment componentEnvironment,
|
|
|
+ fmi2String instanceName,
|
|
|
+ fmi2Status status,
|
|
|
+ fmi2String category,
|
|
|
+ fmi2String message,...)
|
|
|
+ {
|
|
|
+ if (message != NULL){
|
|
|
+
|
|
|
+ fprintf(stderr, message,"\n");
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ fmi2CallbackFunctions* callbackFuncs;
|
|
|
+
|
|
|
+ void iterate_events();
|
|
|
+};
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+FMI<X>::FMI(const char* modelname,
|
|
|
+ const char* guid,
|
|
|
+ const char* resource_location,
|
|
|
+ int num_state_variables,
|
|
|
+ int num_event_indicators,
|
|
|
+ const char* so_file_name,
|
|
|
+ const double tolerance,
|
|
|
+ int num_extra_event_indicators,
|
|
|
+ double start_time):
|
|
|
+ // One extra variable at the end for time
|
|
|
+ ode_system<X>(num_state_variables+1,num_event_indicators+num_extra_event_indicators),
|
|
|
+ next_time_event(adevs_inf<double>()),
|
|
|
+ t_now(start_time),
|
|
|
+ so_hndl(NULL),
|
|
|
+ cont_time_mode(false),
|
|
|
+ num_extra_event_indicators(num_extra_event_indicators)
|
|
|
+{
|
|
|
+ fmi2CallbackFunctions tmp = {adevs::FMI<X>::fmilogger,calloc,free,NULL,NULL};
|
|
|
+ callbackFuncs = new fmi2CallbackFunctions(tmp);
|
|
|
+ so_hndl = OPEN_LIB(so_file_name);
|
|
|
+ if (so_hndl == NULL)
|
|
|
+ {
|
|
|
+ throw adevs::exception("Could not load so file",this);
|
|
|
+ }
|
|
|
+ // This only works with a POSIX compliant compiler/system
|
|
|
+ _fmi2Instantiate = (fmi2Component (*)(fmi2String, fmi2Type,
|
|
|
+ fmi2String, fmi2String, const fmi2CallbackFunctions*,
|
|
|
+ fmi2Boolean, fmi2Boolean))GET_FUNC(so_hndl,"fmi2Instantiate");
|
|
|
+ assert(_fmi2Instantiate != NULL);
|
|
|
+ _fmi2FreeInstance = (void (*)(fmi2Component))GET_FUNC(so_hndl,"fmi2FreeInstance");
|
|
|
+ assert(_fmi2FreeInstance != NULL);
|
|
|
+ _fmi2SetupExperiment = (fmi2Status (*)(fmi2Component, fmi2Boolean,
|
|
|
+ fmi2Real, fmi2Real, fmi2Boolean, fmi2Real))GET_FUNC(so_hndl,"fmi2SetupExperiment");
|
|
|
+ assert(_fmi2SetupExperiment != NULL);
|
|
|
+ _fmi2EnterInitializationMode = (fmi2Status (*)(fmi2Component))GET_FUNC(so_hndl,"fmi2EnterInitializationMode");
|
|
|
+ assert(_fmi2EnterInitializationMode != NULL);
|
|
|
+ _fmi2ExitInitializationMode = (fmi2Status (*)(fmi2Component))GET_FUNC(so_hndl,"fmi2ExitInitializationMode");
|
|
|
+ assert(_fmi2ExitInitializationMode != NULL);
|
|
|
+ _fmi2GetReal = (fmi2Status (*)(fmi2Component, const fmi2ValueReference*, size_t, fmi2Real*))
|
|
|
+ GET_FUNC(so_hndl,"fmi2GetReal");
|
|
|
+ assert(_fmi2GetReal != NULL);
|
|
|
+ _fmi2GetInteger = (fmi2Status (*)(fmi2Component, const fmi2ValueReference*, size_t, fmi2Integer*))
|
|
|
+ GET_FUNC(so_hndl,"fmi2GetInteger");
|
|
|
+ assert(_fmi2GetInteger != NULL);
|
|
|
+ _fmi2GetBoolean = (fmi2Status (*)(fmi2Component, const fmi2ValueReference*, size_t, fmi2Boolean*))
|
|
|
+ GET_FUNC(so_hndl,"fmi2GetBoolean");
|
|
|
+ assert(_fmi2GetBoolean != NULL);
|
|
|
+ _fmi2GetString = (fmi2Status (*)(fmi2Component, const fmi2ValueReference*, size_t, fmi2String*))
|
|
|
+ GET_FUNC(so_hndl,"fmi2GetString");
|
|
|
+ assert(_fmi2GetString != NULL);
|
|
|
+ _fmi2SetReal = (fmi2Status (*)(fmi2Component, const fmi2ValueReference*, size_t, const fmi2Real*))
|
|
|
+ GET_FUNC(so_hndl,"fmi2SetReal");
|
|
|
+ assert(_fmi2SetReal != NULL);
|
|
|
+ _fmi2SetInteger = (fmi2Status (*)(fmi2Component, const fmi2ValueReference*, size_t, const fmi2Integer*))
|
|
|
+ GET_FUNC(so_hndl,"fmi2SetInteger");
|
|
|
+ assert(_fmi2SetInteger != NULL);
|
|
|
+ _fmi2SetBoolean = (fmi2Status (*)(fmi2Component, const fmi2ValueReference*, size_t, const fmi2Boolean*))
|
|
|
+ GET_FUNC(so_hndl,"fmi2SetBoolean");
|
|
|
+ assert(_fmi2SetBoolean != NULL);
|
|
|
+ _fmi2SetString = (fmi2Status (*)(fmi2Component, const fmi2ValueReference*, size_t, const fmi2String*))
|
|
|
+ GET_FUNC(so_hndl,"fmi2SetString");
|
|
|
+ assert(_fmi2SetString != NULL);
|
|
|
+ _fmi2EnterEventMode = (fmi2Status (*)(fmi2Component))GET_FUNC(so_hndl,"fmi2EnterEventMode");
|
|
|
+ assert(_fmi2EnterEventMode != NULL);
|
|
|
+ _fmi2NewDiscreteStates = (fmi2Status (*)(fmi2Component,fmi2EventInfo*))GET_FUNC(so_hndl,"fmi2NewDiscreteStates");
|
|
|
+ assert(_fmi2NewDiscreteStates != NULL);
|
|
|
+ _fmi2EnterContinuousTimeMode = (fmi2Status (*)(fmi2Component))GET_FUNC(so_hndl,"fmi2EnterContinuousTimeMode");
|
|
|
+ assert(_fmi2EnterContinuousTimeMode != NULL);
|
|
|
+ _fmi2CompletedIntegratorStep = (fmi2Status (*)(fmi2Component, fmi2Boolean, fmi2Boolean*, fmi2Boolean*))
|
|
|
+ GET_FUNC(so_hndl,"fmi2CompletedIntegratorStep");
|
|
|
+ assert(_fmi2CompletedIntegratorStep != NULL);
|
|
|
+ _fmi2SetTime = (fmi2Status (*)(fmi2Component, fmi2Real))GET_FUNC(so_hndl,"fmi2SetTime");
|
|
|
+ assert(_fmi2SetTime != NULL);
|
|
|
+ _fmi2SetContinuousStates = (fmi2Status (*)(fmi2Component, const fmi2Real*, size_t))
|
|
|
+ GET_FUNC(so_hndl,"fmi2SetContinuousStates");
|
|
|
+ assert(_fmi2SetContinuousStates != NULL);
|
|
|
+ _fmi2GetDerivatives = (fmi2Status (*)(fmi2Component, fmi2Real*, size_t))GET_FUNC(so_hndl,"fmi2GetDerivatives");
|
|
|
+ assert(_fmi2GetDerivatives != NULL);
|
|
|
+ _fmi2GetEventIndicators = (fmi2Status (*)(fmi2Component, fmi2Real*, size_t))GET_FUNC(so_hndl,"fmi2GetEventIndicators");
|
|
|
+ assert(_fmi2GetEventIndicators != NULL);
|
|
|
+ _fmi2GetContinuousStates = (fmi2Status (*)(fmi2Component, fmi2Real*, size_t))GET_FUNC(so_hndl,"fmi2GetContinuousStates");
|
|
|
+ assert(_fmi2GetContinuousStates != NULL);
|
|
|
+ // Create the FMI component
|
|
|
+ c = _fmi2Instantiate(modelname,fmi2ModelExchange,guid,resource_location,callbackFuncs,fmi2False,fmi2False);
|
|
|
+ assert(c != NULL);
|
|
|
+ _fmi2SetupExperiment(c,fmi2True,tolerance,-1.0,fmi2False,-1.0);
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+void FMI<X>::iterate_events()
|
|
|
+{
|
|
|
+ fmi2Status status;
|
|
|
+ // Put into consistent initial state
|
|
|
+ fmi2EventInfo eventInfo;
|
|
|
+ do
|
|
|
+ {
|
|
|
+ status = _fmi2NewDiscreteStates(c,&eventInfo);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ }
|
|
|
+ while (eventInfo.newDiscreteStatesNeeded == fmi2True);
|
|
|
+ if (eventInfo.nextEventTimeDefined == fmi2True)
|
|
|
+ next_time_event = eventInfo.nextEventTime;
|
|
|
+ else
|
|
|
+ next_time_event = adevs_inf<double>();
|
|
|
+ assert(status == fmi2OK);
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+void FMI<X>::init(double* q)
|
|
|
+{
|
|
|
+ fmi2Status status;
|
|
|
+ // Initialize all variables
|
|
|
+ status = _fmi2EnterInitializationMode(c);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ // Done with initialization
|
|
|
+ status = _fmi2ExitInitializationMode(c);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ // Put into consistent initial state
|
|
|
+ iterate_events();
|
|
|
+ // Enter continuous time mode to start integration
|
|
|
+ status = _fmi2EnterContinuousTimeMode(c);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ // Set initial value for time
|
|
|
+ status = _fmi2SetTime(c,t_now);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ // Get starting state variable values
|
|
|
+ status = _fmi2GetContinuousStates(c,q,this->numVars()-1);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ q[this->numVars()-1] = t_now;
|
|
|
+ cont_time_mode = true;
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+void FMI<X>::der_func(const double* q, double* dq)
|
|
|
+{
|
|
|
+ fmi2Status status;
|
|
|
+ if (!cont_time_mode)
|
|
|
+ {
|
|
|
+ status = _fmi2EnterContinuousTimeMode(c);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ cont_time_mode = true;
|
|
|
+ }
|
|
|
+ status =_fmi2SetTime(c,q[this->numVars()-1]);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ status = _fmi2SetContinuousStates(c,q,this->numVars()-1);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ status = _fmi2GetDerivatives(c,dq,this->numVars()-1);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ dq[this->numVars()-1] = 1.0;
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+void FMI<X>::state_event_func(const double* q, double* z)
|
|
|
+{
|
|
|
+ fmi2Status status;
|
|
|
+ if (!cont_time_mode)
|
|
|
+ {
|
|
|
+ status = _fmi2EnterContinuousTimeMode(c);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ cont_time_mode = true;
|
|
|
+ }
|
|
|
+ status = _fmi2SetTime(c,q[this->numVars()-1]);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ status = _fmi2SetContinuousStates(c,q,this->numVars()-1);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ status = _fmi2GetEventIndicators(c,z,this->numEvents()-num_extra_event_indicators);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+double FMI<X>::time_event_func(const double* q)
|
|
|
+{
|
|
|
+ return next_time_event-q[this->numVars()-1];
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+void FMI<X>::postStep(double* q)
|
|
|
+{
|
|
|
+ assert(cont_time_mode);
|
|
|
+ // Don't advance the FMI state by zero units of time
|
|
|
+ // when in continuous mode
|
|
|
+ if (q[this->numVars()-1] <= t_now)
|
|
|
+ return;
|
|
|
+ // Try to complete the integration step
|
|
|
+ fmi2Status status;
|
|
|
+ fmi2Boolean enterEventMode;
|
|
|
+ fmi2Boolean terminateSimulation;
|
|
|
+ t_now = q[this->numVars()-1];
|
|
|
+ status = _fmi2SetTime(c,t_now);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ status = _fmi2SetContinuousStates(c,q,this->numVars()-1);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ status = _fmi2CompletedIntegratorStep(c,fmi2True,&enterEventMode,&terminateSimulation);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ // Force an event if one is indicated
|
|
|
+ if (enterEventMode == fmi2True)
|
|
|
+ next_time_event = t_now;
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+void FMI<X>::postTrialStep(double* q)
|
|
|
+{
|
|
|
+ assert(cont_time_mode);
|
|
|
+ // Restore values changed by der_func and state_event_func
|
|
|
+ fmi2Status status;
|
|
|
+ status = _fmi2SetTime(c,q[this->numVars()-1]);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ status = _fmi2SetContinuousStates(c,q,this->numVars()-1);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+void FMI<X>::internal_event(double* q, const bool* state_event)
|
|
|
+{
|
|
|
+ fmi2Status status;
|
|
|
+ // postStep will have updated the continuous variables, so
|
|
|
+ // we just process discrete events here.
|
|
|
+ if (cont_time_mode)
|
|
|
+ {
|
|
|
+ status = _fmi2EnterEventMode(c);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ cont_time_mode = false;
|
|
|
+ }
|
|
|
+ // Process events
|
|
|
+ iterate_events();
|
|
|
+ // Update the state variable array
|
|
|
+ status = _fmi2GetContinuousStates(c,q,this->numVars()-1);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+void FMI<X>::external_event(double* q, double e, const Bag<X>& xb)
|
|
|
+{
|
|
|
+ fmi2Status status;
|
|
|
+ // Go to event mode if we have not yet done so
|
|
|
+ if (cont_time_mode)
|
|
|
+ {
|
|
|
+ status = _fmi2EnterEventMode(c);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ cont_time_mode = false;
|
|
|
+ }
|
|
|
+ // process any events that need processing
|
|
|
+ iterate_events();
|
|
|
+ status = _fmi2GetContinuousStates(c,q,this->numVars()-1);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+void FMI<X>::confluent_event(double *q, const bool* state_event, const Bag<X>& xb)
|
|
|
+{
|
|
|
+ fmi2Status status;
|
|
|
+ // postStep will have updated the continuous variables, so
|
|
|
+ // we just process discrete events here.
|
|
|
+ if (cont_time_mode)
|
|
|
+ {
|
|
|
+ status = _fmi2EnterEventMode(c);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ cont_time_mode = false;
|
|
|
+ }
|
|
|
+ iterate_events();
|
|
|
+ status = _fmi2GetContinuousStates(c,q,this->numVars()-1);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+void FMI<X>::output_func(const double *q, const bool* state_event, Bag<X>& yb)
|
|
|
+{
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+void FMI<X>::gc_output(Bag<X>& gb)
|
|
|
+{
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+FMI<X>::~FMI()
|
|
|
+{
|
|
|
+ _fmi2FreeInstance(c);
|
|
|
+ delete callbackFuncs;
|
|
|
+ CLOSE_LIB(so_hndl);
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+double FMI<X>::get_real(int k)
|
|
|
+{
|
|
|
+ const fmi2ValueReference ref = k;
|
|
|
+ fmi2Real val;
|
|
|
+ fmi2Status status = _fmi2GetReal(c,&ref,1,&val);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ return val;
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+void FMI<X>::set_real(int k, double val)
|
|
|
+{
|
|
|
+ const fmi2ValueReference ref = k;
|
|
|
+ fmi2Real fmi_val = val;
|
|
|
+ fmi2Status status = _fmi2SetReal(c,&ref,1,&fmi_val);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+int FMI<X>::get_int(int k)
|
|
|
+{
|
|
|
+ const fmi2ValueReference ref = k;
|
|
|
+ fmi2Integer val;
|
|
|
+ fmi2Status status = _fmi2GetInteger(c,&ref,1,&val);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ return val;
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+void FMI<X>::set_int(int k, int val)
|
|
|
+{
|
|
|
+ const fmi2ValueReference ref = k;
|
|
|
+ fmi2Integer fmi_val = val;
|
|
|
+ fmi2Status status = _fmi2SetInteger(c,&ref,1,&fmi_val);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+bool FMI<X>::get_bool(int k)
|
|
|
+{
|
|
|
+ const fmi2ValueReference ref = k;
|
|
|
+ fmi2Boolean val;
|
|
|
+ fmi2Status status = _fmi2GetBoolean(c,&ref,1,&val);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ return (val == fmi2True);
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+void FMI<X>::set_bool(int k, bool val)
|
|
|
+{
|
|
|
+ const fmi2ValueReference ref = k;
|
|
|
+ fmi2Boolean fmi_val = fmi2False;
|
|
|
+ if (val) fmi_val = fmi2True;
|
|
|
+ fmi2Status status = _fmi2SetBoolean(c,&ref,1,&fmi_val);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+std::string FMI<X>::get_string(int k)
|
|
|
+{
|
|
|
+ const fmi2ValueReference ref = k;
|
|
|
+ fmi2String val;
|
|
|
+ fmi2Status status = _fmi2GetString(c,&ref,1,&val);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+ return val;
|
|
|
+}
|
|
|
+
|
|
|
+template <typename X>
|
|
|
+void FMI<X>::set_string(int k, std::string& val)
|
|
|
+{
|
|
|
+ const fmi2ValueReference ref = k;
|
|
|
+ fmi2String fmi_val = fmi2False;
|
|
|
+ fmi2Status status = _fmi2SetString(c,&ref,1,&fmi_val);
|
|
|
+ assert(status == fmi2OK);
|
|
|
+}
|
|
|
+
|
|
|
+} // end of namespace
|
|
|
+
|
|
|
+#endif
|