FMI_GM.c 21 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718
  1. /* ---------------------------------------------------------------------------*
  2. * Sample implementation of an generic master FMU -
  3. * ---------------------------------------------------------------------------*/
  4. /*
  5. Template for a FMU
  6. */
  7. #define MODEL_IDENTIFIER GM
  8. #define MODEL_GUID "{41f87101-edf2-4eef-90f3-42db56d4565f}"
  9. #define FMI2_FUNCTION_PREFIX PW_CONTROLLER_SA_
  10. #define MAX 10000
  11. #include <stdio.h>
  12. #include "string.h"
  13. #include "fmi2Functions.h"
  14. #include <float.h>
  15. #include "FMI_GM.h"
  16. #include <math.h>
  17. #include "fmi2.h"
  18. #include "sim_support.h"
  19. #define NUMBER_OF_REALS 1
  20. #define NUMBER_OF_STRINGS 0
  21. #define NUMBER_OF_BOOLEANS 11
  22. #define NUMBER_OF_INTEGERS 0
  23. /*
  24. * The input
  25. */
  26. #define _in_armature_current 0
  27. #define _in_driver_up 0
  28. #define _in_driver_up_stop 1
  29. #define _in_driver_down 2
  30. #define _in_driver_down_stop 3
  31. #define _in_passenger_up 4
  32. #define _in_passenger_up_stop 5
  33. #define _in_passenger_down 6
  34. #define _in_passenger_down_stop 7
  35. /*
  36. * The output:
  37. */
  38. #define _out_u 9
  39. #define _out_d 10
  40. double relativeError(double a, double b){
  41. if(a==0){
  42. return 0;
  43. }
  44. return fabs((a - b) / a);
  45. }
  46. /*
  47. * Helper function for absolute error
  48. */
  49. double absoluteError(double a, double b){
  50. return fabs(a - b);
  51. }
  52. /*
  53. * is_close function for double comparison
  54. */
  55. int is_close(double a, double b, double REL_TOL, double ABS_TOL){
  56. return ((absoluteError(a,b)<ABS_TOL) && (relativeError(a,b)<REL_TOL));
  57. }
  58. fmi2Status fmi2SetDebugLogging(fmi2Component fc, fmi2Boolean loggingOn, size_t nCategories, const fmi2String categories[])
  59. {
  60. return fmi2OK;
  61. }
  62. fmi2Status fmi2SetString(fmi2Component fc, const fmi2ValueReference vr[], size_t nvr, const fmi2String value[])
  63. {
  64. return fmi2Error;
  65. }
  66. fmi2Status fmi2GetString(fmi2Component fc, const fmi2ValueReference vr[], size_t nvr, fmi2String value[])
  67. {
  68. return fmi2Error;
  69. }
  70. fmi2Status fmi2SetReal(fmi2Component fc, const fmi2ValueReference vr[], size_t nvr, const fmi2Real value[])
  71. {
  72. FMUInstance* comp = (FMUInstance *)fc;
  73. int i;
  74. for (i = 0; i < nvr; i++)
  75. {
  76. comp->r[vr[i]] = value[i];
  77. }
  78. /*Generated: */
  79. fmi2Boolean in_condition[_NR_OF_IN_CONDITIONS];
  80. /*Condition checking:*/
  81. // get_next_step ->
  82. in_condition[0] = comp->get_next_step;
  83. //true
  84. in_condition[1] = 1;
  85. if(in_condition[0]){
  86. fmi2Component fmu0_temp;
  87. comp->fmu[0].getFMUstate(comp->c_fmu[0],&fmu0_temp);
  88. fmi2Status status;
  89. status = comp->fmu[0].doStep(comp->c_fmu[0],comp->time_last_fmu[0], MAX, fmi2False);
  90. if(status == fmi2Discard){
  91. fmi2Real nextTime;
  92. comp->fmu[0].getRealStatus(comp->c_fmu[0],fmi2LastSuccessfulTime, &nextTime);
  93. comp->next_time_step = nextTime;
  94. }else{
  95. comp->next_time_step = -1;
  96. }
  97. comp->fmu[0].setFMUstate(comp->c_fmu[0],fmu0_temp);
  98. comp->fmu[0].freeInstance(fmu0_temp);
  99. }
  100. if(in_condition[1]){
  101. comp->stored_arm_current = comp->r[_in_armature_current];
  102. /* If mealy do update_in and recursive call */
  103. }
  104. //out_condition_executed := empty map
  105. memset(comp->out_conditions_executed,0,sizeof(fmi2Boolean)*_NR_OF_OUT_CONDITIONS);
  106. return fmi2OK;
  107. }
  108. fmi2Status fmi2GetReal(fmi2Component fc, const fmi2ValueReference vr[], size_t nvr, fmi2Real value[])
  109. {
  110. FMUInstance* comp = (FMUInstance *)fc;
  111. int i;
  112. for (i = 0; i < nvr; i++)
  113. {
  114. value[i] = comp->r[(vr[i])];
  115. }
  116. return fmi2OK;
  117. }
  118. fmi2Status fmi2SetBoolean(fmi2Component fc, const fmi2ValueReference vr[], size_t nvr, const fmi2Boolean value[])
  119. {
  120. FMUInstance* comp = (FMUInstance *)fc;
  121. int i;
  122. for (i = 0; i < nvr; i++)
  123. {
  124. comp->b[vr[i]] = value[i];
  125. }
  126. return fmi2OK;
  127. }
  128. fmi2Status fmi2GetBoolean(fmi2Component fc, const fmi2ValueReference vr[], size_t nvr, fmi2Boolean value[])
  129. {
  130. FMUInstance* comp = (FMUInstance *)fc;
  131. int i;
  132. int isEmpty = 1;
  133. for (int i=0; i<_NR_OF_OUT_CONDITIONS;i++){
  134. if(comp->out_conditions_executed[i] !=0){
  135. isEmpty = 0;
  136. break;
  137. }
  138. }
  139. /*Eval conditions:*/
  140. if(1){
  141. comp->out_conditions_executed[0] = 1;
  142. }
  143. /*Set correct values */
  144. if (isEmpty){
  145. for(int i=0; i<_NR_OF_OUT_CONDITIONS;i++){
  146. if(comp->out_conditions_executed[i]){
  147. comp->b[_out_u] = comp->stored_up;
  148. comp->b[_out_d] = comp->stored_down;
  149. }
  150. }
  151. }
  152. /*
  153. Copy to given array */
  154. for (i = 0; i < nvr; i++)
  155. {
  156. value[i] = comp->b[vr[i]];
  157. }
  158. return fmi2OK;
  159. }
  160. fmi2Component fmi2Instantiate(fmi2String instanceName, fmi2Type fmuType, fmi2String fmuGUID, fmi2String fmuLocation, const fmi2CallbackFunctions* functions, fmi2Boolean visible, fmi2Boolean loggingOn)
  161. {
  162. //Declare data structure for fmu instance
  163. FMUInstance* fi;
  164. printf("%s in fmiInstantiate\n",instanceName);
  165. //Perform checks on passed callback functions
  166. if (loggingOn) {
  167. if (!functions->logger);
  168. //return NULL;
  169. }
  170. //Check for instanceName
  171. if (!instanceName || strlen(instanceName)==0) {
  172. // print (and/or log) instanceName is missing
  173. //return NULL;
  174. }
  175. //Check passed GUID to defined model GUID
  176. if (strcmp(fmuGUID, MODEL_GUID))
  177. {
  178. // print (and/or log) GUID doesn't match
  179. //return NULL;
  180. }
  181. //Allocate fmu instance Memory
  182. // TODO check if "canNotUseMemoryManagementFunctions == true/false". If false memory allocation not possible
  183. fi = (FMUInstance *)functions->allocateMemory(1, sizeof(FMUInstance));
  184. if (fi) {
  185. // 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??
  186. fi->r = functions->allocateMemory(NUMBER_OF_REALS, sizeof(fmi2Real));
  187. fi->i = functions->allocateMemory(NUMBER_OF_INTEGERS, sizeof(fmi2Integer));
  188. fi->b = functions->allocateMemory(NUMBER_OF_BOOLEANS, sizeof(fmi2Boolean));
  189. fi->s = functions->allocateMemory(NUMBER_OF_STRINGS, sizeof(fmi2String));
  190. } // variables in predefined arrays (performance issue) --> makes multiple instances of fmu impossible
  191. fi->instanceName = functions->allocateMemory(1 + strlen(instanceName), sizeof(char));
  192. fi->GUID = functions->allocateMemory(1 + strlen(fmuGUID), sizeof(char));
  193. strcpy((char*)fi->instanceName, instanceName);
  194. strcpy((char*)fi->GUID, fmuGUID);
  195. fi->functions = functions;
  196. fi->loggingOn = loggingOn;
  197. fi->isVisible = visible;
  198. fi->state = fmuInstantiated;
  199. /* Load the inner FMUs:*/
  200. loadDll("libpw_controller.dll", &(fi->fmu[0]), "PW_Controller");
  201. fi->fmuResourceLocation[0] = "libpw_controller";
  202. /*Instantiate inner components*/
  203. for (int i=0; i<1; i++){
  204. fi->c_fmu[i] = fi->fmu[i].instantiate("inner", fmi2CoSimulation, "1", fi->fmuResourceLocation[i] , fi->functions, visible, 0);
  205. }
  206. return fi;
  207. }
  208. fmi2Status fmi2SetupExperiment(fmi2Component fc, fmi2Boolean toleranceDefined, fmi2Real tolerance,
  209. fmi2Real startTime, fmi2Boolean stopTimeDefined, fmi2Real stopTime) {
  210. FMUInstance* fi = (FMUInstance*) fc;
  211. printf("%s in fmiSetupExperiment\n",fi->instanceName);
  212. if (fi->state != fmuInstantiated)
  213. {
  214. printf("fmu: %s was not instatiated before calling fmiSetupExperiment\n", fi->instanceName);
  215. return fmi2Error;
  216. }
  217. fi->currentTime = startTime;
  218. fi->stopTimeDefined = stopTimeDefined;
  219. fi->toleranceDefined = toleranceDefined;
  220. if (stopTimeDefined)
  221. {
  222. fi->stopTime = stopTime;
  223. }
  224. /*
  225. * setup inner
  226. */
  227. fmi2Status fmi2Flag = fmi2OK;
  228. fi->state = fmuExperimentSettedUp;
  229. for(int i=0; i<1; i++){
  230. fmi2Flag = fi->fmu[i].setupExperiment(fi->c_fmu[i], toleranceDefined, tolerance, startTime, fmi2True, stopTime);
  231. if (fmi2Flag == fmi2Error){
  232. fi->state = fmuError;
  233. }
  234. }
  235. return fmi2Flag;
  236. }
  237. fmi2Status fmi2EnterInitializationMode(fmi2Component fc)
  238. {
  239. FMUInstance* fi = (FMUInstance*) fc;
  240. printf("%s in fmiEnterInitializationMode\n",fi->instanceName);
  241. if (fi->state != fmuExperimentSettedUp)
  242. {
  243. printf("fmu: %s experiment was not set-up before calling fmiEnterInitializationMode\n", fi->instanceName);
  244. return fmi2Error;
  245. }
  246. fi->state = fmuInitMode;
  247. fmi2Status fmi2Flag = fmi2OK;
  248. for(int i=0; i<1; i++){
  249. fmi2Flag = fi->fmu[i].enterInitializationMode(fi->c_fmu[i]);
  250. if (fmi2Flag == fmi2Error){
  251. return fi->state = fmuError;
  252. }
  253. }
  254. fi->ABS_TOLERANCE = 1e-08;
  255. fi->REL_TOLERANCE = 0.0001;
  256. fi->CROSSING = 5;
  257. fi->init_armature_current = fi->CROSSING;
  258. fi->init_down = 0;
  259. fi->init_up = 0;
  260. fi->next_time_step = INFINITY;
  261. fi->stored_arm_current = fi->CROSSING;
  262. fi->aux_obj_detected = 0;
  263. fi->get_next_step = 1;
  264. fi->previous_arm_current = fi->CROSSING;
  265. return fmi2Flag;
  266. }
  267. fmi2Status fmi2ExitInitializationMode(fmi2Component fc)
  268. {
  269. FMUInstance* fi = (FMUInstance*) fc;
  270. printf("%s in fmiExitInitializationMode\n",fi->instanceName);
  271. if (fi->state != fmuInitMode)
  272. {
  273. printf("fmu: %s did not enter Initialization Mode before calling fmiExitInitializationMode\n", fi->instanceName);
  274. return fmi2Error;
  275. }
  276. // TODO
  277. //initStatus = calculateInitialUnknownValues();
  278. //initialize();
  279. fi->state = fmuInitialized;
  280. fmi2Status fmi2Flag = fmi2OK;
  281. for(int i=0; i<1;i++){
  282. fmi2Flag = fi->fmu[i].exitInitializationMode(fi->c_fmu[i]);
  283. if (fmi2Flag == fmi2Error){
  284. return fi->state = fmuError;
  285. }
  286. }
  287. return fmi2Flag;
  288. }
  289. fmi2Status fmi2DoStep(fmi2Component fc , fmi2Real currentCommPoint, fmi2Real commStepSize, fmi2Boolean noPrevFMUState)
  290. {
  291. FMUInstance* fi = (FMUInstance *)fc;
  292. fmi2Status simStatus = fmi2OK;
  293. memset(fi->out_conditions_executed,0,sizeof(fmi2Boolean)*_NR_OF_OUT_CONDITIONS);
  294. fmi2Boolean b_temp_out[2];
  295. printf("%s in fmiDoStep(), ct:%f, h:%f\n",fi->instanceName,currentCommPoint,commStepSize);
  296. fi->aux_obj_detected = 0;
  297. fi->step_size = commStepSize;
  298. if(!(! is_close(fi->previous_arm_current, fi->CROSSING , fi->REL_TOLERANCE, fi->ABS_TOLERANCE) && fi->previous_arm_current < fi->CROSSING)
  299. && (!is_close(fi->stored_arm_current,fi->CROSSING,fi->REL_TOLERANCE,fi->ABS_TOLERANCE) && fi->stored_arm_current > fi->CROSSING)){
  300. double negative_value = fi->previous_arm_current - fi->CROSSING;
  301. double positive_value = fi->stored_arm_current - fi->CROSSING;
  302. fi->step_size = (commStepSize * (-negative_value)) / (positive_value - negative_value);
  303. simStatus = fmi2Discard;
  304. }else{
  305. if(!(! is_close(fi->previous_arm_current,fi->CROSSING,fi->REL_TOLERANCE,fi->ABS_TOLERANCE) && fi->previous_arm_current < fi->CROSSING)
  306. && is_close(fi->stored_arm_current,fi->CROSSING, fi->REL_TOLERANCE, fi->ABS_TOLERANCE)){
  307. fi->aux_obj_detected = 1;
  308. }
  309. fi->step_size = commStepSize;
  310. }
  311. if(fi->aux_obj_detected || (fi->next_time_step != -1 && currentCommPoint > fi->next_time_step ) ||1){ //this is not correct. It also has to work when new values are given to all of the bools
  312. fmi2ValueReference vr_in_star[8] = {1,2,3,4,5,6,7,8};
  313. fi->fmu[0].setBoolean(fi->c_fmu[0], vr_in_star,8, &(fi->b[0]));
  314. fmi2ValueReference vr_aux_obj_detected[1] = {0};
  315. fi->fmu[0].setBoolean(fi->c_fmu[0], vr_aux_obj_detected,1, &(fi->aux_obj_detected));
  316. simStatus = fi->fmu[0].doStep(fi->c_fmu[0], currentCommPoint, commStepSize, fmi2True);
  317. fmi2ValueReference vr_out_star[2]={9,10};
  318. fi->fmu[0].getBoolean(fi->c_fmu[0],vr_out_star,2,&b_temp_out[0]);
  319. fi->get_next_step = 1;
  320. }else{
  321. fi->get_next_step = 0;
  322. }
  323. if(is_close(fi->step_size, commStepSize, fi->REL_TOLERANCE,fi->ABS_TOLERANCE)){
  324. fi->previous_arm_current = fi->stored_arm_current;
  325. }
  326. if(simStatus == fmi2Discard){
  327. fmi2Real theNextTime = currentCommPoint + fi->step_size;
  328. for(int i=0; i<1; i++){
  329. fmi2Real theFMUtime;
  330. fi->fmu[i].getRealStatus(fi->c_fmu[i], fmi2LastSuccessfulTime, &theFMUtime);
  331. if(theFMUtime<theNextTime){
  332. theNextTime = theFMUtime;
  333. }
  334. }
  335. fi->currentTime = theNextTime;
  336. }else if(simStatus == fmi2OK){
  337. fi->currentTime = currentCommPoint + commStepSize;
  338. fi->time_last_fmu[0] = currentCommPoint + commStepSize;
  339. }else{
  340. }
  341. /* do out functions*/
  342. if(b_temp_out[0]){
  343. fi->stored_up = 1;
  344. }
  345. if(!b_temp_out[0]){
  346. fi->stored_up = 0;
  347. }
  348. if(b_temp_out[1]){
  349. fi->stored_down = 1;
  350. }
  351. if(!b_temp_out[1]){
  352. fi->stored_down = 0;
  353. }
  354. memset(fi->in_condition_executed, 0, sizeof(fmi2Boolean)*_NR_OF_IN_CONDITIONS);
  355. return simStatus;
  356. }
  357. fmi2Status fmi2Terminate(fmi2Component fc)
  358. {
  359. FMUInstance* fi = (FMUInstance *)fc;
  360. printf("%s in fmiTerminate\n",fi->instanceName);
  361. // do check if fi may be terminated
  362. for (int i=0;i<1;i++){
  363. fi->fmu[i].terminate(fi->c_fmu[i]);
  364. }
  365. fi->state = fmuTerminated;
  366. return fmi2OK;
  367. }
  368. void fmi2FreeInstance(fmi2Component fc)
  369. {
  370. FMUInstance* fi = (FMUInstance*) fc;
  371. printf("%s in fmiFreeInstance\n",fi->instanceName);
  372. for(int i=0;i<1;i++){
  373. fi->fmu[i].freeInstance(fi->c_fmu[i]);
  374. }
  375. if (fi) {
  376. fi->functions->freeMemory(fi->r);
  377. fi->functions->freeMemory(fi->i);
  378. fi->functions->freeMemory(fi->b);
  379. fi->functions->freeMemory(fi->s);// TODO has to be done with loop
  380. fi->functions->freeMemory((void*)fi->instanceName);
  381. fi->functions->freeMemory((void*)fi->GUID);
  382. fi->functions->freeMemory((void*)fi);
  383. }
  384. }
  385. //To be implemented
  386. const char* fmi2GetVersion() {
  387. printf("Function fmiGetVersion not supported\n");
  388. return NULL;
  389. }
  390. const char* fmi2GetTypesPlatform() {
  391. printf("Function fmiGetTypesPlatform not supported\n");
  392. return NULL;
  393. }
  394. fmi2Status fmi2Reset(fmi2Component fc)
  395. {
  396. printf("Function fmiReset not supported\n");
  397. return fmi2Error;
  398. }
  399. fmi2Status fmi2SetInteger(fmi2Component fc, const fmi2ValueReference vr[], size_t nvr, const fmi2Integer value[])
  400. {
  401. printf("Function fmiSetInteger not supported\n");
  402. return fmi2Error;
  403. }
  404. fmi2Status fmi2GetInteger(fmi2Component fc, const fmi2ValueReference vr[], size_t nvr, fmi2Integer value[])
  405. {
  406. printf("Function fmiGetInteger not supported\n");
  407. return fmi2Error;
  408. }
  409. /*******OWN IMPLEMENTATION OF Get/Set FMU state*******/
  410. fmi2Status fmi2GetFMUstate (fmi2Component c, fmi2FMUstate* FMUstate) {
  411. FMUInstance* orig = (FMUInstance*)c;
  412. FMUInstance* fi = (FMUInstance *)FMUstate;
  413. *FMUstate = fi;
  414. fi = orig->functions->allocateMemory(1, sizeof(FMUInstance));
  415. *FMUstate = fi;
  416. fi->functions = orig->functions;
  417. if (fi) {
  418. // 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??
  419. fi->r = fi->functions->allocateMemory(NUMBER_OF_REALS, sizeof(fmi2Real));
  420. fi->i = fi->functions->allocateMemory(NUMBER_OF_INTEGERS, sizeof(fmi2Integer));
  421. fi->b = fi->functions->allocateMemory(NUMBER_OF_BOOLEANS, sizeof(fmi2Boolean));
  422. fi->s = fi->functions->allocateMemory(NUMBER_OF_STRINGS, sizeof(fmi2String));
  423. } // variables in predefined arrays (performance issue) --> makes multiple instances of fmu impossible
  424. fi->instanceName = orig->functions->allocateMemory(1 + strlen(orig->instanceName), sizeof(char));
  425. fi->GUID = orig->functions->allocateMemory(1 + strlen(orig->GUID), sizeof(char));
  426. strcpy((char *)fi->instanceName, (char *)orig->instanceName);
  427. strcpy((char *)fi->GUID, (char *)orig->GUID);
  428. fi->functions = orig->functions;
  429. fi->loggingOn = orig->loggingOn;
  430. fi->isVisible = orig->isVisible;
  431. fi->state = orig->state;
  432. fi->startTime = orig->startTime;
  433. fi->stopTime = orig->stopTime;
  434. fi->currentTime = orig->currentTime;
  435. /* TODO: Store all the rest here.*/
  436. fi->fmu[0] = orig->fmu[0];
  437. fi->c_fmu[0] = orig->c_fmu[0];
  438. for(int i=0; i<_NR_OF_IN_CONDITIONS;i++){
  439. fi->in_condition_executed[i] = orig->in_condition_executed[i];
  440. }
  441. for(int i=0; i<_NR_OF_OUT_CONDITIONS;i++){
  442. fi->out_conditions_executed[i] = orig->out_conditions_executed[i];
  443. }
  444. for(int i=0;i<_NR_OF_FMUS;i++){
  445. fi->time_last_fmu[i] = orig->time_last_fmu[i];
  446. }
  447. /* Generated */
  448. fi->REL_TOLERANCE = orig->REL_TOLERANCE;
  449. fi->ABS_TOLERANCE = orig->ABS_TOLERANCE;
  450. fi->CROSSING = orig->CROSSING;
  451. fi->init_up = orig->init_up;
  452. fi->init_down = orig->init_down;
  453. fi->init_armature_current = orig->init_armature_current;
  454. fi->next_time_step = orig->next_time_step;
  455. fi->stored_arm_current = orig->stored_arm_current;
  456. fi->step_size = orig->step_size;
  457. fi->aux_obj_detected = orig->aux_obj_detected;
  458. fi->get_next_step = orig->get_next_step;
  459. fi->previous_arm_current = orig->previous_arm_current;
  460. fi->stored_up = orig->stored_up;
  461. fi->stored_down = orig->stored_down;
  462. fi->toleranceDefined = orig->toleranceDefined;
  463. /*
  464. * This is a hierarchical call. First let the lower FMUs do their state saving
  465. * We will store the saved fmu state in the fi->c_order[i]
  466. */
  467. for(int i=0;i<1;i++){
  468. fi->fmu[i]=orig->fmu[i];
  469. orig->fmu[i].getFMUstate(orig->c_fmu[i],fi->c_fmu[i]);
  470. fi->fmuResourceLocation[i] = fi->functions->allocateMemory(1+strlen(orig->fmuResourceLocation[i]), sizeof(char));
  471. strcpy((char *)fi->fmuResourceLocation[i],(char *)orig->fmuResourceLocation[i]);
  472. /*make shallow copies of the stored fmus*/
  473. }
  474. //copy r
  475. int i=0;
  476. for (i=0; i< NUMBER_OF_REALS;i++){
  477. printf("Setting real: %i %f\n", i, orig->r[i]);
  478. fi->r[i] = orig->r[i];
  479. printf("Setted real: %i %f\n", i, fi->r[i]);
  480. }
  481. //copy s
  482. for (i=0; i< NUMBER_OF_STRINGS;i++){
  483. //fi->s[i] = orig->s[i]; // why are this not deep copies?
  484. fi->s[i] = fi->functions->allocateMemory(1+strlen(orig->s[i]),sizeof(char));
  485. strcpy((char *)fi->s[i],(char *)orig->s[i]);
  486. }
  487. //copy i
  488. for (i=0; i< NUMBER_OF_INTEGERS;i++){
  489. fi->i[i] = orig->i[i];
  490. }
  491. //copy b
  492. for (i=0; i< NUMBER_OF_BOOLEANS;i++){
  493. fi->b[i] = orig->b[i];
  494. }
  495. return fmi2OK;
  496. }
  497. fmi2Status fmi2SetFMUstate (fmi2Component c, fmi2FMUstate FMUstate) {
  498. FMUInstance* orig = (FMUInstance*)FMUstate;
  499. FMUInstance* fi = (FMUInstance*)c;
  500. /*
  501. * First restore the hierarchical fmus.
  502. */
  503. for(int i=0;i<1;i++){
  504. fi->fmu[i].setFMUstate(fi->c_fmu[i],orig->c_fmu[i]);
  505. fi->fmuResourceLocation[i] = orig->functions->allocateMemory(1+strlen(orig->fmuResourceLocation[i]), sizeof(char));
  506. strcpy((char *)fi->fmuResourceLocation[i],(char *)orig->fmuResourceLocation[i]);
  507. }
  508. //set time etc correct, name and GUID should still be ok ;-)
  509. printf("setting time values from %f to %f\n", fi->currentTime, orig->currentTime);
  510. fi->state = orig->state;
  511. fi->startTime = orig->startTime;
  512. fi->stopTime = orig->stopTime;
  513. fi->currentTime = orig->currentTime;
  514. fi->fmu[0] = orig->fmu[0];
  515. fi->c_fmu[0] = orig->c_fmu[0];
  516. for(int i=0; i<_NR_OF_IN_CONDITIONS;i++){
  517. fi->in_condition_executed[i] = orig->in_condition_executed[i];
  518. }
  519. for(int i=0; i<_NR_OF_OUT_CONDITIONS;i++){
  520. fi->out_conditions_executed[i] = orig->out_conditions_executed[i];
  521. }
  522. for(int i=0;i<_NR_OF_FMUS;i++){
  523. fi->time_last_fmu[i] = orig->time_last_fmu[i];
  524. }
  525. /* Generated */
  526. fi->REL_TOLERANCE = orig->REL_TOLERANCE;
  527. fi->ABS_TOLERANCE = orig->ABS_TOLERANCE;
  528. fi->CROSSING = orig->CROSSING;
  529. fi->init_up = orig->init_up;
  530. fi->init_down = orig->init_down;
  531. fi->init_armature_current = orig->init_armature_current;
  532. fi->next_time_step = orig->next_time_step;
  533. fi->stored_arm_current = orig->stored_arm_current;
  534. fi->step_size = orig->step_size;
  535. fi->aux_obj_detected = orig->aux_obj_detected;
  536. fi->get_next_step = orig->get_next_step;
  537. fi->previous_arm_current = orig->previous_arm_current;
  538. fi->stored_up = orig->stored_up;
  539. fi->stored_down = orig->stored_down;
  540. fi->toleranceDefined = orig->toleranceDefined;
  541. fi->toleranceDefined = orig->toleranceDefined;
  542. printf("setting real values\n");
  543. //copy r
  544. int i=0;
  545. for (i=0; i< NUMBER_OF_REALS;i++){
  546. fi->r[i] = orig->r[i];
  547. }
  548. printf("setting string values\n");
  549. //copy s
  550. for (i=0; i< NUMBER_OF_STRINGS;i++){
  551. fi->s[i] = orig->s[i];
  552. }
  553. //copy i
  554. for (i=0; i< NUMBER_OF_INTEGERS;i++){
  555. fi->i[i] = orig->i[i];
  556. }
  557. //copy b
  558. for (i=0; i< NUMBER_OF_BOOLEANS;i++){
  559. fi->b[i] = orig->b[i];
  560. }
  561. return fmi2OK;
  562. }
  563. /****************************************************/
  564. fmi2Status fmi2FreeFMUstate(fmi2Component c, fmi2FMUstate* FMUstate) {
  565. printf("Function fmiFreeFMUstate not supported\n");
  566. return fmi2Error;
  567. }
  568. fmi2Status fmi2SerializedFMUstateSize(fmi2Component c, fmi2FMUstate FMUstate, size_t *size) {
  569. printf("Function fmiSerializedFMUstateSize not supported\n");
  570. return fmi2Error;
  571. }
  572. fmi2Status fmi2SerializeFMUstate (fmi2Component c, fmi2FMUstate FMUstate, fmi2Byte serializedState[], size_t size) {
  573. printf("Function fmiSerializeFMUstate not supported\n");
  574. return fmi2Error;
  575. }
  576. fmi2Status fmi2DeSerializeFMUstate (fmi2Component c, const fmi2Byte serializedState[], size_t size, fmi2FMUstate* FMUstate) {
  577. printf("Function fmiDeSerializeFMUstate not supported\n");
  578. return fmi2Error;
  579. }
  580. fmi2Status fmi2GetDirectionalDerivative(fmi2Component c, const fmi2ValueReference vUnknown_ref[], size_t nUnknown,
  581. const fmi2ValueReference vKnown_ref[] , size_t nKnown, const fmi2Real dvKnown[], fmi2Real dvUnknown[]) {
  582. printf("Function fmiGetDirectionalDerivative not supported\n");
  583. return fmi2Error;
  584. }
  585. fmi2Status fmi2SetRealInputDerivatives(fmi2Component c, const fmi2ValueReference vr[], size_t nvr,
  586. const fmi2Integer order[], const fmi2Real value[]) {
  587. printf("Function fmiGetDirectionalDerivative not supported\n");
  588. return fmi2Error;
  589. }
  590. fmi2Status fmi2GetRealOutputDerivatives(fmi2Component c, const fmi2ValueReference vr[], size_t nvr,
  591. const fmi2Integer order[], fmi2Real value[]) {
  592. printf("Function fmiGetDirectionalDerivative not supported\n");
  593. return fmi2Error;
  594. }
  595. fmi2Status fmi2CancelStep(fmi2Component c) {
  596. printf("Function fmiGetDirectionalDerivative not supported\n");
  597. return fmi2Error;
  598. }
  599. fmi2Status fmi2GetStatus(fmi2Component c, const fmi2StatusKind s, fmi2Status *value) {
  600. printf("Function fmiGetStatus not supported\n");
  601. return fmi2Error;
  602. }
  603. fmi2Status fmi2GetRealStatus(fmi2Component c, const fmi2StatusKind s, fmi2Real *value) {
  604. if(s == fmi2LastSuccessfulTime){
  605. FMUInstance* comp = (FMUInstance*) c;
  606. *value = comp->currentTime;
  607. return fmi2OK;
  608. }
  609. printf("Function fmiGetRealStatus not supported\n");
  610. return fmi2Error;
  611. }
  612. fmi2Status fmi2GetIntegerStatus(fmi2Component c, const fmi2StatusKind s, fmi2Integer *value) {
  613. printf("Function fmiGetIntegerStatus not supported\n");
  614. return fmi2Error;
  615. }
  616. fmi2Status fmi2GetBooleanStatus(fmi2Component c, const fmi2StatusKind s, fmi2Boolean *value) {
  617. printf("Function fmiGetBooleanStatus not supported\n");
  618. return fmi2Error;
  619. }
  620. fmi2Status fmi2GetStringStatus(fmi2Component c, const fmi2StatusKind s, fmi2String *value) {
  621. printf("Function fmiGetStringStatus not supported\n");
  622. return fmi2Error;
  623. }