Fix int32_t microseconds overflow in navigation.

src/main/common/time.h

@@ -24,10 +24,19 @@
 
 #include "config/parameter_group.h"
 
-// time difference, 32 bits always sufficient
+// time difference, signed 32 bits of microseconds overflows at ~35 minutes
+// this is worth leaving as int32_t for performance reasons, use timeDeltaLarge_t for deltas that can be big
 typedef int32_t timeDelta_t;
+#define TIMEDELTA_MAX INT32_MAX
+
+// time difference large, signed 64 bits of microseconds overflows at ~300000 years
+typedef int64_t timeDeltaLarge_t;
+#define TIMEDELTALARGE_MAX INT64_MAX
+
 // millisecond time
-typedef uint32_t timeMs_t ;
+typedef uint32_t timeMs_t;
+#define TIMEMS_MAX UINT32_MAX
+
 // microsecond time
 #ifdef USE_64BIT_TIME
 typedef uint64_t timeUs_t;
@@ -48,6 +57,7 @@ typedef uint32_t timeUs_t;
 #define MS2S(ms)    ((ms) * 1e-3f)
 #define HZ2S(hz)    US2S(HZ2US(hz))
 
+// Use this function only to get small deltas (difference overflows at ~35 minutes)
 static inline timeDelta_t cmpTimeUs(timeUs_t a, timeUs_t b) { return (timeDelta_t)(a - b); }
 
 typedef enum {

src/main/io/ledstrip.c

@@ -935,7 +935,7 @@ void ledStripUpdate(timeUs_t currentTimeUs)
     for (timId_e timId = 0; timId < timTimerCount; timId++) {
         // sanitize timer value, so that it can be safely incremented. Handles inital timerVal value.
         // max delay is limited to 5s
-        int32_t delta = cmpTimeUs(currentTimeUs, timerVal[timId]);
+        timeDelta_t delta = cmpTimeUs(currentTimeUs, timerVal[timId]);
         if (delta < 0 && delta > -LED_STRIP_MS(5000))
             continue;  // not ready yet
         timActive |= 1 << timId;

src/main/io/osd.c

@@ -967,7 +967,7 @@ static bool osdIsHeadingValid(void)
     } else {
         return isImuHeadingValid();
     }
-#else 
+#else
     return isImuHeadingValid();
 #endif
 }
@@ -980,7 +980,7 @@ int16_t osdGetHeading(void)
     } else {
         return attitude.values.yaw;
     }
-#else 
+#else
     return attitude.values.yaw;
 #endif
 }
@@ -1614,7 +1614,7 @@ static bool osdDrawSingleElement(uint8_t item)
             /*static int32_t updatedTimeSeconds = 0;*/
             timeUs_t currentTimeUs = micros();
             static int32_t timeSeconds = -1;
-            if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) {
+            if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) {
 #ifdef USE_WIND_ESTIMATOR
                 timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation);
 #else
@@ -1645,7 +1645,7 @@ static bool osdDrawSingleElement(uint8_t item)
         static timeUs_t updatedTimestamp = 0;
         timeUs_t currentTimeUs = micros();
         static int32_t distanceMeters = -1;
-        if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) {
+        if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) {
 #ifdef USE_WIND_ESTIMATOR
             distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation);
 #else
@@ -1911,7 +1911,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 pitchAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.pitch);
             } else {
                 rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
-                pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch);    
+                pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch);
             }
 #else
             rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
@@ -2232,7 +2232,7 @@ static bool osdDrawSingleElement(uint8_t item)
             if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
                 if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
                     value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
-                        1, efficiencyTimeDelta * 1e-6f);
+                        1, US2S(efficiencyTimeDelta));
 
                     efficiencyUpdated = currentTimeUs;
                 } else {
@@ -2262,7 +2262,7 @@ static bool osdDrawSingleElement(uint8_t item)
             if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
                 if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
                     value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f,
-                        1, efficiencyTimeDelta * 1e-6f);
+                        1, US2S(efficiencyTimeDelta));
 
                     efficiencyUpdated = currentTimeUs;
                 } else {
@@ -3296,7 +3296,7 @@ static void osdShowArmed(void)
 
 static void osdFilterData(timeUs_t currentTimeUs) {
     static timeUs_t lastRefresh = 0;
-    float refresh_dT = cmpTimeUs(currentTimeUs, lastRefresh) * 1e-6;
+    float refresh_dT = US2S(cmpTimeUs(currentTimeUs, lastRefresh));
 
     GForce = sqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS;
     for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS;

src/main/io/osd_dji_hd.c

@@ -112,7 +112,7 @@
 })
 
 
-/* 
+/*
  * DJI HD goggles use MSPv1 compatible with Betaflight 4.1.0
  * DJI uses a subset of messages and assume fixed bit positions for flight modes
  *
@@ -685,7 +685,7 @@ static void osdDJIFormatDistanceStr(char *buff, int32_t dist)
             if (abs(centifeet) < FEET_PER_MILE * 100 / 2) {
                 // Show feet when dist < 0.5mi
                 tfp_sprintf(buff, "%d%s", (int)(centifeet / 100), "FT");
-            } 
+            }
             else {
                 // Show miles when dist >= 0.5mi
                 tfp_sprintf(buff, "%d.%02d%s", (int)(centifeet / (100*FEET_PER_MILE)),
@@ -722,7 +722,7 @@ static void osdDJIEfficiencyMahPerKM(char *buff)
     if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) {
         if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
             value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
-                1, efficiencyTimeDelta * 1e-6f);
+                1, US2S(efficiencyTimeDelta));
 
             efficiencyUpdated = currentTimeUs;
         }
@@ -755,7 +755,7 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name)
     if (enabledElements[0] == 'W') {
         enabledElements += 1;
     }
-    
+
     int elemLen = strlen(enabledElements);
 
     if (elemLen > 0) {
@@ -825,14 +825,14 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name)
                     // during a lost aircraft recovery and blinking
                     // will cause it to be missing from some frames.
                 }
-            } 
+            }
             else {
                 if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
                     const char *navStateMessage = navigationStateMessage();
                     if (navStateMessage) {
                         messages[messageCount++] = navStateMessage;
                     }
-                } 
+                }
                 else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
                     messages[messageCount++] = "AUTOLAUNCH";
                 }
@@ -994,7 +994,7 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
             for (int i = 0; i < STICK_CHANNEL_COUNT; i++) {
                 sbufWriteU16(dst, rxGetChannelValue(i));
             }
-            break;            
+            break;
 
         case DJI_MSP_RAW_GPS:
             sbufWriteU8(dst, gpsSol.fixType);

src/main/io/smartport_master.c

@@ -550,7 +550,7 @@ void smartportMasterHandle(timeUs_t currentTimeUs)
         return;
     }
 
-    if (!pollTimestamp || (cmpTimeUs(currentTimeUs, pollTimestamp) > SMARTPORT_POLLING_INTERVAL * 1000)) {
+    if (!pollTimestamp || (cmpTimeUs(currentTimeUs, pollTimestamp) > MS2US(SMARTPORT_POLLING_INTERVAL))) {
         if (forwardRequestCount() && (forcedPolledPhyID == -1)) { // forward next payload if there is one in queue and we are not waiting from the response of the previous one
             smartportMasterForwardNextPayload();
         } else {

src/main/navigation/navigation_fixedwing.c

@@ -171,11 +171,11 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
 
     if ((posControl.flags.estAltStatus >= EST_USABLE)) {
         if (posControl.flags.verticalPositionDataNew) {
-            const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
+            const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
             previousTimePositionUpdate = currentTimeUs;
 
-            // Check if last correction was too log ago - ignore this update
-            if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
+            // Check if last correction was not too long ago
+            if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
                 updateAltitudeVelocityAndPitchController_FW(deltaMicrosPositionUpdate);
             }
             else {
@@ -401,10 +401,10 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs)
     if ((posControl.flags.estPosStatus >= EST_USABLE)) {
         // If we have new position - update velocity and acceleration controllers
         if (posControl.flags.horizontalPositionDataNew) {
-            const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
+            const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
             previousTimePositionUpdate = currentTimeUs;
 
-            if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
+            if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
                 // Calculate virtual position target at a distance of forwardVelocity * HZ2S(POSITION_TARGET_UPDATE_RATE_HZ)
                 // Account for pilot's roll input (move position target left/right at max of max_manual_speed)
                 // POSITION_TARGET_UPDATE_RATE_HZ should be chosen keeping in mind that position target shouldn't be reached until next pos update occurs
@@ -440,10 +440,10 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
     if ((posControl.flags.estPosStatus >= EST_USABLE)) {
         // If we have new position - update velocity and acceleration controllers
         if (posControl.flags.horizontalPositionDataNew) {
-            const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
+            const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
             previousTimePositionUpdate = currentTimeUs;
 
-            if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
+            if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
                 float velThrottleBoost = (NAV_FW_MIN_VEL_SPEED_BOOST - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate);
 
                 // If we are in the deadband of 50cm/s - don't update speed boost
@@ -473,7 +473,7 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
 int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs)
 {
     static timeUs_t previousTimePitchToThrCorr = 0;
-    const timeDelta_t deltaMicrosPitchToThrCorr = currentTimeUs -  previousTimePitchToThrCorr;
+    const timeDeltaLarge_t deltaMicrosPitchToThrCorr = currentTimeUs -  previousTimePitchToThrCorr;
     previousTimePitchToThrCorr = currentTimeUs;
 
     static pt1Filter_t pitchToThrFilterState;

src/main/navigation/navigation_multicopter.c

@@ -209,11 +209,11 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
 
     // If we have an update on vertical position data - update velocity and accel targets
     if (posControl.flags.verticalPositionDataNew) {
-        const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
+        const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
         previousTimePositionUpdate = currentTimeUs;
 
-        // Check if last correction was too log ago - ignore this update
-        if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
+        // Check if last correction was not too long ago
+        if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
             // If we are preparing for takeoff - start with lowset possible climb rate, adjust alt target and make sure throttle doesn't jump
             if (prepareForTakeoffOnReset) {
                 const navEstimatedPosVel_t * posToUse = navGetCurrentActualPositionAndVelocity();
@@ -477,8 +477,8 @@ static float computeNormalizedVelocity(const float value, const float maxValue)
 }
 
 static float computeVelocityScale(
-    const float value, 
-    const float maxValue, 
+    const float value,
+    const float maxValue,
     const float attenuationFactor,
     const float attenuationStart,
     const float attenuationEnd
@@ -491,7 +491,7 @@ static float computeVelocityScale(
 }
 
 static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit, const float maxSpeed)
-{    
+{
     const float measurementX = navGetCurrentActualPositionAndVelocity()->vel.x;
     const float measurementY = navGetCurrentActualPositionAndVelocity()->vel.y;
 
@@ -539,15 +539,15 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
      * Scale down dTerm with 2D speed
      */
     const float setpointScale = computeVelocityScale(
-        setpointXY, 
-        maxSpeed, 
+        setpointXY,
+        maxSpeed,
         multicopterPosXyCoefficients.dTermAttenuation,
         multicopterPosXyCoefficients.dTermAttenuationStart,
         multicopterPosXyCoefficients.dTermAttenuationEnd
     );
     const float measurementScale = computeVelocityScale(
-        posControl.actualState.velXY, 
-        maxSpeed, 
+        posControl.actualState.velXY,
+        maxSpeed,
         multicopterPosXyCoefficients.dTermAttenuation,
         multicopterPosXyCoefficients.dTermAttenuationStart,
         multicopterPosXyCoefficients.dTermAttenuationEnd
@@ -560,23 +560,23 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
     // Pre-calculated accelLimit and the logic of navPidApply2 function guarantee that our newAccel won't exceed maxAccelLimit
     // Thus we don't need to do anything else with calculated acceleration
     float newAccelX = navPidApply3(
-        &posControl.pids.vel[X], 
-        setpointX, 
-        measurementX, 
-        US2S(deltaMicros), 
-        accelLimitXMin, 
-        accelLimitXMax, 
+        &posControl.pids.vel[X],
+        setpointX,
+        measurementX,
+        US2S(deltaMicros),
+        accelLimitXMin,
+        accelLimitXMax,
         0,      // Flags
         1.0f,   // Total gain scale
         dtermScale    // Additional dTerm scale
     );
     float newAccelY = navPidApply3(
-        &posControl.pids.vel[Y], 
-        setpointY, 
-        measurementY, 
-        US2S(deltaMicros), 
-        accelLimitYMin, 
-        accelLimitYMax, 
+        &posControl.pids.vel[Y],
+        setpointY,
+        measurementY,
+        US2S(deltaMicros),
+        accelLimitYMin,
+        accelLimitYMax,
         0,      // Flags
         1.0f,   // Total gain scale
         dtermScale    // Additional dTerm scale
@@ -638,14 +638,14 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs)
     if ((posControl.flags.estPosStatus >= EST_USABLE)) {
         // If we have new position - update velocity and acceleration controllers
         if (posControl.flags.horizontalPositionDataNew) {
-            const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
+            const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
             previousTimePositionUpdate = currentTimeUs;
 
             if (!bypassPositionController) {
                 // Update position controller
-                if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
+                if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
                     // Get max speed from generic NAV (waypoint specific), don't allow to move slower than 0.5 m/s
-                    const float maxSpeed = getActiveWaypointSpeed(); 
+                    const float maxSpeed = getActiveWaypointSpeed();
                     updatePositionVelocityController_MC(maxSpeed);
                     updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed);
                 }
@@ -761,11 +761,11 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
 
     // Normal sensor data
     if (posControl.flags.verticalPositionDataNew) {
-        const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
+        const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
         previousTimePositionUpdate = currentTimeUs;
 
-        // Check if last correction was too log ago - ignore this update
-        if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
+        // Check if last correction was not too long ago
+        if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
             updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL);
             updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
             updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);

src/main/navigation/navigation_private.h

@@ -37,6 +37,9 @@
 
 #define INAV_SURFACE_MAX_DISTANCE           40
 
+#define MAX_POSITION_UPDATE_INTERVAL_US     HZ2US(MIN_POSITION_UPDATE_RATE_HZ)        // convenience macro
+_Static_assert(MAX_POSITION_UPDATE_INTERVAL_US <= TIMEDELTA_MAX, "deltaMicros can overflow!");
+
 typedef enum {
     NAV_POS_UPDATE_NONE                 = 0,
     NAV_POS_UPDATE_Z                    = 1 << 1,
@@ -197,7 +200,7 @@ typedef enum {
 
     NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME                        = 35,
     NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME                      = 36,
-    NAV_PERSISTENT_ID_WAYPOINT_HOVER_ABOVE_HOME                 = 37,  
+    NAV_PERSISTENT_ID_WAYPOINT_HOVER_ABOVE_HOME                 = 37,
 
 } navigationPersistentId_e;