Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 23:05:17 +03:00
Code Issues Wiki Activity

Fix int32_t microseconds overflow in navigation.

Browse source
This commit is contained in:
pieniacy 2021-04-06 21:04:35 +02:00
parent c26a676bb1
commit 8df415d544
15 changed files with 87 additions and 74 deletions
Download patch file Download diff file Expand all files Collapse all files

14
src/main/common/time.h
View file

@ -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 {

2
src/main/io/ledstrip.c
View file

@ -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;

10
src/main/io/osd.c
View file

@ -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
@ -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;

2
src/main/io/osd_dji_hd.c
View file

@ -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;
}

2
src/main/io/smartport_master.c
View file

@ -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 {

16
src/main/navigation/navigation_fixedwing.c
View file

@ -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;

16
src/main/navigation/navigation_multicopter.c
View file

@ -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();
@ -638,12 +638,12 @@ 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();
updatePositionVelocityController_MC(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);

3
src/main/navigation/navigation_private.h
View file

@ -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,

8
src/main/navigation/navigation_rover_boat.c
View file

@ -71,11 +71,11 @@ void applyRoverBoatPositionController(timeUs_t currentTimeUs)
static timeUs_t previousTimePositionUpdate; // Occurs @ GPS update rate
static timeUs_t previousTimeUpdate; // Occurs @ looptime rate
const timeDelta_t deltaMicros = currentTimeUs - previousTimeUpdate;
const timeDeltaLarge_t deltaMicros = currentTimeUs - previousTimeUpdate;
previousTimeUpdate = currentTimeUs;
// If last position update was too long in the past - ignore it (likely restarting altitude controller)
if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
if (deltaMicros > MAX_POSITION_UPDATE_INTERVAL_US) {
previousTimeUpdate = currentTimeUs;
previousTimePositionUpdate = currentTimeUs;
resetFixedWingPositionController();
@ -86,10 +86,10 @@ void applyRoverBoatPositionController(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) {
update2DPositionHeadingController(currentTimeUs, deltaMicrosPositionUpdate);
} else {
resetFixedWingPositionController();

4
src/main/rx/fport2.c
View file

@ -531,7 +531,7 @@ static uint8_t frameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
}
#endif
if (frameReceivedTimestamp && (cmpTimeUs(currentTimeUs, frameReceivedTimestamp) > FPORT2_MAX_TELEMETRY_AGE_MS * 1000)) {
if (frameReceivedTimestamp && (cmpTimeUs(currentTimeUs, frameReceivedTimestamp) > MS2US(FPORT2_MAX_TELEMETRY_AGE_MS))) {
lqTrackerSet(rxRuntimeConfig->lqTracker, 0);
frameReceivedTimestamp = 0;
}
@ -593,7 +593,7 @@ static bool processFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
}
timeUs_t otaResponseTime = cmpTimeUs(micros(), otaFrameEndTimestamp);
timeDelta_t otaResponseTime = cmpTimeUs(micros(), otaFrameEndTimestamp);
if (!firmwareUpdateError && (otaResponseTime <= otaMaxResponseTime)) { // We can answer in time (firmwareUpdateStore can take time because it might need to erase flash)
writeUplinkFramePhyID(downlinkPhyID, otaResponsePayload);
DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT2_OTA_FRAME_RESPONSE_TIME, otaResponseTime);

2
src/main/rx/ghst.c
View file

@ -178,7 +178,7 @@ STATIC_UNIT_TESTED void ghstDataReceive(uint16_t c, void *data)
static bool shouldSendTelemetryFrame(void)
{
const timeUs_t now = micros();
const timeUs_t timeSinceRxFrameEndUs = cmpTimeUs(now, ghstRxFrameEndAtUs);
const timeDelta_t timeSinceRxFrameEndUs = cmpTimeUs(now, ghstRxFrameEndAtUs);
return telemetryBufLen > 0 && timeSinceRxFrameEndUs > GHST_RX_TO_TELEMETRY_MIN_US && timeSinceRxFrameEndUs < GHST_RX_TO_TELEMETRY_MAX_US;
}

2
src/main/rx/sumd.c
View file

@ -79,7 +79,7 @@ static void sumdDataReceive(uint16_t c, void *rxCallbackData)
static uint8_t sumdIndex;
sumdTime = micros();
if (cmpTimeUs(sumdTime, sumdTimeLast) > 4000)
if (cmpTimeUs(sumdTime, sumdTimeLast) > MS2US(4))
sumdIndex = 0;
sumdTimeLast = sumdTime;

2
src/main/rx/sumh.c
View file

@ -69,7 +69,7 @@ static void sumhDataReceive(uint16_t c, void *rxCallbackData)
sumhTime = micros();
sumhTimeInterval = cmpTimeUs(sumhTime, sumhTimeLast);
sumhTimeLast = sumhTime;
if (sumhTimeInterval > 5000) {
if (sumhTimeInterval > MS2US(5)) {
sumhFramePosition = 0;
}

6
src/main/sensors/battery.c
View file

@ -558,7 +558,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta)
} else {
if (cmpTimeUs(currentTime, recordTimestamp) > 500000)
if (cmpTimeUs(currentTime, recordTimestamp) > MS2US(500))
recordTimestamp = 0;
if (!recordTimestamp) {
@ -575,7 +575,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta)
if (impedanceFilterState.state) {
pt1FilterSetTimeConstant(&impedanceFilterState, impedanceSampleCount > IMPEDANCE_STABLE_SAMPLE_COUNT_THRESH ? 1.2 : 0.5);
pt1FilterApply3(&impedanceFilterState, impedanceSample, timeDelta * 1e-6f);
pt1FilterApply3(&impedanceFilterState, impedanceSample, US2S(timeDelta));
} else {
pt1FilterReset(&impedanceFilterState, impedanceSample);
}
@ -589,7 +589,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta)
uint16_t sagCompensatedVBatSample = MIN(batteryFullVoltage, vbat + (int32_t)powerSupplyImpedance * amperage / 1000);
pt1FilterSetTimeConstant(&sagCompVBatFilterState, sagCompensatedVBatSample < pt1FilterGetLastOutput(&sagCompVBatFilterState) ? 40 : 500);
sagCompensatedVBat = lrintf(pt1FilterApply3(&sagCompVBatFilterState, sagCompensatedVBatSample, timeDelta * 1e-6f));
sagCompensatedVBat = lrintf(pt1FilterApply3(&sagCompVBatFilterState, sagCompensatedVBatSample, US2S(timeDelta)));
}
DEBUG_SET(DEBUG_SAG_COMP_VOLTAGE, 0, powerSupplyImpedance);

2
src/main/sensors/rangefinder.c
View file

@ -217,7 +217,7 @@ timeDelta_t rangefinderUpdate(void)
rangefinder.dev.update(&rangefinder.dev);
}
return rangefinder.dev.delayMs * 1000; // to microseconds
return MS2US(rangefinder.dev.delayMs);
}
/**