Skip to content

Commit 8181087

Browse files
miDebraffael0
miDeb
authored and
raffael0
committed
update sequence manager loop implementation
factor out linear and no interpolation to dedicated functions.
1 parent 92d91bb commit 8181087

File tree

3 files changed

+161
-88
lines changed

3 files changed

+161
-88
lines changed

‎include/SequenceManager.h‎

Lines changed: 31 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,36 @@ class SequenceManager : public Singleton<SequenceManager>
6060

6161
void plotMaps(uint8_t option);
6262

63+
/**
64+
* Interpolate linearly between start and end values.
65+
*
66+
* @param startTimeAndValues A tuple containing the start time and values.
67+
* The first element is the start time,
68+
* and the second element is a vector of values.
69+
* @param endTimeAndValues An optional tuple containing the end time and values.
70+
* The first element is the end ,
71+
* and the second element is a vector of values.
72+
* @param currentTime The current time.
73+
* @return A tuple containing:
74+
* - A vector of interpolated values, if one could be computed.
75+
* - A boolean indicating whether the interpolation is complete. If true,
76+
* the end value should be used as the new start value for the next interpolation.
77+
*/
78+
static std::tuple<std::optional<std::vector<double>>, bool> interpolateLinear(
79+
const std::tuple<int64_t, std::vector<double>> &startTimeAndValues,
80+
const std::optional<std::tuple<int64_t, std::vector<double>>>& endTimeAndValues,
81+
int64_t currentTime
82+
);
83+
84+
/**
85+
* No interpolation. Conforms to the api of interpolateLinear.
86+
*/
87+
static std::tuple<std::optional<std::vector<double>>, bool> interpolateNone(
88+
const std::tuple<int64_t, std::vector<double>> &startTimeAndValues,
89+
const std::optional<std::tuple<int64_t, std::vector<double>>>& endTimeAndValues,
90+
int64_t currentTime
91+
);
92+
6393
bool sequenceRunning = false;
6494
bool sequenceToStop = false;
6595

@@ -97,6 +127,6 @@ class SequenceManager : public Singleton<SequenceManager>
97127
LLInterface *llInterface = nullptr;
98128
EventManager *eventManager = nullptr;
99129
FileSystemAbstraction *fileSystem = nullptr;
100-
130+
101131
std::thread sequenceThread;
102132
};

‎src/SequenceManager.cpp‎

Lines changed: 129 additions & 86 deletions
Original file line numberDiff line numberDiff line change
@@ -436,85 +436,72 @@ void SequenceManager::sequenceLoop(int64_t interval_us)
436436
msg += std::to_string(sensor.second.begin()->second[1]) + ";";
437437
}
438438

439-
bool shallExec;
440-
std::vector<double> nextValue = {0};
441-
442439
for (const auto &devItem : deviceMap)
443440
{
441+
if (devItem.second.empty())
442+
{
443+
continue;
444+
}
445+
auto currentItem = *devItem.second.begin();
446+
auto nextItem = devItem.second.size() > 1
447+
? std::optional(*std::next(devItem.second.begin()))
448+
: std::nullopt;
449+
std::optional<std::vector<double>> nextValue;
450+
bool shouldAdvance;
451+
452+
Interpolation inter = Interpolation::NONE;
453+
if (interpolationMap.find(devItem.first) == interpolationMap.end())
454+
{
455+
Debug::error("%s not found in interpolation map, falling back to no interpolation",
456+
devItem.first.c_str());
457+
}
458+
else
459+
{
460+
inter = interpolationMap[devItem.first];
461+
}
444462

445-
shallExec = true;
463+
switch (inter)
464+
{
465+
case Interpolation::LINEAR:
466+
{
467+
std::tie(nextValue, shouldAdvance) = interpolateLinear(
468+
currentItem,
469+
nextItem,
470+
sequenceTime_us
471+
);
472+
break;
473+
}
474+
case Interpolation::NONE:
475+
default:
476+
std::tie(nextValue, shouldAdvance) = interpolateNone(
477+
currentItem,
478+
nextItem,
479+
sequenceTime_us
480+
);
481+
break;
482+
}
446483

447-
if (devItem.second.size() > 1)
448-
{
449-
auto currentItem = devItem.second.begin();
450-
auto nextItem = std::next(devItem.second.begin());
484+
if (nextValue.has_value())
485+
{
486+
try
487+
{
488+
//Debug::error("%d: %s, %f", microTime, devItem.first.c_str(), nextValue[0]);
489+
eventManager->ExecuteCommand(devItem.first, nextValue.value(), false);
490+
}
491+
catch (const std::exception& e)
492+
{
493+
Debug::error("SequenceManager::sequenceLoop ExecuteCommand error: %s", e.what());
494+
}
451495

452-
if (sequenceTime_us >= nextItem->first)
453-
{
454-
nextValue = nextItem->second;
455-
deviceMap[devItem.first].erase(deviceMap[devItem.first].begin());
456-
}
457-
else
458-
{
459-
Interpolation inter = Interpolation::NONE;
460-
if (interpolationMap.find(devItem.first) == interpolationMap.end())
461-
{
462-
Debug::error("%s not found in interpolation map, falling back to no interpolation", devItem.first.c_str());
463-
}
464-
else
465-
{
466-
inter = interpolationMap[devItem.first];
467-
}
468-
switch (inter)
469-
{
470-
case Interpolation::LINEAR:
471-
{
472-
nextValue = currentItem->second;
473-
double scale = ((nextItem->second[0] - currentItem->second[0]) * 1.0) / (nextItem->first - currentItem->first);
474-
nextValue[0] = (scale * (sequenceTime_us - currentItem->first)) + currentItem->second[0];
475-
break;
476-
}
477-
case Interpolation::NONE:
478-
default:
479-
nextValue = currentItem->second;
480-
if (sequenceStartTime == currentItem->first)
481-
{
482-
shallExec = true;
483-
deviceMap[devItem.first].erase(deviceMap[devItem.first].begin());
484-
485-
}
486-
else {
487-
shallExec = false;
488-
}
489-
}
490-
}
491-
492-
if (shallExec)
493-
{
494-
try
495-
{
496-
//Debug::error("%d: %s, %f", microTime, devItem.first.c_str(), nextValue[0]);
497-
eventManager->ExecuteCommand(devItem.first, nextValue, false);
498-
}
499-
catch(const std::exception& e)
500-
{
501-
Debug::error("SequenceManager::sequenceLoop ExecuteCommand error: %s", e.what());
502-
}
503-
}
504-
}
505-
else if (devItem.second.size() ==1)
506-
{
507-
auto currentItem = devItem.second.begin();
508-
if (sequenceTime_us >= currentItem->first) {
509-
nextValue = currentItem->second;
510-
deviceMap[devItem.first].erase(deviceMap[devItem.first].begin());
511-
eventManager->ExecuteCommand(devItem.first, nextValue, false);
512-
}
496+
std::stringstream nextValueStringStream;
497+
std::copy(nextValue.value().begin(), nextValue.value().end(), std::ostream_iterator<double>(nextValueStringStream, ", "));
498+
msg += "[" + nextValueStringStream.str() + "];";
513499
}
514500

515-
std::stringstream nextValueStringStream;
516-
std::copy(nextValue.begin(), nextValue.end(), std::ostream_iterator<double>(nextValueStringStream, ", "));
517-
msg += "[" + nextValueStringStream.str() + "];";
501+
if (shouldAdvance)
502+
{
503+
deviceMap[devItem.first].erase(deviceMap[devItem.first].begin());
504+
}
518505
}
519506

520507
//delete depricated timestamp and update sensorsNominalRangeMap as well
@@ -526,26 +513,82 @@ void SequenceManager::sequenceLoop(int64_t interval_us)
526513
{
527514
for (const auto& sensor : beginRangeIt->second)
528515
{
529-
sensorsNominalRangeMap[sensor.first].erase(sensorsNominalRangeMap[sensor.first].begin());
530-
}
531-
sensorsNominalRangeTimeMap.erase(beginRangeIt);
532-
//plotMaps();
533-
Debug::info("updated sensor ranges at time: %d in ms", sequenceTime_us/1000);
534-
}
535-
}
516+
sensorsNominalRangeMap[sensor.first].erase(sensorsNominalRangeMap[sensor.first].begin());
517+
}
518+
sensorsNominalRangeTimeMap.erase(beginRangeIt);
519+
//plotMaps();
520+
Debug::info("updated sensor ranges at time: %d in ms", sequenceTime_us / 1000);
521+
}
522+
}
536523

537-
CheckSensors(sequenceTime_us);
524+
CheckSensors(sequenceTime_us);
538525

539-
syncMtx.unlock();
540-
Debug::log(msg + "\n");
541-
}
542-
sequenceToStop = false;
526+
syncMtx.unlock();
527+
Debug::log(msg + "\n");
528+
}
529+
sequenceToStop = false;
543530

544-
Debug::info("Sequence ended");
531+
Debug::info("Sequence ended");
545532

546533
Debug::flush();
547534

548-
sequenceRunning = false;
535+
sequenceRunning = false;
536+
}
537+
538+
std::tuple<std::optional<std::vector<double>>, bool> SequenceManager::interpolateLinear(
539+
const std::tuple<int64_t, std::vector<double>>& startTimeAndValues,
540+
const std::optional<std::tuple<int64_t, std::vector<double>>>& endTimeAndValues,
541+
const int64_t currentTime
542+
)
543+
{
544+
if (currentTime < std::get<0>(startTimeAndValues))
545+
{
546+
return {std::nullopt, false}; // We have not yet started the interpolation
547+
}
548+
549+
if (!endTimeAndValues.has_value())
550+
{
551+
// No end, return the start value and finish this interpolation
552+
return {std::get<1>(startTimeAndValues), true};
553+
}
554+
555+
if (currentTime >= std::get<0>(endTimeAndValues.value()))
556+
{
557+
// We have reached the end time, return the end value and finish this interpolation
558+
return {std::get<1>(endTimeAndValues.value()), true};
559+
}
560+
561+
// We are in the middle of the interpolation
562+
const std::vector<double>& startValues = std::get<1>(startTimeAndValues);
563+
const std::vector<double>& endValues = std::get<1>(endTimeAndValues.value());
564+
565+
const int64_t startTime = std::get<0>(startTimeAndValues);
566+
const int64_t endTime = std::get<0>(endTimeAndValues.value());
567+
568+
const double scale = static_cast<double>(currentTime - startTime) / static_cast<double>(endTime - startTime);
569+
570+
std::vector<double> interpolatedValues(startValues.size());
571+
for (size_t i = 0; i < startValues.size(); ++i)
572+
{
573+
interpolatedValues[i] = startValues[i] + scale * (endValues[i] - startValues[i]);
574+
}
575+
576+
return {interpolatedValues, false}; // Return the interpolated values and indicate that we are not done yet
577+
}
578+
579+
std::tuple<std::optional<std::vector<double>>, bool> SequenceManager::interpolateNone(
580+
const std::tuple<int64_t, std::vector<double>> &startTimeAndValues,
581+
const std::optional<std::tuple<int64_t, std::vector<double>>>& endTimeAndValues,
582+
const int64_t currentTime
583+
)
584+
{
585+
if (currentTime < std::get<0>(startTimeAndValues))
586+
{
587+
return {std::nullopt, false}; // We have not yet started the interpolation
588+
} else {
589+
// return the start value and indicate we're done.
590+
return {std::get<1>(startTimeAndValues), true};
591+
}
549592
}
550593

551594
void SequenceManager::abortSequence()

‎tests/SequenceManagerTest.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -157,7 +157,7 @@ TEST_F(SequenceManagerTest, LinearInterpolationIsCorrect) {
157157
// We should have approximatly 100 calls.
158158
EXPECT_NEAR(observed.size(), expected_calls,1);
159159
// Check linearity: value should be start_value + (end_value - start_value) * (t/duration)
160-
for (int i = 0; i < expected_calls; ++i) {
160+
for (int i = 0; i < observed.size(); ++i) {
161161
double t = observed[i].first;
162162
double value = observed[i].second;
163163
double expected = start_value + (end_value - start_value) * (t / duration);

0 commit comments

Comments
 (0)