interpolate inputs with input_real_derivatives (#8364)

* interpolate inputs with input_real_derivatives

* Fix the dt and add hack for debugging

* remove intermediate steps and add test for iterpolation

Co-authored-by: Lennart Ochel <lennart.ochel@ri.se>

OMCompiler/SimulationRuntime/fmi/export/openmodelica/fmu2_model_interface.c.inc

@@ -582,7 +582,10 @@ fmi2Component fmi2Instantiate(fmi2String instanceName, fmi2Type fmuType, fmi2Str
   comp->states_der = (fmi2Real*)functions->allocateMemory(NUMBER_OF_STATES, sizeof(fmi2Real));
   comp->event_indicators = (fmi2Real*)functions->allocateMemory(NUMBER_OF_EVENT_INDICATORS, sizeof(fmi2Real));
   comp->event_indicators_prev = (fmi2Real*)functions->allocateMemory(NUMBER_OF_EVENT_INDICATORS, sizeof(fmi2Real));
-  comp->input_real_derivative = (fmi2Real*)functions->allocateMemory(NUMBER_OF_REAL_INPUTS, sizeof(fmi2Real));
+  if (NUMBER_OF_REAL_INPUTS == 0)
+    comp->input_real_derivative = NULL; // initialize to 0
+  else
+    comp->input_real_derivative = (fmi2Real*)functions->allocateMemory(NUMBER_OF_REAL_INPUTS, sizeof(fmi2Real));
 
   comp->_need_update = 1;
 
@@ -1578,6 +1581,18 @@ fmi2Status fmi2DoStep(fmi2Component c, fmi2Real currentCommunicationPoint, fmi2R
 
   tCommunication = currentCommunicationPoint;
 
+  // copy the input values
+  fmi2Real realInputDerivatives[NUMBER_OF_REAL_INPUTS];
+  if (NUMBER_OF_REAL_INPUTS > 0)
+  {
+    for (int i = 0; i < NUMBER_OF_REALS; ++i)
+    {
+      int mappedIndex = mapInputReference2InputNumber(i);
+      if (mappedIndex != -1)
+        realInputDerivatives[mappedIndex] = getReal(comp, i);
+    }
+  }
+
   fmi2EnterEventMode(c);
   fmi2EventIteration(c, &eventInfo);
   fmi2EnterContinuousTimeMode(c);
@@ -1591,6 +1606,22 @@ fmi2Status fmi2DoStep(fmi2Component c, fmi2Real currentCommunicationPoint, fmi2R
 
     /* fprintf(stderr, "DoStep %g -> %g State: %s\n", comp->fmuData->localData[0]->timeValue, tNext, stateToString(comp)); */
 
+    // set the real Inputs with output_derivative values
+    if (NUMBER_OF_REAL_INPUTS > 0)
+    {
+      for (int i = 0; i < NUMBER_OF_REALS; ++i)
+      {
+        int mappedIndex = mapInputReference2InputNumber(i);
+        if (mapInputReference2InputNumber(i) != -1)
+        {
+          double dt = comp->fmuData->localData[0]->timeValue - t;
+          double new_input_value = realInputDerivatives[mappedIndex] + comp->input_real_derivative[mappedIndex] * dt;
+          if (setReal(comp, i, new_input_value) != fmi2OK) // to be implemented by the includer of this file
+            return fmi2Error;
+        }
+      }
+    }
+
     if (NUMBER_OF_STATES > 0)
     {
       status = fmi2GetDerivatives(c, states_der, NUMBER_OF_STATES);
@@ -1643,6 +1674,22 @@ fmi2Status fmi2DoStep(fmi2Component c, fmi2Real currentCommunicationPoint, fmi2R
     }
     fmi2SetTime(c, tNext);
 
+    // set the real Inputs with output_derivative values
+    if (NUMBER_OF_REAL_INPUTS > 0)
+    {
+      for (int i = 0; i < NUMBER_OF_REALS; ++i)
+      {
+        int mappedIndex = mapInputReference2InputNumber(i);
+        if (mapInputReference2InputNumber(i) != -1)
+        {
+          double dt = comp->fmuData->localData[0]->timeValue - t;
+          double new_input_value = realInputDerivatives[mappedIndex] + comp->input_real_derivative[mappedIndex] * dt;
+          if (setReal(comp, i, new_input_value) != fmi2OK) // to be implemented by the includer of this file
+            return fmi2Error;
+        }
+      }
+    }
+
     /* set the continuous states */
     if (NUMBER_OF_STATES > 0)
     {

testsuite/omsimulator/Makefile

@@ -7,6 +7,7 @@ DualMassOscillator.mos \
 enumeration.mos \
 enumeration2.mos \
 enumeration3.mos \
+fmi_interpolate_cs.mos \
 initialization_omc.mos \
 initialization.mos \
 initialization2_omc.mos \

testsuite/omsimulator/fmi_interpolate_cs.mos

@@ -0,0 +1,64 @@
+// keywords: fmu export
+// status: correct
+// teardown_command: rm -rf fmi_interpolate_cs.lua fmi_interpolate_cs_res.mat fmu1.fmu fmu2.fmu fmi_interpolate_cs_fmu1.log fmi_interpolate_cs_fmu2.log fmi_interpolate_cs_systemCall.log temp-fmi_interpolate_cs/
+
+loadString("
+model fmu1
+  output Real y = sin(time);
+end fmu1;
+
+model fmu2
+  input Real u;
+  Real x(start=0.0, fixed=true);
+equation
+  der(x) = u;
+end fmu2;
+"); getErrorString();
+
+
+buildModelFMU(fmu1, version="2.0", fmuType="cs", platforms={"static"}); getErrorString();
+buildModelFMU(fmu2, version="2.0", fmuType="cs", platforms={"static"}); getErrorString();
+
+writeFile("fmi_interpolate_cs.lua", "
+oms_setCommandLineOption(\"--suppressPath=true --inputExtrapolation=true\")
+oms_setTempDirectory(\"./temp-fmi_interpolate_cs/\")
+
+oms_newModel(\"fmi_interpolate_cs\")
+oms_addSystem(\"fmi_interpolate_cs.root\", oms_system_wc)
+oms_addSubModel(\"fmi_interpolate_cs.root.fmu1\", \"fmu1.fmu\")
+oms_addSubModel(\"fmi_interpolate_cs.root.fmu2\", \"fmu2.fmu\")
+
+oms_addConnection(\"fmi_interpolate_cs.root.fmu1.y\", \"fmi_interpolate_cs.root.fmu2.u\")
+
+oms_setStopTime(\"fmi_interpolate_cs\", 5)
+oms_setFixedStepSize(\"fmi_interpolate_cs.root\", 0.1)
+
+oms_instantiate(\"fmi_interpolate_cs\")
+
+oms_initialize(\"fmi_interpolate_cs\")
+
+oms_simulate(\"fmi_interpolate_cs\")
+
+oms_terminate(\"fmi_interpolate_cs\")
+oms_delete(\"fmi_interpolate_cs\")
+"); getErrorString();
+
+system(getInstallationDirectoryPath() + "/bin/OMSimulator fmi_interpolate_cs.lua", "fmi_interpolate_cs_systemCall.log");
+readFile("fmi_interpolate_cs_systemCall.log");
+
+// Result:
+// true
+// ""
+// "fmu1.fmu"
+// ""
+// "fmu2.fmu"
+// ""
+// true
+// ""
+// 0
+// "warning: Not all components support \"canGetAndSetState\"; an explicit master algorithm will be used
+// info:    Result file: fmi_interpolate_cs_res.mat (bufferSize=10)
+// info:    1 warnings
+// info:    0 errors
+// "
+// endResult