Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 00:05:28 +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;

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

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

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

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

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;

56
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();
@ -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);

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

10
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();
@ -143,4 +143,4 @@ void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags,
}
}
#endif
#endif

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

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

@ -119,7 +119,7 @@ void ghstRxWriteTelemetryData(const void *data, int len)
}
void ghstRxSendTelemetryData(void)
{
{
// if there is telemetry data to write
if (telemetryBufLen > 0) {
serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
@ -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;
}
@ -191,7 +191,7 @@ static void ghstIdle(void)
static void ghstUpdateFailsafe(unsigned pktIdx)
{
// pktIdx is an offset of RC channel packet,
// pktIdx is an offset of RC channel packet,
// We'll track arrival time of each of the frame types we ever saw arriving from this receiver
if (pktIdx < GHST_UL_RC_CHANS_FRAME_COUNT) {
if (ghstFsTracker[pktIdx].onTimePacketCounter < GHST_RC_FRAME_COUNT_THRESHOLD) {
@ -282,7 +282,7 @@ static bool ghstProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
int startIdx = 0;
if (
ghstValidatedFrame.frame.type >= GHST_UL_RC_CHANS_HS4_FIRST &&
ghstValidatedFrame.frame.type >= GHST_UL_RC_CHANS_HS4_FIRST &&
ghstValidatedFrame.frame.type <= GHST_UL_RC_CHANS_HS4_LAST
) {
const ghstPayloadPulses_t* const rcChannels = (ghstPayloadPulses_t*)&ghstValidatedFrame.frame.payload;
@ -300,7 +300,7 @@ static bool ghstProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
case GHST_UL_RC_CHANS_HS4_RSSI: {
const ghstPayloadPulsesRSSI_t* const rssiFrame = (ghstPayloadPulsesRSSI_t*)&ghstValidatedFrame.frame.payload;
lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(constrain(rssiFrame->lq, 0, 100), 0, 100, 0, RSSI_MAX_VALUE));
break;
}

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);
}
/**