|
@@ -7,7 +7,7 @@ Template for a FMU
|
|
|
|
|
|
#define MODEL_IDENTIFIER GM
|
|
|
#define MODEL_GUID "{41f87101-edf2-4eef-90f3-42db56d4565f}"
|
|
|
-#define FMI2_FUNCTION_PREFIX POWER_SA
|
|
|
+#define FMI2_FUNCTION_PREFIX LOOP_SA
|
|
|
|
|
|
|
|
|
#include <stdio.h>
|
|
@@ -36,7 +36,13 @@ Template for a FMU
|
|
|
#define _in_displacement 2
|
|
|
|
|
|
|
|
|
+#define _window_sa 0
|
|
|
+#define _obstacle 1
|
|
|
+
|
|
|
double relativeError(double a, double b){
|
|
|
+ if (a == 0){
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
return fabs((a - b) / a);
|
|
|
}
|
|
|
|
|
@@ -202,20 +208,21 @@ fmi2Component fmi2Instantiate(fmi2String instanceName, fmi2Type fmuType, fmi2Str
|
|
|
fi->state = fmuInstantiated;
|
|
|
/* Load the inner FMUs:*/
|
|
|
|
|
|
- loadDll("libFMI_Window_sa.dll", &(fi->fmu[0]), "WINDOW_SA");
|
|
|
- fi->fmuResourceLocation[0] = "libFMI_Window_sa";
|
|
|
- loadDll("Obstacle.dll", &(fi->fmu[1]),"");
|
|
|
- fi->fmuResourceLocation[1] = "Obstacle.dll";
|
|
|
+ loadDll("libFMI_Window_sa.dll", &(fi->fmu[_window_sa]), "WINDOW_SA");
|
|
|
+ fi->fmuResourceLocation[_window_sa] = "libFMI_Window_sa";
|
|
|
+ loadDll("Obstacle.dll", &(fi->fmu[_obstacle]),"");
|
|
|
+ fi->fmuResourceLocation[_obstacle] = "Obstacle.dll";
|
|
|
|
|
|
- fi->fmu_guid[0]= functions->allocateMemory(1 + strlen("1"), sizeof(char));
|
|
|
- fi->fmu_guid[1] = functions->allocateMemory(1 + strlen("{c6327117-e5f2-4e48-abcd-318439d1e7c4}"), sizeof(char));
|
|
|
- strcpy(fi->fmu_guid[0], "1");
|
|
|
- strcpy(fi->fmu_guid[1], "{c6327117-e5f2-4e48-abcd-318439d1e7c4}");
|
|
|
+ fi->fmu_guid[_window_sa]= functions->allocateMemory(1 + strlen("1"), sizeof(char));
|
|
|
+ fi->fmu_guid[_obstacle] = functions->allocateMemory(1 + strlen("{c6327117-e5f2-4e48-abcd-318439d1e7c4}"), sizeof(char));
|
|
|
+ strcpy(fi->fmu_guid[_window_sa], "1");
|
|
|
+ strcpy(fi->fmu_guid[_obstacle], "{8de5bd74-8d30-4a72-9170-0e4bf874b6a8}");
|
|
|
|
|
|
/*Instantiate inner components*/
|
|
|
- for (int i=0; i<1; i++){
|
|
|
- fi->c_fmu[i] = fi->fmu[i].instantiate("inner", fmi2CoSimulation, fi->fmu_guid[i], fi->fmuResourceLocation[i] , fi->functions, visible, 0);
|
|
|
- }
|
|
|
+
|
|
|
+ fi->c_fmu[_window_sa] = fi->fmu[_window_sa].instantiate("window_sa", fmi2CoSimulation, fi->fmu_guid[_window_sa], fi->fmuResourceLocation[_window_sa] , fi->functions, visible, 0);
|
|
|
+ fi->c_fmu[_obstacle] = fi->fmu[_obstacle].instantiate("obstacle", fmi2CoSimulation, fi->fmu_guid[_obstacle], fi->fmuResourceLocation[_obstacle] , fi->functions, visible, 0);
|
|
|
+
|
|
|
return fi;
|
|
|
}
|
|
|
|
|
@@ -242,7 +249,7 @@ fmi2Status fmi2SetupExperiment(fmi2Component fc, fmi2Boolean toleranceDefined, f
|
|
|
*/
|
|
|
fmi2Status fmi2Flag = fmi2OK;
|
|
|
fi->state = fmuExperimentSettedUp;
|
|
|
- for(int i=0; i<1; i++){
|
|
|
+ for(int i=0; i<_NR_OF_FMUS; i++){
|
|
|
fmi2Flag = fi->fmu[i].setupExperiment(fi->c_fmu[i], toleranceDefined, tolerance, startTime, fmi2True, stopTime);
|
|
|
if (fmi2Flag == fmi2Error){
|
|
|
fi->state = fmuError;
|
|
@@ -265,7 +272,7 @@ fmi2Status fmi2EnterInitializationMode(fmi2Component fc)
|
|
|
}
|
|
|
fi->state = fmuInitMode;
|
|
|
fmi2Status fmi2Flag = fmi2OK;
|
|
|
- for(int i=0; i<1; i++){
|
|
|
+ for(int i=0; i<_NR_OF_FMUS; i++){
|
|
|
fmi2Flag = fi->fmu[i].enterInitializationMode(fi->c_fmu[i]);
|
|
|
if (fmi2Flag == fmi2Error){
|
|
|
return fi->state = fmuError;
|
|
@@ -288,7 +295,7 @@ fmi2Status fmi2ExitInitializationMode(fmi2Component fc)
|
|
|
//initialize();
|
|
|
fi->state = fmuInitialized;
|
|
|
fmi2Status fmi2Flag = fmi2OK;
|
|
|
- for(int i=0; i<1;i++){
|
|
|
+ for(int i=0; i<_NR_OF_FMUS;i++){
|
|
|
fmi2Flag = fi->fmu[i].exitInitializationMode(fi->c_fmu[i]);
|
|
|
if (fmi2Flag == fmi2Error){
|
|
|
return fi->state = fmuError;
|
|
@@ -304,19 +311,47 @@ static fmi2Status DoInnerStep(fmi2Component fc, int index, fmi2Real currentCommP
|
|
|
fmi2Real dt =currentCommPoint - fi->time_last_fmu[index];
|
|
|
fmi2Real h = commStepSize + dt;
|
|
|
int repeat = 0;
|
|
|
+
|
|
|
+ fmi2Component obstacle_temp, window_sa_temp;
|
|
|
+ fmi2ValueReference vr_to_window_sa[3]={2,3,4};
|
|
|
+ fmi2ValueReference vr_disp[1] = {2};
|
|
|
+ fmi2ValueReference vr_F_obstacle[1] = {0};
|
|
|
+ fmi2ValueReference vr_from_window[2] = {0,1};
|
|
|
+ fmi2Real toWindowSA[3];
|
|
|
+ fmi2Real fromWindow[2];
|
|
|
for (int iter= 0; iter< MAXITER; iter++) {
|
|
|
- save_state(obstacle);
|
|
|
- save_state(window_sa);
|
|
|
- obstacle.disp := prev_disp; do_step(obstacle,t,H); do_step(window_sa,t,H);
|
|
|
- repeat := is_close(prev_disp, window_sa.disp, REL_TOL, ABS_TOL); prev_disp := window_sa.disp;
|
|
|
- if (repeat) {
|
|
|
- break; } else {
|
|
|
- rollback(obstacle);
|
|
|
- rollback(window_sa);
|
|
|
+ fi->fmu[_obstacle].getFMUstate(fi->c_fmu[_obstacle], &obstacle_temp);
|
|
|
+ fi->fmu[_window_sa].getFMUstate(fi->c_fmu[_window_sa], &window_sa_temp);
|
|
|
+
|
|
|
+ fi->fmu[_obstacle].setReal(fi->c_fmu[_obstacle], vr_disp,1, &fi->prev_disp);
|
|
|
+ fi->fmu[_obstacle].doStep(fi->c_fmu[_obstacle], currentCommPoint, commStepSize, fmi2True);
|
|
|
+ fi->fmu[_obstacle].getReal(fi->c_fmu[_obstacle],vr_F_obstacle,1,&toWindowSA[0]);
|
|
|
+
|
|
|
+ toWindowSA[1] = fi->r[_in_displacement];
|
|
|
+ toWindowSA[2] = fi->r[_in_speed];
|
|
|
+ fi->fmu[_window_sa].setReal(fi->c_fmu[_window_sa], vr_to_window_sa, 3, &toWindowSA[0]);
|
|
|
+ fi->fmu[_window_sa].doStep(fi->c_fmu[_window_sa], currentCommPoint, commStepSize, fmi2True);
|
|
|
+ fi->fmu[_window_sa].getReal(fi->c_fmu[_window_sa], vr_from_window, 2, &fromWindow[0]);
|
|
|
+
|
|
|
+ repeat = is_close(fi->prev_disp, fromWindow[1], REL_TOL, ABS_TOL);
|
|
|
+ fi->prev_disp = fromWindow[1];
|
|
|
+ if (repeat) {
|
|
|
+ break;
|
|
|
+ } else {
|
|
|
+ //rollback(obstacle);
|
|
|
+ fi->fmu[_obstacle].setFMUstate(fi->c_fmu[_obstacle], obstacle_temp);
|
|
|
+ fi->fmu[_window_sa].setFMUstate(fi->c_fmu[_window_sa], window_sa_temp);
|
|
|
+
|
|
|
+ //rollback(window_sa);
|
|
|
+ }
|
|
|
+ fi->fmu[_obstacle].freeInstance(obstacle_temp);
|
|
|
+ fi->fmu[_window_sa].freeInstance(window_sa_temp);
|
|
|
+ if(1){
|
|
|
+ fi->r[_out_tau] = fromWindow[0];
|
|
|
+ }
|
|
|
}
|
|
|
return status;
|
|
|
}
|
|
|
-
|
|
|
fmi2Status fmi2DoStep(fmi2Component fc , fmi2Real currentCommPoint, fmi2Real commStepSize, fmi2Boolean noPrevFMUState)
|
|
|
{
|
|
|
FMUInstance* fi = (FMUInstance *)fc;
|
|
@@ -454,12 +489,7 @@ fmi2Status fmi2GetFMUstate (fmi2Component c, fmi2FMUstate* FMUstate) {
|
|
|
}
|
|
|
|
|
|
/* Generated */
|
|
|
- fi->stored_windowsa_u = orig->stored_windowsa_u;
|
|
|
- fi->stored_windowsa_d = orig->stored_windowsa_d;
|
|
|
- fi->stored_tau = orig->stored_tau;
|
|
|
- fi->stored_armature_current = orig->stored_armature_current;
|
|
|
- fi->stored_speed = orig->stored_speed;
|
|
|
- fi->stored_displacement = orig->stored_displacement;
|
|
|
+ fi->prev_disp = orig->prev_disp;
|
|
|
|
|
|
fi->toleranceDefined = orig->toleranceDefined;
|
|
|
/*
|
|
@@ -529,12 +559,7 @@ fmi2Status fmi2SetFMUstate (fmi2Component c, fmi2FMUstate FMUstate) {
|
|
|
}
|
|
|
|
|
|
/* Generated */
|
|
|
- fi->stored_windowsa_u = orig->stored_windowsa_u;
|
|
|
- fi->stored_windowsa_d = orig->stored_windowsa_d;
|
|
|
- fi->stored_tau = orig->stored_tau;
|
|
|
- fi->stored_armature_current = orig->stored_armature_current;
|
|
|
- fi->stored_speed = orig->stored_speed;
|
|
|
- fi->stored_displacement = orig->stored_displacement;
|
|
|
+ fi->prev_disp = orig->prev_disp;
|
|
|
|
|
|
fi->toleranceDefined = orig->toleranceDefined;
|
|
|
|