Unverified Commit 1fe93894 authored by Paweł Spychalski's avatar Paweł Spychalski Committed by GitHub
Browse files

Merge pull request #6806 from pieniacy/pieniacy-fix-nav-us-overflow

Fix int32_t microseconds overflow in navigation.
parents 7ed71121 8df415d5
......@@ -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 {
......
......@@ -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;
......
......@@ -1644,7 +1644,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
......@@ -1675,7 +1675,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
......@@ -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)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
1, efficiencyTimeDelta * 1e-6f);
1, US2S(efficiencyTimeDelta));
efficiencyUpdated = currentTimeUs;
} else {
......@@ -2292,7 +2292,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 {
......@@ -3332,7 +3332,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;
......
......@@ -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
*
......@@ -687,7 +687,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)),
......@@ -724,7 +724,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;
}
......@@ -757,7 +757,7 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name)
if (enabledElements[0] == 'W') {
enabledElements += 1;
}
int elemLen = strlen(enabledElements);
if (elemLen > 0) {
......@@ -827,14 +827,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";
}
......@@ -996,7 +996,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);
......
......@@ -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 {
......
......@@ -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;
......
......@@ -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);
......
......@@ -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;
......
......@@ -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
\ No newline at end of file
#endif
......@@ -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);
......
......@@ -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;
}
......
......@@ -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;
......
......@@ -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;
}
......
......@@ -570,7 +570,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) {
......@@ -587,7 +587,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);
}
......@@ -601,7 +601,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);
......
......@@ -227,7 +227,7 @@ timeDelta_t rangefinderUpdate(void)
rangefinder.dev.update(&rangefinder.dev);
}
return rangefinder.dev.delayMs * 1000; // to microseconds
return MS2US(rangefinder.dev.delayMs);
}
/**
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment