FMI_GM.c 23 KB

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