diff --git a/flight/UAVObjects/eventdispatcher.c b/flight/UAVObjects/eventdispatcher.c index 402f508c7..353739468 100644 --- a/flight/UAVObjects/eventdispatcher.c +++ b/flight/UAVObjects/eventdispatcher.c @@ -310,6 +310,7 @@ static int32_t processPeriodicUpdates() PeriodicObjectList* objEntry; int32_t timeNow; int32_t timeToNextUpdate; + int32_t offset; // Get lock xSemaphoreTakeRecursive(mutex, portMAX_DELAY); @@ -327,7 +328,8 @@ static int32_t processPeriodicUpdates() if (objEntry->timeToNextUpdateMs <= timeNow) { // Reset timer - objEntry->timeToNextUpdateMs = timeNow + objEntry->updatePeriodMs; + offset = ( timeNow - objEntry->timeToNextUpdateMs ) % objEntry->updatePeriodMs; + objEntry->timeToNextUpdateMs = timeNow + objEntry->updatePeriodMs - offset; // Invoke callback, if one if ( objEntry->evInfo.cb != 0) { diff --git a/ground/src/plugins/uavtalk/telemetry.cpp b/ground/src/plugins/uavtalk/telemetry.cpp index 5ecd5752e..0c7770449 100644 --- a/ground/src/plugins/uavtalk/telemetry.cpp +++ b/ground/src/plugins/uavtalk/telemetry.cpp @@ -437,6 +437,7 @@ void Telemetry::processPeriodicUpdates() ObjectTimeInfo objinfo; qint32 elapsedMs = 0; QTime time; + qint32 offset; for (int n = 0; n < objList.length(); ++n) { objinfo = objList[n]; @@ -448,7 +449,8 @@ void Telemetry::processPeriodicUpdates() if (objinfo.timeToNextUpdateMs <= 0) { // Reset timer - objinfo.timeToNextUpdateMs = objinfo.updatePeriodMs; + offset = (-objinfo.timeToNextUpdateMs) % objinfo.updatePeriodMs; + objinfo.timeToNextUpdateMs = objinfo.updatePeriodMs - offset; // Send object time.start(); processObjectUpdates(objinfo.obj, EV_UPDATED_MANUAL, true, false);