FMI_Environment.c 16 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537
  1. /* ---------------------------------------------------------------------------*
  2. * Sample implementation of an FMU - a power window controller
  3. * The code is generated from a Yakindu SC
  4. * ---------------------------------------------------------------------------*/
  5. /*
  6. Template for a sc FMU
  7. */
  8. #define MODEL_IDENTIFIER SEL
  9. #define MODEL_GUID "{41f87101-edf2-4eef-90f3-42db56d4565f}"
  10. #define FMI2_FUNCTION_PREFIX PW_ENV
  11. #include <stdio.h>
  12. #include "string.h"
  13. #include "fmi2Functions.h"
  14. #include <float.h>
  15. #include "FMI_Environment.h"
  16. #include <math.h>
  17. #define NUMBER_OF_REALS 0
  18. #define NUMBER_OF_STRINGS 0
  19. #define NUMBER_OF_BOOLEANS 9
  20. #define NUMBER_OF_INTEGERS 0
  21. #define _out_driver_up 0
  22. #define _out_driver_up_stop 1
  23. #define _out_driver_down 2
  24. #define _out_driver_down_stop 3
  25. #define _out_passenger_up 4
  26. #define _out_passenger_up_stop 5
  27. #define _out_passenger_down 6
  28. #define _out_passenger_down_stop 7
  29. #define _out_object 8
  30. /*
  31. * Helper function for relative error
  32. */
  33. double relativeError(double a, double b){
  34. return fabs((a - b) / a);
  35. }
  36. /*
  37. * Helper function for absolute error
  38. */
  39. double absoluteError(double a, double b){
  40. return fabs(a - b);
  41. }
  42. /*
  43. * is_close function for double comparison
  44. */
  45. int is_close(double a, double b, double REL_TOL, double ABS_TOL){
  46. return ((absoluteError(a,b)<ABS_TOL) && (relativeError(a,b)<REL_TOL));
  47. }
  48. fmi2Status fmi2SetDebugLogging(fmi2Component fc, fmi2Boolean loggingOn, size_t nCategories, const fmi2String categories[])
  49. {
  50. return fmi2OK;
  51. }
  52. fmi2Status fmi2SetString(fmi2Component fc, const fmi2ValueReference vr[], size_t nvr, const fmi2String value[])
  53. {
  54. return fmi2Error;
  55. }
  56. fmi2Status fmi2GetString(fmi2Component fc, const fmi2ValueReference vr[], size_t nvr, fmi2String value[])
  57. {
  58. return fmi2Error;
  59. }
  60. fmi2Status fmi2SetReal(fmi2Component fc, const fmi2ValueReference vr[], size_t nvr, const fmi2Real value[])
  61. {
  62. FMUInstance* comp = (FMUInstance *)fc;
  63. int i;
  64. for (i = 0; i < nvr; i++)
  65. {
  66. if(vr[i]>=5){
  67. printf("Value reference: %d, cannot be set, it is a store element\n", vr[i]);
  68. }else{
  69. printf("Value reference: %d\n", vr[i]);
  70. comp->r[vr[i]] = value[i];
  71. }
  72. //setExposedReal(vr[i], value[i]);
  73. //comp->r[vr[i]] = value[i];
  74. }
  75. return fmi2OK;
  76. }
  77. fmi2Status fmi2GetReal(fmi2Component fc, const fmi2ValueReference vr[], size_t nvr, fmi2Real value[])
  78. {
  79. FMUInstance* comp = (FMUInstance *)fc;
  80. int i;
  81. for (i = 0; i < nvr; i++)
  82. {
  83. value[i] = comp->r[(vr[i])];
  84. }
  85. return fmi2OK;
  86. }
  87. fmi2Status fmi2SetBoolean(fmi2Component fc, const fmi2ValueReference vr[], size_t nvr, const fmi2Boolean value[])
  88. {
  89. FMUInstance* comp = (FMUInstance *)fc;
  90. int i;
  91. for (i = 0; i < nvr; i++)
  92. {
  93. comp->b[vr[i]] = value[i];
  94. }
  95. return fmi2OK;
  96. }
  97. fmi2Status fmi2GetBoolean(fmi2Component fc, const fmi2ValueReference vr[], size_t nvr, fmi2Boolean value[])
  98. {
  99. FMUInstance* comp = (FMUInstance *)fc;
  100. int i;
  101. for (i = 0; i < nvr; i++)
  102. {
  103. value[i] = comp->b[vr[i]];
  104. }
  105. return fmi2OK;
  106. }
  107. fmi2Component fmi2Instantiate(fmi2String instanceName, fmi2Type fmuType, fmi2String fmuGUID, fmi2String fmuLocation, const fmi2CallbackFunctions* functions, fmi2Boolean visible, fmi2Boolean loggingOn)
  108. {
  109. //Declare data structure for fmu instance
  110. FMUInstance* fi;
  111. printf("%s in fmiInstantiate\n",instanceName);
  112. //Perform checks on passed callback functions
  113. if (loggingOn) {
  114. if (!functions->logger);
  115. //return NULL;
  116. }
  117. //Check for instanceName
  118. if (!instanceName || strlen(instanceName)==0) {
  119. // print (and/or log) instanceName is missing
  120. //return NULL;
  121. }
  122. //Check passed GUID to defined model GUID
  123. if (strcmp(fmuGUID, MODEL_GUID))
  124. {
  125. // print (and/or log) GUID doesn't match
  126. //return NULL;
  127. }
  128. //Allocate fmu instance Memory
  129. // TODO check if "canNotUseMemoryManagementFunctions == true/false". If false memory allocation not possible
  130. fi = (FMUInstance *)functions->allocateMemory(1, sizeof(FMUInstance));
  131. fi->functions = functions;
  132. if (fi) {
  133. // Think about what to do with variable values. Using these structs and pointers slows down simulation computations. Maybe only necessary for input, output and tunable parameters??
  134. fi->r = functions->allocateMemory(NUMBER_OF_REALS, sizeof(fmi2Real));
  135. fi->i = functions->allocateMemory(NUMBER_OF_INTEGERS, sizeof(fmi2Integer));
  136. fi->b = functions->allocateMemory(NUMBER_OF_BOOLEANS, sizeof(fmi2Boolean));
  137. fi->s = functions->allocateMemory(NUMBER_OF_STRINGS, sizeof(fmi2String));
  138. } // variables in predefined arrays (performance issue) --> makes multiple instances of fmu impossible
  139. fi->instanceName = functions->allocateMemory(1 + strlen(instanceName), sizeof(char));
  140. fi->GUID = functions->allocateMemory(1 + strlen(fmuGUID), sizeof(char));
  141. strcpy((char*)fi->instanceName, instanceName);
  142. strcpy((char*)fi->GUID, fmuGUID);
  143. fi->functions = functions;
  144. fi->loggingOn = loggingOn;
  145. fi->isVisible = visible;
  146. fi->state = fmuInstantiated;
  147. return fi;
  148. }
  149. fmi2Status fmi2SetupExperiment(fmi2Component fc, fmi2Boolean toleranceDefined, fmi2Real tolerance,
  150. fmi2Real startTime, fmi2Boolean stopTimeDefined, fmi2Real stopTime) {
  151. FMUInstance* fi = (FMUInstance*) fc;
  152. printf("%s in fmiSetupExperiment\n",fi->instanceName);
  153. if (fi->state != fmuInstantiated)
  154. {
  155. printf("fmu: %s was not instatiated before calling fmiSetupExperiment\n", fi->instanceName);
  156. return fmi2Error;
  157. }
  158. fi->currentTime = startTime;
  159. fi->stopTimeDefined = stopTimeDefined;
  160. fi->toleranceDefined = toleranceDefined;
  161. if (stopTimeDefined)
  162. {
  163. fi->stopTime = stopTime;
  164. }
  165. if (toleranceDefined)
  166. {
  167. fi->tolerance = tolerance;
  168. }
  169. //TODO
  170. //fi->stepSize = getStepSize();
  171. fi->state = fmuExperimentSettedUp;
  172. return fmi2OK;
  173. }
  174. fmi2Status fmi2EnterInitializationMode(fmi2Component fc)
  175. {
  176. FMUInstance* fi = (FMUInstance*) fc;
  177. printf("%s in fmiEnterInitializationMode\n",fi->instanceName);
  178. if (fi->state != fmuExperimentSettedUp)
  179. {
  180. printf("fmu: %s experiment was not set-up before calling fmiEnterInitializationMode\n", fi->instanceName);
  181. return fmi2Error;
  182. }
  183. fi->state = fmuInitMode;
  184. return fmi2OK;
  185. }
  186. fmi2Status fmi2ExitInitializationMode(fmi2Component fc)
  187. {
  188. FMUInstance* fi = (FMUInstance*) fc;
  189. fmi2Status initStatus = fmi2OK;
  190. printf("%s in fmiExitInitializationMode\n",fi->instanceName);
  191. if (fi->state != fmuInitMode)
  192. {
  193. printf("fmu: %s did not enter Initialization Mode before calling fmiExitInitializationMode\n", fi->instanceName);
  194. return fmi2Error;
  195. }
  196. // TODO
  197. //initStatus = calculateInitialUnknownValues();
  198. //initialize();
  199. fi->state = fmuInitialized;
  200. return initStatus;
  201. }
  202. fmi2Status fmi2DoStep(fmi2Component fc , fmi2Real currentCommPoint, fmi2Real commStepSize, fmi2Boolean noPrevFMUState)
  203. {
  204. FMUInstance* fi = (FMUInstance *)fc;
  205. fmi2Status simStatus = fmi2OK;
  206. printf("%s in fmiDoStep()\n",fi->instanceName);
  207. if(currentCommPoint < 0.5){
  208. fi->b[_out_driver_up] = 0;
  209. fi->b[_out_driver_up_stop] = 0;
  210. fi->b[_out_driver_down] = 0;
  211. fi->b[_out_driver_down_stop] = 0;
  212. fi->b[_out_passenger_up] = 0;
  213. fi->b[_out_passenger_up_stop] = 0;
  214. fi->b[_out_passenger_down] = 0;
  215. fi->b[_out_passenger_down_stop] = 0;
  216. fi->b[_out_object] = 0;
  217. }else if(currentCommPoint < 1){
  218. fi->b[_out_driver_up] = 1;
  219. fi->b[_out_driver_up_stop] = 0;
  220. fi->b[_out_driver_down] = 0;
  221. fi->b[_out_driver_down_stop] = 0;
  222. fi->b[_out_passenger_up] = 0;
  223. fi->b[_out_passenger_up_stop] = 0;
  224. fi->b[_out_passenger_down] = 0;
  225. fi->b[_out_passenger_down_stop] = 0;
  226. fi->b[_out_object] = 0;
  227. }else if(currentCommPoint < 1.5){
  228. fi->b[_out_driver_up] = 1;
  229. fi->b[_out_driver_up_stop] = 0;
  230. fi->b[_out_driver_down] = 0;
  231. fi->b[_out_driver_down_stop] = 0;
  232. fi->b[_out_passenger_up] = 0;
  233. fi->b[_out_passenger_up_stop] = 0;
  234. fi->b[_out_passenger_down] = 0;
  235. fi->b[_out_passenger_down_stop] = 0;
  236. fi->b[_out_object] = 1;
  237. }else if(currentCommPoint < 1.7){
  238. fi->b[_out_driver_up] = 0;
  239. fi->b[_out_driver_up_stop] = 0;
  240. fi->b[_out_driver_down] = 0;
  241. fi->b[_out_driver_down_stop] = 0;
  242. fi->b[_out_passenger_up] = 0;
  243. fi->b[_out_passenger_up_stop] = 0;
  244. fi->b[_out_passenger_down] = 0;
  245. fi->b[_out_passenger_down_stop] = 0;
  246. fi->b[_out_object] = 0;
  247. }else if(currentCommPoint < 2.7){
  248. fi->b[_out_driver_up] = 0;
  249. fi->b[_out_driver_up_stop] = 0;
  250. fi->b[_out_driver_down] = 0;
  251. fi->b[_out_driver_down_stop] = 0;
  252. fi->b[_out_passenger_up] = 0;
  253. fi->b[_out_passenger_up_stop] = 0;
  254. fi->b[_out_passenger_down] = 0;
  255. fi->b[_out_passenger_down_stop] = 0;
  256. fi->b[_out_object] = 0;
  257. }else if(currentCommPoint < 3.1){
  258. fi->b[_out_driver_up] = 1;
  259. fi->b[_out_driver_up_stop] = 0;
  260. fi->b[_out_driver_down] = 0;
  261. fi->b[_out_driver_down_stop] = 0;
  262. fi->b[_out_passenger_up] = 0;
  263. fi->b[_out_passenger_up_stop] = 0;
  264. fi->b[_out_passenger_down] = 1;
  265. fi->b[_out_passenger_down_stop] = 0;
  266. fi->b[_out_object] = 0;
  267. }else if(currentCommPoint < 3.5){
  268. fi->b[_out_driver_up] = 0;
  269. fi->b[_out_driver_up_stop] = 1;
  270. fi->b[_out_driver_down] = 0;
  271. fi->b[_out_driver_down_stop] = 0;
  272. fi->b[_out_passenger_up] = 0;
  273. fi->b[_out_passenger_up_stop] = 0;
  274. fi->b[_out_passenger_down] = 0;
  275. fi->b[_out_passenger_down_stop] = 1;
  276. fi->b[_out_object] = 0;
  277. }
  278. return simStatus;
  279. }
  280. fmi2Status fmi2Terminate(fmi2Component fc)
  281. {
  282. FMUInstance* fi = (FMUInstance *)fc;
  283. printf("%s in fmiTerminate\n",fi->instanceName);
  284. // do check if fi may be terminated
  285. fi->state = fmuTerminated;
  286. return fmi2OK;
  287. }
  288. void fmi2FreeInstance(fmi2Component fc)
  289. {
  290. FMUInstance* fi = (FMUInstance*) fc;
  291. printf("%s in fmiFreeInstance\n",fi->instanceName);
  292. if (fi) {
  293. fi->functions->freeMemory(fi->r);
  294. fi->functions->freeMemory(fi->i);
  295. fi->functions->freeMemory(fi->b);
  296. fi->functions->freeMemory(fi->s);// TODO has to be done with loop
  297. fi->functions->freeMemory((void*)fi->instanceName);
  298. fi->functions->freeMemory((void*)fi->GUID);
  299. fi->functions->freeMemory((void*)fi);
  300. }
  301. }
  302. //To be implemented
  303. const char* fmi2GetVersion() {
  304. printf("Function fmiGetVersion not supported\n");
  305. return NULL;
  306. }
  307. const char* fmi2GetTypesPlatform() {
  308. printf("Function fmiGetTypesPlatform not supported\n");
  309. return NULL;
  310. }
  311. fmi2Status fmi2Reset(fmi2Component fc)
  312. {
  313. printf("Function fmiReset not supported\n");
  314. return fmi2Error;
  315. }
  316. fmi2Status fmi2SetInteger(fmi2Component fc, const fmi2ValueReference vr[], size_t nvr, const fmi2Integer value[])
  317. {
  318. printf("Function fmiSetInteger not supported\n");
  319. return fmi2Error;
  320. }
  321. fmi2Status fmi2GetInteger(fmi2Component fc, const fmi2ValueReference vr[], size_t nvr, fmi2Integer value[])
  322. {
  323. printf("Function fmiGetInteger not supported\n");
  324. return fmi2Error;
  325. }
  326. /*******OWN IMPLEMENTATION OF Get/Set FMU state*******/
  327. fmi2Status fmi2GetFMUstate (fmi2Component c, fmi2FMUstate* FMUstate) {
  328. FMUInstance* orig = (FMUInstance*)c;
  329. FMUInstance* fi = (FMUInstance *)FMUstate;
  330. fi = orig->functions->allocateMemory(1, sizeof(FMUInstance));
  331. *FMUstate = fi;
  332. if (fi) {
  333. // Think about what to do with variable values. Using these structs and pointers slows down simulation computations. Maybe only necessary for input, output and tunable parameters??
  334. fi->r = orig->functions->allocateMemory(NUMBER_OF_REALS, sizeof(fmi2Real));
  335. fi->i = orig->functions->allocateMemory(NUMBER_OF_INTEGERS, sizeof(fmi2Integer));
  336. fi->b = orig->functions->allocateMemory(NUMBER_OF_BOOLEANS, sizeof(fmi2Boolean));
  337. fi->s = orig->functions->allocateMemory(NUMBER_OF_STRINGS, sizeof(fmi2String));
  338. } // variables in predefined arrays (performance issue) --> makes multiple instances of fmu impossible
  339. fi->instanceName = orig->functions->allocateMemory(1 + strlen(orig->instanceName), sizeof(char));
  340. fi->GUID = orig->functions->allocateMemory(1 + strlen(orig->GUID), sizeof(char));
  341. strcpy((char *)fi->instanceName, (char *)orig->instanceName);
  342. strcpy((char *)fi->GUID, (char *)orig->GUID);
  343. fi->functions = orig->functions;
  344. fi->loggingOn = orig->loggingOn;
  345. fi->isVisible = orig->isVisible;
  346. fi->state = orig->state;
  347. fi->stepSize = orig->stepSize;
  348. fi->startTime = orig->startTime;
  349. fi->stopTime = orig->stopTime;
  350. fi->currentTime = orig->currentTime;
  351. //copy r
  352. int i=0;
  353. for (i=0; i< NUMBER_OF_REALS;i++){
  354. printf("Setting real: %i %f\n", i, orig->r[i]);
  355. fi->r[i] = orig->r[i];
  356. printf("Setted real: %i %f\n", i, fi->r[i]);
  357. }
  358. //copy s
  359. for (i=0; i< NUMBER_OF_STRINGS;i++){
  360. fi->s[i] = orig->s[i];
  361. }
  362. //copy i
  363. for (i=0; i< NUMBER_OF_INTEGERS;i++){
  364. fi->i[i] = orig->i[i];
  365. }
  366. //copy b
  367. for (i=0; i< NUMBER_OF_BOOLEANS;i++){
  368. fi->b[i] = orig->b[i];
  369. }
  370. return fmi2OK;
  371. }
  372. fmi2Status fmi2SetFMUstate (fmi2Component c, fmi2FMUstate FMUstate) {
  373. FMUInstance* orig = (FMUInstance*)FMUstate;
  374. FMUInstance* fi = (FMUInstance*)c;
  375. //set time etc correct, name and GUID should still be ok ;-)
  376. printf("setting time values from %f to %f\n", fi->currentTime, orig->currentTime);
  377. fi->state = orig->state;
  378. fi->stepSize = orig->stepSize;
  379. fi->startTime = orig->startTime;
  380. fi->stopTime = orig->stopTime;
  381. fi->currentTime = orig->currentTime;
  382. printf("setting real values\n");
  383. //copy r
  384. int i=0;
  385. for (i=0; i< NUMBER_OF_REALS;i++){
  386. fi->r[i] = orig->r[i];
  387. }
  388. printf("setting string values\n");
  389. //copy s
  390. for (i=0; i< NUMBER_OF_STRINGS;i++){
  391. fi->s[i] = orig->s[i];
  392. }
  393. //copy i
  394. for (i=0; i< NUMBER_OF_INTEGERS;i++){
  395. fi->i[i] = orig->i[i];
  396. }
  397. //copy b
  398. for (i=0; i< NUMBER_OF_BOOLEANS;i++){
  399. fi->b[i] = orig->b[i];
  400. }
  401. return fmi2OK;
  402. }
  403. /****************************************************/
  404. fmi2Status fmi2FreeFMUstate(fmi2Component c, fmi2FMUstate* FMUstate) {
  405. printf("Function fmiFreeFMUstate not supported\n");
  406. return fmi2Error;
  407. }
  408. fmi2Status fmi2SerializedFMUstateSize(fmi2Component c, fmi2FMUstate FMUstate, size_t *size) {
  409. printf("Function fmiSerializedFMUstateSize not supported\n");
  410. return fmi2Error;
  411. }
  412. fmi2Status fmi2SerializeFMUstate (fmi2Component c, fmi2FMUstate FMUstate, fmi2Byte serializedState[], size_t size) {
  413. printf("Function fmiSerializeFMUstate not supported\n");
  414. return fmi2Error;
  415. }
  416. fmi2Status fmi2DeSerializeFMUstate (fmi2Component c, const fmi2Byte serializedState[], size_t size, fmi2FMUstate* FMUstate) {
  417. printf("Function fmiDeSerializeFMUstate not supported\n");
  418. return fmi2Error;
  419. }
  420. fmi2Status fmi2GetDirectionalDerivative(fmi2Component c, const fmi2ValueReference vUnknown_ref[], size_t nUnknown,
  421. const fmi2ValueReference vKnown_ref[] , size_t nKnown, const fmi2Real dvKnown[], fmi2Real dvUnknown[]) {
  422. printf("Function fmiGetDirectionalDerivative not supported\n");
  423. return fmi2Error;
  424. }
  425. fmi2Status fmi2SetRealInputDerivatives(fmi2Component c, const fmi2ValueReference vr[], size_t nvr,
  426. const fmi2Integer order[], const fmi2Real value[]) {
  427. printf("Function fmiGetDirectionalDerivative not supported\n");
  428. return fmi2Error;
  429. }
  430. fmi2Status fmi2GetRealOutputDerivatives(fmi2Component c, const fmi2ValueReference vr[], size_t nvr,
  431. const fmi2Integer order[], fmi2Real value[]) {
  432. printf("Function fmiGetDirectionalDerivative not supported\n");
  433. return fmi2Error;
  434. }
  435. fmi2Status fmi2CancelStep(fmi2Component c) {
  436. printf("Function fmiGetDirectionalDerivative not supported\n");
  437. return fmi2Error;
  438. }
  439. fmi2Status fmi2GetStatus(fmi2Component c, const fmi2StatusKind s, fmi2Status *value) {
  440. printf("Function fmiGetStatus not supported\n");
  441. return fmi2Error;
  442. }
  443. fmi2Status fmi2GetRealStatus(fmi2Component c, const fmi2StatusKind s, fmi2Real *value) {
  444. if(s == fmi2LastSuccessfulTime){
  445. FMUInstance* comp = (FMUInstance*) c;
  446. *value = comp->currentTime;
  447. return fmi2OK;
  448. }
  449. printf("Function fmiGetRealStatus not supported\n");
  450. return fmi2Error;
  451. }
  452. fmi2Status fmi2GetIntegerStatus(fmi2Component c, const fmi2StatusKind s, fmi2Integer *value) {
  453. printf("Function fmiGetIntegerStatus not supported\n");
  454. return fmi2Error;
  455. }
  456. fmi2Status fmi2GetBooleanStatus(fmi2Component c, const fmi2StatusKind s, fmi2Boolean *value) {
  457. printf("Function fmiGetBooleanStatus not supported\n");
  458. return fmi2Error;
  459. }
  460. fmi2Status fmi2GetStringStatus(fmi2Component c, const fmi2StatusKind s, fmi2String *value) {
  461. printf("Function fmiGetStringStatus not supported\n");
  462. return fmi2Error;
  463. }