Skip to content

Commit ece2b64

Browse files
lochellochel
committed
Fix edge case for time events and handle terminate requests by FMUs (#1368)
1 parent 088ef19 commit ece2b64

3 files changed

Lines changed: 90 additions & 50 deletions

File tree

‎src/OMSimulatorLib/ComponentFMUCS.cpp‎

Lines changed: 8 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -881,11 +881,14 @@ oms_status_enu_t oms::ComponentFMUCS::stepUntil(double stopTime)
881881
fmi2Status status = fmi2_doStep(fmu, time, hdef, fmi2True);
882882
time += hdef;
883883

884-
#if !defined(NO_TLM)
885-
//Write to TLM sockets if top level system is of TLM type
886-
if(topLevelSystem->getType() == oms_system_tlm)
887-
reinterpret_cast<SystemTLM*>(topLevelSystem)->writeToSockets(reinterpret_cast<SystemWC*>(getParentSystem()), time, this);
888-
#endif
884+
if (status == fmi2Discard)
885+
{
886+
getModel().setStopTime(time);
887+
logInfo("fmi2_doStep discarded for FMU \"" + std::string(getFullCref()) + "\"");
888+
return oms_status_ok;
889+
}
890+
else if (status != fmi2OK)
891+
return logError_FMUCall("fmi2_doStep", this);
889892
}
890893
time = stopTime;
891894
return oms_status_ok;

‎src/OMSimulatorLib/SystemSC.cpp‎

Lines changed: 79 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -49,27 +49,27 @@ int oms::cvode_rhs(realtype t, N_Vector y, N_Vector ydot, void* user_data)
4949
fmi2Status fmistatus;
5050

5151
// update states in FMUs
52-
for (int i=0, j=0; i < system->fmus.size(); ++i)
52+
for (size_t i=0, j=0; i < system->fmus.size(); ++i)
5353
{
54+
// set time
55+
fmistatus = fmi2_setTime(system->fmus[i]->getFMU(), t);
56+
if (fmi2OK != fmistatus) logError_FMUCall("fmi2_setTime", system->fmus[i]);
57+
5458
if (0 == system->nStates[i])
5559
continue;
5660

57-
for (int k = 0; k < system->nStates[i]; k++, j++)
61+
for (size_t k = 0; k < system->nStates[i]; k++, j++)
5862
system->states[i][k] = NV_Ith_S(y, j);
5963

6064
// set states
6165
status = system->fmus[i]->setContinuousStates(system->states[i]);
6266
if (oms_status_ok != status) return status;
63-
64-
// set time
65-
fmistatus = fmi2_setTime(system->fmus[i]->getFMU(), t);
66-
if (fmi2OK != fmistatus) logError_FMUCall("fmi2_setTime", system->fmus[i]);
6767
}
6868

6969
system->updateInputs(system->eventGraph);
7070

7171
// get state derivatives
72-
for (int j=0, k=0; j < system->fmus.size(); ++j)
72+
for (size_t j=0, k=0; j < system->fmus.size(); ++j)
7373
{
7474
if (0 == system->nStates[j])
7575
continue;
@@ -93,21 +93,21 @@ int oms::cvode_roots(realtype t, N_Vector y, realtype *gout, void *user_data)
9393
fmi2Status fmistatus;
9494

9595
// update states in FMUs
96-
for (int i=0, j=0; i < system->fmus.size(); ++i)
96+
for (size_t i=0, j=0; i < system->fmus.size(); ++i)
9797
{
98+
// set time
99+
fmistatus = fmi2_setTime(system->fmus[i]->getFMU(), t);
100+
if (fmi2OK != fmistatus) logError_FMUCall("fmi2_setTime", system->fmus[i]);
101+
98102
if (0 == system->nStates[i])
99103
continue;
100104

101-
for (int k = 0; k < system->nStates[i]; k++, j++)
105+
for (size_t k = 0; k < system->nStates[i]; k++, j++)
102106
system->states[i][k] = NV_Ith_S(y, j);
103107

104108
// set states
105109
status = system->fmus[i]->setContinuousStates(system->states[i]);
106110
if (oms_status_ok != status) return status;
107-
108-
// set time
109-
fmistatus = fmi2_setTime(system->fmus[i]->getFMU(), t);
110-
if (fmi2OK != fmistatus) logError_FMUCall("fmi2_setTime", system->fmus[i]);
111111
}
112112

113113
system->updateInputs(system->eventGraph);
@@ -312,7 +312,7 @@ oms_status_enu_t oms::SystemSC::initialize()
312312

313313
// Check if nominals are greater 0
314314
bool illegalNominals = false;
315-
for(int l=0; l<nStates[i]; l++)
315+
for(size_t l=0; l<nStates[i]; l++)
316316
{
317317
if (states_nominal[i][l] <= 0)
318318
{
@@ -339,19 +339,19 @@ oms_status_enu_t oms::SystemSC::initialize()
339339
if (oms_solver_sc_cvode == solverMethod)
340340
{
341341
size_t n_states = 0;
342-
for (int i=0; i < fmus.size(); ++i)
342+
for (size_t i=0; i < fmus.size(); ++i)
343343
n_states += nStates[i];
344344

345345
solverData.cvode.y = N_VNew_Serial(static_cast<long>(n_states));
346346
if (!solverData.cvode.y) logError("SUNDIALS_ERROR: N_VNew_Serial() failed - returned NULL pointer");
347-
for (int j=0, k=0; j < fmus.size(); ++j)
347+
for (size_t j=0, k=0; j < fmus.size(); ++j)
348348
for (size_t i=0; i < nStates[j]; ++i, ++k)
349349
NV_Ith_S(solverData.cvode.y, k) = states[j][i];
350350
//N_VPrint_Serial(solverData.cvode.y);
351351

352352
solverData.cvode.abstol = N_VNew_Serial(static_cast<long>(n_states));
353353
if (!solverData.cvode.abstol) logError("SUNDIALS_ERROR: N_VNew_Serial() failed - returned NULL pointer");
354-
for (int j=0, k=0; j < fmus.size(); ++j)
354+
for (size_t j=0, k=0; j < fmus.size(); ++j)
355355
for (size_t i=0; i < nStates[j]; ++i, ++k)
356356
NV_Ith_S(solverData.cvode.abstol, k) = 0.01*absoluteTolerance*states_nominal[j][i];
357357
//N_VPrint_Serial(solverData.cvode.abstol);
@@ -594,12 +594,22 @@ oms_status_enu_t oms::SystemSC::doStepEuler()
594594
bool event_detected = false;
595595

596596
fmi2Real tnext = end_time + 1.0;
597-
for (int i = 0; i < fmus.size(); ++i)
597+
bool terminated = false;
598+
for (size_t i = 0; i < fmus.size(); ++i)
599+
{
598600
if (fmus[i]->getEventInfo()->nextEventTimeDefined && (tnext > fmus[i]->getEventInfo()->nextEventTime) && (time < fmus[i]->getEventInfo()->nextEventTime))
599601
tnext = fmus[i]->getEventInfo()->nextEventTime;
600602

603+
if(fmus[i]->getEventInfo()->terminateSimulation)
604+
{
605+
logInfo("Simulation terminated by FMU " + std::string(fmus[i]->getFullCref()) + " at time " + std::to_string(time));
606+
getModel().setStopTime(time);
607+
terminated = true;
608+
}
609+
}
610+
601611
// Step 3: Main integration loop
602-
while (time < end_time)
612+
while (time < end_time && !terminated)
603613
{
604614
if(tnext < event_time)
605615
event_time = tnext;
@@ -611,27 +621,28 @@ oms_status_enu_t oms::SystemSC::doStepEuler()
611621
logDebug("step_size: " + std::to_string(step_size) + " | " + std::to_string(time) + " -> " + std::to_string(event_time));
612622
for (size_t i = 0; i < fmus.size(); ++i)
613623
{
624+
fmistatus = fmi2_setTime(fmus[i]->getFMU(), event_time);
625+
if (fmi2OK != fmistatus) logError_FMUCall("fmi2_setTime", fmus[i]);
626+
614627
if (0 == nStates[i])
615628
continue;
616629

617-
for (int k = 0; k < nStates[i]; ++k)
630+
for (size_t k = 0; k < nStates[i]; ++k)
618631
states[i][k] = states_backup[i][k] + step_size * states_der_backup[i][k];
619632

620633
status = fmus[i]->setContinuousStates(states[i]);
621634
if (oms_status_ok != status) return status;
622-
623-
fmistatus = fmi2_setTime(fmus[i]->getFMU(), event_time);
624-
if (fmi2OK != fmistatus) logError_FMUCall("fmi2_setTime", fmus[i]);
625635
}
626636

627637
// b. Event Detection
628638
event_detected = event_time == tnext;
639+
logDebug("Event detected: " + std::to_string(event_detected));
629640
for (size_t i = 0; i < fmus.size() && !event_detected; ++i)
630641
{
631642
fmistatus = fmi2_getEventIndicators(fmus[i]->getFMU(), event_indicators[i], nEventIndicators[i]);
632643
if (fmi2OK != fmistatus) logError_FMUCall("fmi2_getEventIndicators", fmus[i]);
633644

634-
for (int k=0; k < nEventIndicators[i]; k++)
645+
for (size_t k=0; k < nEventIndicators[i]; k++)
635646
{
636647
if ((event_indicators[i][k] > 0) != (event_indicators_prev[i][k] > 0))
637648
{
@@ -653,7 +664,7 @@ oms_status_enu_t oms::SystemSC::doStepEuler()
653664
time = event_time;
654665
step_size_adjustment = maximumStepSize;
655666

656-
for (int i = 0; i < fmus.size(); ++i)
667+
for (size_t i = 0; i < fmus.size(); ++i)
657668
{
658669
fmistatus = fmi2_completedIntegratorStep(fmus[i]->getFMU(), fmi2True, &callEventUpdate[i], &terminateSimulation[i]);
659670
if (fmi2OK != fmistatus) return logError_FMUCall("fmi2_completedIntegratorStep", fmus[i]);
@@ -689,7 +700,7 @@ oms_status_enu_t oms::SystemSC::doStepEuler()
689700
getModel().emit(time, false);
690701

691702
// Enter event mode and handle discrete state updates for each FMU
692-
for (int i = 0; i < fmus.size(); ++i)
703+
for (size_t i = 0; i < fmus.size(); ++i)
693704
{
694705
fmistatus = fmi2_completedIntegratorStep(fmus[i]->getFMU(), fmi2True, &callEventUpdate[i], &terminateSimulation[i]);
695706
if (fmi2OK != fmistatus) return logError_FMUCall("fmi2_completedIntegratorStep", fmus[i]);
@@ -715,11 +726,19 @@ oms_status_enu_t oms::SystemSC::doStepEuler()
715726
}
716727

717728
// find next time event
718-
tnext = end_time + 1.0;
719-
for (int i = 0; i < fmus.size(); ++i)
720-
if (fmus[i]->getEventInfo()->nextEventTimeDefined && (tnext > fmus[i]->getEventInfo()->nextEventTime))
729+
tnext = end_time + 1.234;
730+
for (size_t i = 0; i < fmus.size(); ++i)
731+
{
732+
if (fmus[i]->getEventInfo()->nextEventTimeDefined && (tnext > fmus[i]->getEventInfo()->nextEventTime) && (time < fmus[i]->getEventInfo()->nextEventTime))
721733
tnext = fmus[i]->getEventInfo()->nextEventTime;
722-
logDebug("tnext: " + std::to_string(tnext));
734+
735+
if(fmus[i]->getEventInfo()->terminateSimulation)
736+
{
737+
logInfo("Simulation terminated by FMU " + std::string(fmus[i]->getFullCref()) + " at time " + std::to_string(time));
738+
getModel().setStopTime(time);
739+
terminated = true;
740+
}
741+
}
723742

724743
// emit the right limit of the event
725744
updateInputs(eventGraph);
@@ -756,23 +775,32 @@ oms_status_enu_t oms::SystemSC::doStepCVODE()
756775

757776
// find next time event
758777
fmi2Real tnext = end_time+1.0;
759-
for (int i = 0; i < fmus.size(); ++i)
778+
for (size_t i = 0; i < fmus.size(); ++i)
779+
{
760780
if (fmus[i]->getEventInfo()->nextEventTimeDefined && (tnext > fmus[i]->getEventInfo()->nextEventTime))
761781
tnext = fmus[i]->getEventInfo()->nextEventTime;
782+
783+
if(fmus[i]->getEventInfo()->terminateSimulation)
784+
{
785+
logInfo("Simulation terminated by FMU " + std::string(fmus[i]->getFullCref()) + " at time " + std::to_string(time));
786+
getModel().setStopTime(time);
787+
time = end_time;
788+
}
789+
}
762790
logDebug("tnext: " + std::to_string(tnext));
763791

764792
while (time < end_time)
765793
{
766794
logDebug("CVode: " + std::to_string(time) + " -> " + std::to_string(end_time));
767-
for (int j=0, k=0; j < fmus.size(); ++j)
795+
for (size_t j=0, k=0; j < fmus.size(); ++j)
768796
for (size_t i=0; i < nStates[j]; ++i, ++k)
769797
NV_Ith_S(solverData.cvode.y, k) = states[j][i];
770798

771799
flag = CVode(solverData.cvode.mem, std::min(tnext, end_time), solverData.cvode.y, &time, CV_NORMAL);
772800

773-
for (int i = 0, j=0; i < fmus.size(); ++i)
801+
for (size_t i = 0, j=0; i < fmus.size(); ++i)
774802
{
775-
for (int k = 0; k < nStates[i]; k++, j++)
803+
for (size_t k = 0; k < nStates[i]; k++, j++)
776804
states[i][k] = NV_Ith_S(solverData.cvode.y, j);
777805

778806
// set states
@@ -787,7 +815,7 @@ oms_status_enu_t oms::SystemSC::doStepCVODE()
787815
if (flag == CV_ROOT_RETURN || time == tnext)
788816
{
789817
//logInfo("event found!!! " + std::to_string(time));
790-
for (int i = 0; i < fmus.size(); ++i)
818+
for (size_t i = 0; i < fmus.size(); ++i)
791819
{
792820
fmistatus = fmi2_completedIntegratorStep(fmus[i]->getFMU(), fmi2True, &callEventUpdate[i], &terminateSimulation[i]);
793821
if (fmi2OK != fmistatus) return logError_FMUCall("fmi2_completedIntegratorStep", fmus[i]);
@@ -798,7 +826,7 @@ oms_status_enu_t oms::SystemSC::doStepCVODE()
798826
getModel().emit(time, false);
799827

800828
// Enter event mode and handle discrete state updates for each FMU
801-
for (int i = 0; i < fmus.size(); ++i)
829+
for (size_t i = 0; i < fmus.size(); ++i)
802830
{
803831
fmistatus = fmi2_enterEventMode(fmus[i]->getFMU());
804832
if (fmi2OK != fmistatus) logError_FMUCall("fmi2_enterEventMode", fmus[i]);
@@ -809,7 +837,7 @@ oms_status_enu_t oms::SystemSC::doStepCVODE()
809837
if (fmi2OK != fmistatus) logError_FMUCall("fmi2_enterContinuousTimeMode", fmus[i]);
810838
}
811839

812-
for (int i = 0; i < fmus.size(); ++i)
840+
for (size_t i = 0; i < fmus.size(); ++i)
813841
{
814842
if (0 == nStates[i])
815843
continue;
@@ -820,9 +848,18 @@ oms_status_enu_t oms::SystemSC::doStepCVODE()
820848

821849
// find next time event
822850
tnext = end_time+1.0;
823-
for (int i = 0; i < fmus.size(); ++i)
851+
for (size_t i = 0; i < fmus.size(); ++i)
852+
{
824853
if (fmus[i]->getEventInfo()->nextEventTimeDefined && (tnext > fmus[i]->getEventInfo()->nextEventTime))
825854
tnext = fmus[i]->getEventInfo()->nextEventTime;
855+
856+
if(fmus[i]->getEventInfo()->terminateSimulation)
857+
{
858+
logInfo("Simulation terminated by FMU " + std::string(fmus[i]->getFullCref()) + " at time " + std::to_string(time));
859+
getModel().setStopTime(time);
860+
time = end_time;
861+
}
862+
}
826863
logDebug("tnext: " + std::to_string(tnext));
827864

828865
// emit the right limit of the event
@@ -839,7 +876,7 @@ oms_status_enu_t oms::SystemSC::doStepCVODE()
839876
if (oms_status_ok != status) return status;
840877
}
841878

842-
for (int j=0, k=0; j < fmus.size(); ++j)
879+
for (size_t j=0, k=0; j < fmus.size(); ++j)
843880
for (size_t i=0; i < nStates[j]; ++i, ++k)
844881
NV_Ith_S(solverData.cvode.y, k) = states[j][i];
845882

@@ -852,7 +889,7 @@ oms_status_enu_t oms::SystemSC::doStepCVODE()
852889
if (flag == CV_SUCCESS)
853890
{
854891
logDebug("CVode completed successfully at t = " + std::to_string(time));
855-
for (int i = 0; i < fmus.size(); ++i)
892+
for (size_t i = 0; i < fmus.size(); ++i)
856893
{
857894
fmistatus = fmi2_completedIntegratorStep(fmus[i]->getFMU(), fmi2True, &callEventUpdate[i], &terminateSimulation[i]);
858895
if (fmi2OK != fmistatus) return logError_FMUCall("fmi2_completedIntegratorStep", fmus[i]);
@@ -886,7 +923,7 @@ oms_status_enu_t oms::SystemSC::stepUntil(double stopTime)
886923

887924
// main simulation loop
888925
oms_status_enu_t status = oms_status_ok;
889-
while (time < stopTime && oms_status_ok == status)
926+
while (time < std::min(stopTime, getModel().getStopTime()) && oms_status_ok == status)
890927
{
891928
status = doStep();
892929
if (status != oms_status_ok)
@@ -932,7 +969,7 @@ oms_status_enu_t oms::SystemSC::updateInputs(DirectedGraph& graph)
932969
const std::vector< scc_t >& sortedConnections = graph.getSortedConnections();
933970
updateAlgebraicLoops(sortedConnections, graph);
934971

935-
for(int i=0; i<sortedConnections.size(); i++)
972+
for(size_t i=0; i<sortedConnections.size(); i++)
936973
{
937974
if (!sortedConnections[i].thisIsALoop)
938975
{

‎src/OMSimulatorLib/SystemWC.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -552,7 +552,7 @@ oms_status_enu_t oms::SystemWC::doStep()
552552
tNext = stopTime;
553553

554554
double h = tNext - time;
555-
logDebug("doStep: " + std::to_string(time) + " -> " + std::to_string(tNext));
555+
logDebug("doStep: " + std::to_string(time) + " -> " + std::to_string(tNext) + ", stopTime " + std::to_string(stopTime) + ", h " + std::to_string(h));
556556

557557
// save component's state
558558
if (masiMax > 1)
@@ -837,7 +837,7 @@ oms_status_enu_t oms::SystemWC::stepUntil(double stopTime)
837837

838838
// main simulation loop
839839
oms_status_enu_t status = oms_status_ok;
840-
while (time < stopTime)
840+
while (time < std::min(stopTime, getModel().getStopTime()) && oms_status_ok == status)
841841
{
842842
status = doStep();
843843

@@ -856,7 +856,7 @@ oms_status_enu_t oms::SystemWC::stepUntil(double stopTime)
856856

857857
// main simulation loop
858858
oms_status_enu_t status = oms_status_ok;
859-
while (time < stopTime && oms_status_ok == status)
859+
while (time < std::min(stopTime, getModel().getStopTime()) && oms_status_ok == status)
860860
{
861861
status = doStep();
862862

0 commit comments

Comments
 (0)