00001
00031 #ifndef _adevs_fmi_h_
00032 #define _adevs_fmi_h_
00033 #include <algorithm>
00034 #include <cmath>
00035 #include <iostream>
00036 #include <dlfcn.h>
00037 #include <cstdlib>
00038 #include "adevs_hybrid.h"
00039 #include "fmi2Functions.h"
00040 #include "fmi2FunctionTypes.h"
00041 #include "fmi2TypesPlatform.h"
00042
00043 namespace adevs
00044 {
00045
00057 template <typename X> class FMI:
00058 public ode_system<X>
00059 {
00060 public:
00067 FMI(const char* modelname,
00068 const char* guid,
00069 int num_state_variables,
00070 int num_event_indicators,
00071 const char* shared_lib_name,
00072 const double tolerance = 1E-8,
00073 int num_extra_event_indicators = 0);
00075 virtual void init(double* q);
00077 virtual void der_func(const double* q, double* dq);
00079 virtual void state_event_func(const double* q, double* z);
00081 virtual double time_event_func(const double* q);
00087 virtual void postStep(double* q);
00094 virtual void internal_event(double* q,
00095 const bool* state_event);
00100 virtual void external_event(double* q, double e,
00101 const Bag<X>& xb);
00106 virtual void confluent_event(double *q, const bool* state_event,
00107 const Bag<X>& xb);
00112 virtual void output_func(const double *q, const bool* state_event,
00113 Bag<X>& yb);
00118 virtual void gc_output(Bag<X>& gb);
00120 virtual ~FMI();
00121
00122 double get_time() const { return t_now; }
00123
00124 double get_real(int k);
00125
00126 void set_real(int k, double val);
00127
00128 int get_int(int k);
00129
00130 void set_int(int k, int val);
00131
00132 bool get_bool(int k);
00133
00134 void set_bool(int k, bool val);
00135
00136 private:
00137
00138 fmi2Component c;
00139
00140 fmi2Component (*_fmi2Instantiate)(fmi2String, fmi2Type,
00141 fmi2String, fmi2String, const fmi2CallbackFunctions*,
00142 fmi2Boolean, fmi2Boolean);
00143 void (*_fmi2FreeInstance)(fmi2Component);
00144 fmi2Status (*_fmi2SetupExperiment)(fmi2Component, fmi2Boolean,
00145 fmi2Real, fmi2Real, fmi2Boolean, fmi2Real);
00146 fmi2Status (*_fmi2EnterInitializationMode)(fmi2Component);
00147 fmi2Status (*_fmi2ExitInitializationMode)(fmi2Component);
00148 fmi2Status (*_fmi2GetReal)(fmi2Component, const fmi2ValueReference*, size_t, fmi2Real*);
00149 fmi2Status (*_fmi2GetInteger)(fmi2Component, const fmi2ValueReference*, size_t, fmi2Integer*);
00150 fmi2Status (*_fmi2GetBoolean)(fmi2Component, const fmi2ValueReference*, size_t, fmi2Boolean*);
00151 fmi2Status (*_fmi2GetString)(fmi2Component, const fmi2ValueReference*, size_t, fmi2String*);
00152 fmi2Status (*_fmi2SetReal)(fmi2Component, const fmi2ValueReference*, size_t, const fmi2Real*);
00153 fmi2Status (*_fmi2SetInteger)(fmi2Component, const fmi2ValueReference*, size_t, const fmi2Integer*);
00154 fmi2Status (*_fmi2SetBoolean)(fmi2Component, const fmi2ValueReference*, size_t, const fmi2Boolean*);
00155 fmi2Status (*_fmi2SetString)(fmi2Component, const fmi2ValueReference*, size_t, const fmi2String*);
00156 fmi2Status (*_fmi2EnterEventMode)(fmi2Component);
00157 fmi2Status (*_fmi2NewDiscreteStates)(fmi2Component,fmi2EventInfo*);
00158 fmi2Status (*_fmi2EnterContinuousTimeMode)(fmi2Component);
00159 fmi2Status (*_fmi2CompletedIntegratorStep)(fmi2Component, fmi2Boolean, fmi2Boolean*, fmi2Boolean*);
00160 fmi2Status (*_fmi2SetTime)(fmi2Component, fmi2Real);
00161 fmi2Status (*_fmi2SetContinuousStates)(fmi2Component, const fmi2Real*, size_t);
00162 fmi2Status (*_fmi2GetDerivatives)(fmi2Component, fmi2Real*, size_t);
00163 fmi2Status (*_fmi2GetEventIndicators)(fmi2Component, fmi2Real*, size_t);
00164 fmi2Status (*_fmi2GetContinuousStates)(fmi2Component, fmi2Real*, size_t);
00165
00166 double next_time_event;
00167
00168 double t_now;
00169
00170 void* so_hndl;
00171
00172 bool cont_time_mode;
00173
00174 int num_extra_event_indicators;
00175
00176 static void fmilogger(
00177 fmi2ComponentEnvironment componentEnvironment,
00178 fmi2String instanceName,
00179 fmi2Status status,
00180 fmi2String category,
00181 fmi2String message, ...)
00182 {
00183 std::cerr << message << std::endl;
00184 }
00185
00186 fmi2CallbackFunctions* callbackFuncs;
00187
00188 void iterate_events();
00189 };
00190
00191 template <typename X>
00192 FMI<X>::FMI(const char* modelname,
00193 const char* guid,
00194 int num_state_variables,
00195 int num_event_indicators,
00196 const char* so_file_name,
00197 const double tolerance,
00198 int num_extra_event_indicators):
00199
00200 ode_system<X>(num_state_variables+1,num_event_indicators+num_extra_event_indicators),
00201 next_time_event(adevs_inf<double>()),
00202 t_now(0.0),
00203 so_hndl(NULL),
00204 cont_time_mode(false),
00205 num_extra_event_indicators(num_extra_event_indicators)
00206 {
00207 fmi2CallbackFunctions tmp = {adevs::FMI<X>::fmilogger,calloc,free,NULL,NULL};
00208 callbackFuncs = new fmi2CallbackFunctions(tmp);
00209 so_hndl = dlopen(so_file_name, RTLD_LAZY);
00210 if (!so_hndl)
00211 {
00212 throw adevs::exception("Could not load so file",this);
00213 }
00214
00215 _fmi2Instantiate = (fmi2Component (*)(fmi2String, fmi2Type,
00216 fmi2String, fmi2String, const fmi2CallbackFunctions*,
00217 fmi2Boolean, fmi2Boolean))dlsym(so_hndl,"fmi2Instantiate");
00218 assert(_fmi2Instantiate != NULL);
00219 _fmi2FreeInstance = (void (*)(fmi2Component))dlsym(so_hndl,"fmi2FreeInstance");
00220 assert(_fmi2FreeInstance != NULL);
00221 _fmi2SetupExperiment = (fmi2Status (*)(fmi2Component, fmi2Boolean,
00222 fmi2Real, fmi2Real, fmi2Boolean, fmi2Real))dlsym(so_hndl,"fmi2SetupExperiment");
00223 assert(_fmi2SetupExperiment != NULL);
00224 _fmi2EnterInitializationMode = (fmi2Status (*)(fmi2Component))dlsym(so_hndl,"fmi2EnterInitializationMode");
00225 assert(_fmi2EnterInitializationMode != NULL);
00226 _fmi2ExitInitializationMode = (fmi2Status (*)(fmi2Component))dlsym(so_hndl,"fmi2ExitInitializationMode");
00227 assert(_fmi2ExitInitializationMode != NULL);
00228 _fmi2GetReal = (fmi2Status (*)(fmi2Component, const fmi2ValueReference*, size_t, fmi2Real*))
00229 dlsym(so_hndl,"fmi2GetReal");
00230 assert(_fmi2GetReal != NULL);
00231 _fmi2GetInteger = (fmi2Status (*)(fmi2Component, const fmi2ValueReference*, size_t, fmi2Integer*))
00232 dlsym(so_hndl,"fmi2GetInteger");
00233 assert(_fmi2GetInteger != NULL);
00234 _fmi2GetBoolean = (fmi2Status (*)(fmi2Component, const fmi2ValueReference*, size_t, fmi2Boolean*))
00235 dlsym(so_hndl,"fmi2GetBoolean");
00236 assert(_fmi2GetBoolean != NULL);
00237 _fmi2GetString = (fmi2Status (*)(fmi2Component, const fmi2ValueReference*, size_t, fmi2String*))
00238 dlsym(so_hndl,"fmi2GetString");
00239 assert(_fmi2GetString != NULL);
00240 _fmi2SetReal = (fmi2Status (*)(fmi2Component, const fmi2ValueReference*, size_t, const fmi2Real*))
00241 dlsym(so_hndl,"fmi2SetReal");
00242 assert(_fmi2SetReal != NULL);
00243 _fmi2SetInteger = (fmi2Status (*)(fmi2Component, const fmi2ValueReference*, size_t, const fmi2Integer*))
00244 dlsym(so_hndl,"fmi2SetInteger");
00245 assert(_fmi2SetInteger != NULL);
00246 _fmi2SetBoolean = (fmi2Status (*)(fmi2Component, const fmi2ValueReference*, size_t, const fmi2Boolean*))
00247 dlsym(so_hndl,"fmi2SetBoolean");
00248 assert(_fmi2SetBoolean != NULL);
00249 _fmi2SetString = (fmi2Status (*)(fmi2Component, const fmi2ValueReference*, size_t, const fmi2String*))
00250 dlsym(so_hndl,"fmi2SetString");
00251 assert(_fmi2SetString != NULL);
00252 _fmi2EnterEventMode = (fmi2Status (*)(fmi2Component))dlsym(so_hndl,"fmi2EnterEventMode");
00253 assert(_fmi2EnterEventMode != NULL);
00254 _fmi2NewDiscreteStates = (fmi2Status (*)(fmi2Component,fmi2EventInfo*))dlsym(so_hndl,"fmi2NewDiscreteStates");
00255 assert(_fmi2NewDiscreteStates != NULL);
00256 _fmi2EnterContinuousTimeMode = (fmi2Status (*)(fmi2Component))dlsym(so_hndl,"fmi2EnterContinuousTimeMode");
00257 assert(_fmi2EnterContinuousTimeMode != NULL);
00258 _fmi2CompletedIntegratorStep = (fmi2Status (*)(fmi2Component, fmi2Boolean, fmi2Boolean*, fmi2Boolean*))
00259 dlsym(so_hndl,"fmi2CompletedIntegratorStep");
00260 assert(_fmi2CompletedIntegratorStep != NULL);
00261 _fmi2SetTime = (fmi2Status (*)(fmi2Component, fmi2Real))dlsym(so_hndl,"fmi2SetTime");
00262 assert(_fmi2SetTime != NULL);
00263 _fmi2SetContinuousStates = (fmi2Status (*)(fmi2Component, const fmi2Real*, size_t))
00264 dlsym(so_hndl,"fmi2SetContinuousStates");
00265 assert(_fmi2SetContinuousStates != NULL);
00266 _fmi2GetDerivatives = (fmi2Status (*)(fmi2Component, fmi2Real*, size_t))dlsym(so_hndl,"fmi2GetDerivatives");
00267 assert(_fmi2GetDerivatives != NULL);
00268 _fmi2GetEventIndicators = (fmi2Status (*)(fmi2Component, fmi2Real*, size_t))dlsym(so_hndl,"fmi2GetEventIndicators");
00269 assert(_fmi2GetEventIndicators != NULL);
00270 _fmi2GetContinuousStates = (fmi2Status (*)(fmi2Component, fmi2Real*, size_t))dlsym(so_hndl,"fmi2GetContinuousStates");
00271 assert(_fmi2GetContinuousStates != NULL);
00272
00273 c = _fmi2Instantiate(modelname,fmi2ModelExchange,guid,"",callbackFuncs,fmi2False,fmi2False);
00274 assert(c != NULL);
00275 _fmi2SetupExperiment(c,fmi2True,tolerance,-1.0,fmi2False,-1.0);
00276 }
00277
00278 template <typename X>
00279 void FMI<X>::iterate_events()
00280 {
00281 fmi2Status status;
00282
00283 fmi2EventInfo eventInfo;
00284 do
00285 {
00286 status = _fmi2NewDiscreteStates(c,&eventInfo);
00287 assert(status == fmi2OK);
00288 }
00289 while (eventInfo.newDiscreteStatesNeeded == fmi2True);
00290 if (eventInfo.nextEventTimeDefined == fmi2True)
00291 next_time_event = eventInfo.nextEventTime;
00292 assert(status == fmi2OK);
00293 }
00294
00295 template <typename X>
00296 void FMI<X>::init(double* q)
00297 {
00298 fmi2Status status;
00299
00300 status = _fmi2SetTime(c,t_now);
00301 assert(status == fmi2OK);
00302
00303 status = _fmi2EnterInitializationMode(c);
00304 assert(status == fmi2OK);
00305
00306 status = _fmi2ExitInitializationMode(c);
00307 assert(status == fmi2OK);
00308
00309 iterate_events();
00310
00311 status = _fmi2EnterContinuousTimeMode(c);
00312 assert(status == fmi2OK);
00313 status = _fmi2GetContinuousStates(c,q,this->numVars()-1);
00314 assert(status == fmi2OK);
00315 q[this->numVars()-1] = t_now;
00316 cont_time_mode = true;
00317 }
00318
00319 template <typename X>
00320 void FMI<X>::der_func(const double* q, double* dq)
00321 {
00322 fmi2Status status;
00323 if (!cont_time_mode)
00324 {
00325 status = _fmi2EnterContinuousTimeMode(c);
00326 assert(status == fmi2OK);
00327 cont_time_mode = true;
00328 }
00329 status =_fmi2SetTime(c,q[this->numVars()-1]);
00330 assert(status == fmi2OK);
00331 status = _fmi2SetContinuousStates(c,q,this->numVars()-1);
00332 assert(status == fmi2OK);
00333 status = _fmi2GetDerivatives(c,dq,this->numVars()-1);
00334 assert(status == fmi2OK);
00335 dq[this->numVars()-1] = 1.0;
00336 }
00337
00338 template <typename X>
00339 void FMI<X>::state_event_func(const double* q, double* z)
00340 {
00341 fmi2Status status;
00342 if (!cont_time_mode)
00343 {
00344 status = _fmi2EnterContinuousTimeMode(c);
00345 assert(status == fmi2OK);
00346 cont_time_mode = true;
00347 }
00348 status = _fmi2SetTime(c,q[this->numVars()-1]);
00349 assert(status == fmi2OK);
00350 status = _fmi2SetContinuousStates(c,q,this->numVars()-1);
00351 assert(status == fmi2OK);
00352 status = _fmi2GetEventIndicators(c,z,this->numEvents()-num_extra_event_indicators);
00353 assert(status == fmi2OK);
00354 }
00355
00356 template <typename X>
00357 double FMI<X>::time_event_func(const double* q)
00358 {
00359 return next_time_event-q[this->numVars()-1];
00360 }
00361
00362 template <typename X>
00363 void FMI<X>::postStep(double* q)
00364 {
00365 assert(cont_time_mode);
00366
00367
00368 if (q[this->numVars()-1] <= t_now)
00369 return;
00370
00371 fmi2Status status;
00372 fmi2Boolean enterEventMode;
00373 fmi2Boolean terminateSimulation;
00374 t_now = q[this->numVars()-1];
00375 status = _fmi2SetTime(c,t_now);
00376 assert(status == fmi2OK);
00377 status = _fmi2SetContinuousStates(c,q,this->numVars()-1);
00378 assert(status == fmi2OK);
00379 status = _fmi2CompletedIntegratorStep(c,fmi2True,&enterEventMode,&terminateSimulation);
00380 assert(status == fmi2OK);
00381
00382 if (enterEventMode == fmi2True)
00383 next_time_event = t_now;
00384 }
00385
00386 template <typename X>
00387 void FMI<X>::internal_event(double* q, const bool* state_event)
00388 {
00389 fmi2Status status;
00390
00391
00392 if (cont_time_mode)
00393 {
00394 status = _fmi2EnterEventMode(c);
00395 assert(status == fmi2OK);
00396 cont_time_mode = false;
00397 }
00398
00399 iterate_events();
00400
00401 status = _fmi2GetContinuousStates(c,q,this->numVars()-1);
00402 assert(status == fmi2OK);
00403 }
00404
00405 template <typename X>
00406 void FMI<X>::external_event(double* q, double e, const Bag<X>& xb)
00407 {
00408 fmi2Status status;
00409
00410 if (cont_time_mode)
00411 {
00412 status = _fmi2EnterEventMode(c);
00413 assert(status == fmi2OK);
00414 cont_time_mode = false;
00415 }
00416
00417 iterate_events();
00418 status = _fmi2GetContinuousStates(c,q,this->numVars()-1);
00419 assert(status == fmi2OK);
00420 }
00421
00422 template <typename X>
00423 void FMI<X>::confluent_event(double *q, const bool* state_event, const Bag<X>& xb)
00424 {
00425 fmi2Status status;
00426
00427
00428 if (cont_time_mode)
00429 {
00430 status = _fmi2EnterEventMode(c);
00431 assert(status == fmi2OK);
00432 cont_time_mode = false;
00433 }
00434 iterate_events();
00435 status = _fmi2GetContinuousStates(c,q,this->numVars()-1);
00436 assert(status == fmi2OK);
00437 }
00438
00439 template <typename X>
00440 void FMI<X>::output_func(const double *q, const bool* state_event, Bag<X>& yb)
00441 {
00442 }
00443
00444 template <typename X>
00445 void FMI<X>::gc_output(Bag<X>& gb)
00446 {
00447 }
00448
00449 template <typename X>
00450 FMI<X>::~FMI()
00451 {
00452 _fmi2FreeInstance(c);
00453 delete callbackFuncs;
00454 dlclose(so_hndl);
00455 }
00456
00457 template <typename X>
00458 double FMI<X>::get_real(int k)
00459 {
00460 const fmi2ValueReference ref = k;
00461 fmi2Real val;
00462 fmi2Status status = _fmi2GetReal(c,&ref,1,&val);
00463 assert(status == fmi2OK);
00464 return val;
00465 }
00466
00467 template <typename X>
00468 void FMI<X>::set_real(int k, double val)
00469 {
00470 const fmi2ValueReference ref = k;
00471 fmi2Real fmi_val = val;
00472 fmi2Status status = _fmi2SetReal(c,&ref,1,&fmi_val);
00473 assert(status == fmi2OK);
00474 }
00475
00476 template <typename X>
00477 int FMI<X>::get_int(int k)
00478 {
00479 const fmi2ValueReference ref = k;
00480 fmi2Integer val;
00481 fmi2Status status = _fmi2GetInteger(c,&ref,1,&val);
00482 assert(status == fmi2OK);
00483 return val;
00484 }
00485
00486 template <typename X>
00487 void FMI<X>::set_int(int k, int val)
00488 {
00489 const fmi2ValueReference ref = k;
00490 fmi2Integer fmi_val = val;
00491 fmi2Status status = _fmi2SetInteger(c,&ref,1,&fmi_val);
00492 assert(status == fmi2OK);
00493 }
00494
00495 template <typename X>
00496 bool FMI<X>::get_bool(int k)
00497 {
00498 const fmi2ValueReference ref = k;
00499 fmi2Boolean val;
00500 fmi2Status status = _fmi2GetBoolean(c,&ref,1,&val);
00501 assert(status == fmi2OK);
00502 return (val == fmi2True);
00503 }
00504
00505 template <typename X>
00506 void FMI<X>::set_bool(int k, bool val)
00507 {
00508 const fmi2ValueReference ref = k;
00509 fmi2Boolean fmi_val = fmi2False;
00510 if (val) fmi_val = fmi2True;
00511 fmi2Status status = _fmi2SetBoolean(c,&ref,1,&fmi_val);
00512 assert(status == fmi2OK);
00513 }
00514
00515 }
00516
00517 #endif