diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 5830f77b51..8d6b5c94ad 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -6162,8 +6162,8 @@ static void cliDshotTelemetryInfo(const char *cmdName, char *cmdline) if (useDshotTelemetry) { cliPrintLinef("Dshot reads: %u", dshotTelemetryState.readCount); cliPrintLinef("Dshot invalid pkts: %u", dshotTelemetryState.invalidPacketCount); - uint32_t directionChangeCycles = dshotDMAHandlerCycleCounters.changeDirectionCompletedAt - dshotDMAHandlerCycleCounters.irqAt; - uint32_t directionChangeDurationUs = clockCyclesToMicros(directionChangeCycles); + int32_t directionChangeCycles = cmp32(dshotDMAHandlerCycleCounters.changeDirectionCompletedAt, dshotDMAHandlerCycleCounters.irqAt); + int32_t directionChangeDurationUs = clockCyclesToMicros(directionChangeCycles); cliPrintLinef("Dshot directionChange cycles: %u, micros: %u", directionChangeCycles, directionChangeDurationUs); cliPrintLinefeed(); diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index f29649e898..12382f5b56 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -153,7 +153,7 @@ uint32_t getCycleCounter(void) return DWT->CYCCNT; } -uint32_t clockCyclesToMicros(uint32_t clockCycles) +int32_t clockCyclesToMicros(int32_t clockCycles) { return clockCycles / usTicks; } diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 5ff1683ecb..2760928384 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -65,7 +65,7 @@ void systemReset(void); void systemResetToBootloader(bootloaderRequestType_e requestType); bool isMPUSoftReset(void); void cycleCounterInit(void); -uint32_t clockCyclesToMicros(uint32_t clockCycles); +int32_t clockCyclesToMicros(int32_t clockCycles); int32_t clockCyclesTo10thMicros(int32_t clockCycles); uint32_t clockMicrosToCycles(uint32_t micros); uint32_t getCycleCounter(void); diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 61f23c635f..4eb9957ced 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -744,12 +744,6 @@ bool isAirmodeActivated() */ bool processRx(timeUs_t currentTimeUs) { - timeDelta_t frameAgeUs; - timeDelta_t frameDeltaUs = rxGetFrameDelta(&frameAgeUs); - - DEBUG_SET(DEBUG_RX_TIMING, 0, MIN(frameDeltaUs / 10, INT16_MAX)); - DEBUG_SET(DEBUG_RX_TIMING, 1, MIN(frameAgeUs / 10, INT16_MAX)); - if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) { return false; } diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 7600adff40..6cbaed5191 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -298,13 +298,18 @@ void updateRcRefreshRate(timeUs_t currentTimeUs) static timeUs_t lastRxTimeUs; timeDelta_t frameAgeUs; - timeDelta_t refreshRateUs = rxGetFrameDelta(&frameAgeUs); - if (!refreshRateUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) { - refreshRateUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol + timeDelta_t frameDeltaUs = rxGetFrameDelta(&frameAgeUs); + + if (!frameDeltaUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) { + frameDeltaUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol } + + DEBUG_SET(DEBUG_RX_TIMING, 0, MIN(frameDeltaUs / 10, INT16_MAX)); + DEBUG_SET(DEBUG_RX_TIMING, 1, MIN(frameAgeUs / 10, INT16_MAX)); + lastRxTimeUs = currentTimeUs; - isRxRateValid = (refreshRateUs >= RC_RX_RATE_MIN_US && refreshRateUs <= RC_RX_RATE_MAX_US); - currentRxRefreshRate = constrain(refreshRateUs, RC_RX_RATE_MIN_US, RC_RX_RATE_MAX_US); + isRxRateValid = (frameDeltaUs >= RC_RX_RATE_MIN_US && frameDeltaUs <= RC_RX_RATE_MAX_US); + currentRxRefreshRate = constrain(frameDeltaUs, RC_RX_RATE_MIN_US, RC_RX_RATE_MAX_US); } uint16_t getCurrentRxRefreshRate(void) diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 0b3a12156f..1698dabebf 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -65,11 +65,11 @@ PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CO PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, .failsafe_throttle = 1000, // default throttle off. .failsafe_throttle_low_delay = 100, // default throttle low delay for "just disarm" on failsafe condition - .failsafe_delay = 4, // 0,4sec - .failsafe_off_delay = 10, // 1sec - .failsafe_switch_mode = 0, // default failsafe switch action is identical to rc link loss + .failsafe_delay = 10, // 1 sec, can regain control on signal recovery, at idle in drop mode + .failsafe_off_delay = 10, // 1 sec in landing phase, if enabled + .failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE1, // default failsafe switch action is identical to rc link loss .failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT,// default full failsafe procedure is 0: auto-landing - .failsafe_recovery_delay = 20, // 2 sec of valid rx data (plus 200ms) needed to allow recovering from failsafe procedure + .failsafe_recovery_delay = 10, // 1 sec of valid rx data needed to allow recovering from failsafe procedure .failsafe_stick_threshold = 30 // 30 percent of stick deflection to exit GPS Rescue procedure ); @@ -86,8 +86,12 @@ const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = { */ void failsafeReset(void) { - failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND; - failsafeState.rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY + failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND; + failsafeState.rxDataFailurePeriod = failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND; // time to start stage2 + failsafeState.rxDataRecoveryPeriod = failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND; + if (failsafeState.rxDataRecoveryPeriod < PERIOD_RXDATA_RECOVERY){ + // PERIOD_RXDATA_RECOVERY (200ms) is the minimum allowed RxData recovery time + failsafeState.rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY; + } failsafeState.validRxDataReceivedAt = 0; failsafeState.validRxDataFailedAt = 0; failsafeState.throttleLowPeriod = 0; @@ -138,26 +142,12 @@ static void failsafeActivate(void) failsafeState.phase = FAILSAFE_LANDING; ENABLE_FLIGHT_MODE(FAILSAFE_MODE); + failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND; failsafeState.events++; } -static void failsafeApplyControlInput(void) -{ -#ifdef USE_GPS_RESCUE - if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE) { - ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE); - return; - } -#endif - - for (int i = 0; i < 3; i++) { - rcData[i] = rxConfig()->midrc; - } - rcData[THROTTLE] = failsafeConfig()->failsafe_throttle; -} - bool failsafeIsReceivingRxData(void) { return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP); @@ -177,42 +167,61 @@ void failsafeOnRxResume(void) void failsafeOnValidDataReceived(void) { failsafeState.validRxDataReceivedAt = millis(); - if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > failsafeState.rxDataRecoveryPeriod) { + if ((cmp32(failsafeState.validRxDataReceivedAt, failsafeState.validRxDataFailedAt) > (int32_t)failsafeState.rxDataRecoveryPeriod) || + (cmp32(failsafeState.validRxDataFailedAt, failsafeState.validRxDataReceivedAt) > 0)) { + // rxDataRecoveryPeriod defaults to 1.0s with minimum of PERIOD_RXDATA_RECOVERY (200ms) failsafeState.rxLinkState = FAILSAFE_RXLINK_UP; + // Allow arming now we have an RX link unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); } } void failsafeOnValidDataFailed(void) { - setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); // To prevent arming with no RX link failsafeState.validRxDataFailedAt = millis(); - if ((failsafeState.validRxDataFailedAt - failsafeState.validRxDataReceivedAt) > failsafeState.rxDataFailurePeriod) { + if ((cmp32(failsafeState.validRxDataFailedAt, failsafeState.validRxDataReceivedAt) > (int32_t)failsafeState.rxDataFailurePeriod)) { + // rxDataFailurePeriod is stage 1 guard time failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN; + // Prevent arming with no RX link + setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); } } void failsafeCheckDataFailurePeriod(void) +// forces link down directly from scheduler? { if (cmp32(millis(), failsafeState.validRxDataReceivedAt) > (int32_t)failsafeState.rxDataFailurePeriod) { - setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); // To prevent arming with no RX link failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN; + // Prevent arming with no RX link + setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); } } +uint32_t failsafeFailurePeriodMs(void) +{ + return failsafeState.rxDataFailurePeriod; +} + FAST_CODE_NOINLINE void failsafeUpdateState(void) +// triggered directly, and ONLY, by the cheduler, at 10ms = PERIOD_RXDATA_FAILURE - intervals { if (!failsafeIsMonitoring()) { return; } bool receivingRxData = failsafeIsReceivingRxData(); + // should be true when FAILSAFE_RXLINK_UP + // FAILSAFE_RXLINK_UP is set in failsafeOnValidDataReceived + // failsafeOnValidDataReceived runs from detectAndApplySignalLossBehaviour + bool armed = ARMING_FLAG(ARMED); bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE); beeperMode_e beeperMode = BEEPER_SILENCE; - if (failsafeSwitchIsOn && failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2) { - receivingRxData = false; // force Stage2 + if (failsafeSwitchIsOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2)) { + // Aux switch set to failsafe stage2 emulates loss of signal without waiting + failsafeOnValidDataFailed(); + receivingRxData = false; } // Beep RX lost only if we are not seeing data and we have been armed earlier @@ -232,12 +241,13 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) if (THROTTLE_HIGH == calculateThrottleStatus()) { failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; } - // Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming) - if (failsafeSwitchIsOn && failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL) { - // KillswitchEvent: failsafe switch is configured as KILL switch and is switched ON + if (failsafeSwitchIsOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL)) { + // Failsafe switch is configured as KILL switch and is switched ON failsafeActivate(); - failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure - failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_1_SECONDS; // require 1 seconds of valid rxData + // Skip auto-landing procedure + failsafeState.phase = FAILSAFE_LANDED; + // Require 3 seconds of valid rxData + failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; reprocessState = true; } else if (!receivingRxData) { if (millis() > failsafeState.throttleLowPeriod @@ -279,7 +289,8 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) case FAILSAFE_PROCEDURE_DROP_IT: // Drop the craft failsafeActivate(); - failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure + // skip auto-landing procedure + failsafeState.phase = FAILSAFE_LANDED; break; #ifdef USE_GPS_RESCUE case FAILSAFE_PROCEDURE_GPS_RESCUE: @@ -296,15 +307,16 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) if (receivingRxData) { failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; reprocessState = true; - } - if (armed) { - failsafeApplyControlInput(); - beeperMode = BEEPER_RX_LOST_LANDING; - } - if (failsafeShouldHaveCausedLandingByNow() || crashRecoveryModeActive() || !armed) { - failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData - failsafeState.phase = FAILSAFE_LANDED; - reprocessState = true; + } else { + if (armed) { + beeperMode = BEEPER_RX_LOST_LANDING; + } + if (failsafeShouldHaveCausedLandingByNow() || crashRecoveryModeActive() || !armed) { + // require 3 seconds of valid rxData + failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; + failsafeState.phase = FAILSAFE_LANDED; + reprocessState = true; + } } break; #ifdef USE_GPS_RESCUE @@ -314,19 +326,22 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; reprocessState = true; } - } - if (armed) { - failsafeApplyControlInput(); - beeperMode = BEEPER_RX_LOST_LANDING; } else { - failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData - failsafeState.phase = FAILSAFE_LANDED; - reprocessState = true; + if (armed) { + ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE); + beeperMode = BEEPER_RX_LOST_LANDING; + } else { + // require 3 seconds of valid rxData + failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; + failsafeState.phase = FAILSAFE_LANDED; + reprocessState = true; + } } break; #endif case FAILSAFE_LANDED: - setArmingDisabled(ARMING_DISABLED_FAILSAFE); // To prevent accidently rearming by an intermittent rx link + // Prevent accidently rearming by an intermittent rx link + setArmingDisabled(ARMING_DISABLED_FAILSAFE); disarm(DISARM_REASON_FAILSAFE); failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING; @@ -337,12 +352,9 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) // Monitoring the rx link to allow rearming when it has become good for > `receivingRxDataPeriodPreset` time. if (receivingRxData) { if (millis() > failsafeState.receivingRxDataPeriod) { - // rx link is good now, when arming via ARM switch, it must be OFF first - if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) { - unsetArmingDisabled(ARMING_DISABLED_FAILSAFE); - failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; - reprocessState = true; - } + // rx link is good now + failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; + reprocessState = true; } } else { failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; @@ -357,6 +369,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) failsafeState.phase = FAILSAFE_IDLE; failsafeState.active = false; DISABLE_FLIGHT_MODE(FAILSAFE_MODE); + unsetArmingDisabled(ARMING_DISABLED_FAILSAFE); reprocessState = true; break; diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 4b28ae81c6..31c5b01b7e 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -28,18 +28,17 @@ #define PERIOD_OF_1_SECONDS 1 * MILLIS_PER_SECOND #define PERIOD_OF_3_SECONDS 3 * MILLIS_PER_SECOND #define PERIOD_OF_30_SECONDS 30 * MILLIS_PER_SECOND -#define PERIOD_RXDATA_FAILURE 200 // millis +#define PERIOD_RXDATA_FAILURE 10 // millis #define PERIOD_RXDATA_RECOVERY 200 // millis - typedef struct failsafeConfig_s { uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500. uint16_t failsafe_throttle_low_delay; // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure". uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10) uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200) - uint8_t failsafe_switch_mode; // failsafe switch action is 0: stage1 (identical to rc link loss), 1: disarms instantly, 2: stage2 + uint8_t failsafe_switch_mode; // failsafe switch action is 0: Stage 1, 1: Disarms instantly, 2: Stage 2 uint8_t failsafe_procedure; // selected full failsafe procedure is 0: auto-landing, 1: Drop it - uint16_t failsafe_recovery_delay; // Time (in 0.1sec) of valid rx data (plus 200ms) needed to allow recovering from failsafe procedure + uint16_t failsafe_recovery_delay; // Time (in 0.1sec) of valid rx data (min 200ms PERIOD_RXDATA_RECOVERY) to allow recovering from failsafe procedure uint8_t failsafe_stick_threshold; // Stick deflection percentage to exit GPS Rescue procedure } failsafeConfig_t; @@ -106,6 +105,5 @@ bool failsafeIsReceivingRxData(void); void failsafeCheckDataFailurePeriod(void); void failsafeOnRxSuspend(uint32_t suspendPeriod); void failsafeOnRxResume(void); - void failsafeOnValidDataReceived(void); void failsafeOnValidDataFailed(void); diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c index 5b696fbe09..b5a2203e51 100644 --- a/src/main/osd/osd.c +++ b/src/main/osd/osd.c @@ -67,6 +67,7 @@ #if defined(USE_DYN_NOTCH_FILTER) #include "flight/dyn_notch_filter.h" #endif +#include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/position.h" @@ -1037,6 +1038,7 @@ STATIC_UNIT_TESTED bool osdProcessStats1(timeUs_t currentTimeUs) resumeRefreshAt = osdShowArmed() + currentTimeUs; } else if (isSomeStatEnabled() && !suppressStatsDisplay + && !failsafeIsActive() && (!(getArmingDisableFlags() & (ARMING_DISABLED_RUNAWAY_TAKEOFF | ARMING_DISABLED_CRASH_DETECTED)) || !VISIBLE(osdElementConfig()->item_pos[OSD_WARNINGS]))) { // suppress stats if runaway takeoff triggered disarm and WARNINGS element is visible osdStatsEnabled = true; diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 6e299c1a2b..ac3a71bcf3 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -43,6 +43,7 @@ #include "fc/rc_controls.h" #include "fc/rc_modes.h" +#include "fc/runtime_config.h" #include "fc/tasks.h" #include "flight/failsafe.h" @@ -105,17 +106,15 @@ static bool rxFlightChannelsValid = false; static bool rxIsInFailsafeMode = true; static uint8_t rxChannelCount; -static timeUs_t rxNextUpdateAtUs = 0; -static uint32_t needRxSignalBefore = 0; -static uint32_t needRxSignalMaxDelayUs; -static uint32_t suspendRxSignalUntil = 0; +static timeUs_t needRxSignalBefore = 0; +static timeUs_t suspendRxSignalUntil = 0; static uint8_t skipRxSamples = 0; static float rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] -uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT]; +uint32_t validRxSignalTimeout[MAX_SUPPORTED_RC_CHANNEL_COUNT]; -#define MAX_INVALID_PULS_TIME 300 +#define MAX_INVALID_PULSE_TIME 300 // hold time in millisecons after bad channel or Rx link loss #define PPM_AND_PWM_SAMPLE_COUNT 3 #define DELAY_50_HZ (1000000 / 50) @@ -284,11 +283,10 @@ void rxInit(void) rxRuntimeState.rcProcessFrameFn = nullProcessFrame; rxRuntimeState.lastRcFrameTimeUs = 0; rcSampleIndex = 0; - needRxSignalMaxDelayUs = DELAY_10_HZ; for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { rcData[i] = rxConfig()->midrc; - rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME; + validRxSignalTimeout[i] = millis() + MAX_INVALID_PULSE_TIME; } rcData[THROTTLE] = (featureIsEnabled(FEATURE_3D)) ? rxConfig()->midrc : rxConfig()->rx_min_usec; @@ -330,7 +328,6 @@ void rxInit(void) #ifdef USE_RX_MSP case RX_PROVIDER_MSP: rxMspInit(rxConfig(), &rxRuntimeState); - needRxSignalMaxDelayUs = DELAY_5_HZ; break; #endif @@ -473,13 +470,17 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs) UNUSED(currentTimeUs); UNUSED(currentDeltaTimeUs); - return taskUpdateRxMainInProgress() || rxDataProcessingRequired || auxiliaryProcessingRequired || !failsafeIsReceivingRxData(); + return taskUpdateRxMainInProgress() || rxDataProcessingRequired || auxiliaryProcessingRequired; } -FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs) +FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs, timeDelta_t anticipatedDeltaTime10thUs) { bool signalReceived = false; bool useDataDrivenProcessing = true; + // Need the next packet in 2.5 x the anticipated time + timeDelta_t needRxSignalMaxDelayUs = anticipatedDeltaTime10thUs * 2 / 8; + + DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 2, needRxSignalMaxDelayUs / 10); if (taskUpdateRxMainInProgress()) { // No need to check for new data as a packet is being processed already @@ -495,7 +496,6 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current if (isPPMDataBeingReceived()) { signalReceived = true; rxIsInFailsafeMode = false; - needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs; resetPPMDataReceivedState(); } @@ -504,7 +504,6 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current if (isPWMDataBeingReceived()) { signalReceived = true; rxIsInFailsafeMode = false; - needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs; useDataDrivenProcessing = false; } @@ -519,13 +518,8 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0; bool rxFrameDropped = (frameStatus & RX_FRAME_DROPPED) != 0; signalReceived = !(rxIsInFailsafeMode || rxFrameDropped); - if (signalReceived) { - needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs; - } - setLinkQuality(signalReceived, currentDeltaTimeUs); } - if (frameStatus & RX_FRAME_PROCESSING_REQUIRED) { auxiliaryProcessingRequired = true; } @@ -535,12 +529,19 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current } if (signalReceived) { - rxSignalReceived = true; - } else if (currentTimeUs >= needRxSignalBefore) { - rxSignalReceived = false; + needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs; + rxSignalReceived = true; // immediately process data + } else { + // no signal, wait 100ms and perform signal loss behaviour every 100ms + if (cmpTimeUs(needRxSignalBefore, currentTimeUs) > (timeDelta_t)DELAY_10_HZ) { + rxSignalReceived = false; + // rcData[] needs to be processed to rcCommand + rxDataProcessingRequired = true; + needRxSignalBefore = currentTimeUs + (timeDelta_t)DELAY_10_HZ; + } } - if ((signalReceived && useDataDrivenProcessing) || cmpTimeUs(currentTimeUs, rxNextUpdateAtUs) > 0) { + if (signalReceived && useDataDrivenProcessing) { rxDataProcessingRequired = true; } } @@ -645,50 +646,83 @@ static void readRxChannelsApplyRanges(void) static void detectAndApplySignalLossBehaviour(void) { const uint32_t currentTimeMs = millis(); - - const bool useValueFromRx = rxSignalReceived && !rxIsInFailsafeMode; + const bool failsafeAuxSwitch = IS_RC_MODE_ACTIVE(BOXFAILSAFE); + bool validRxPacket = rxSignalReceived && !failsafeAuxSwitch; + // becomes false when a packet is bad or we use a failsafe switch - logged to blackbox + rxFlightChannelsValid = true; + // becomes false when one or more flight channels has bad Rx data for longer than hold time DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 0, rxSignalReceived); DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 1, rxIsInFailsafeMode); - rxFlightChannelsValid = true; for (int channel = 0; channel < rxChannelCount; channel++) { float sample = rcRaw[channel]; + const bool thisChannelValid = validRxPacket && isPulseValid(sample); // for all or just one channel alone - const bool validPulse = useValueFromRx && isPulseValid(sample); - - if (validPulse) { - rcInvalidPulsPeriod[channel] = currentTimeMs + MAX_INVALID_PULS_TIME; + if (thisChannelValid) { + // reset invalid pulse period timer for each good-channel + validRxSignalTimeout[channel] = currentTimeMs + MAX_INVALID_PULSE_TIME; + } + // set failsafe and hold values + if (failsafeIsActive() && ARMING_FLAG(ARMED)) { + // STAGE 2 failsafe is active, and armed. Apply failsafe values until failsafe ends or disarmed + if (channel < NON_AUX_CHANNEL_COUNT) { + if (channel == THROTTLE ) { + sample = failsafeConfig()->failsafe_throttle; + } else { + sample = rxConfig()->midrc; + } + } else if (!failsafeAuxSwitch) { + // Aux channels as Set in Configurator, unless failsafe initiated by switch + sample = getRxfailValue(channel); + } } else { - if (cmp32(currentTimeMs, rcInvalidPulsPeriod[channel]) < 0) { - continue; // skip to next channel to hold channel value MAX_INVALID_PULS_TIME - } else { - sample = getRxfailValue(channel); // after that apply rxfail value - if (channel < NON_AUX_CHANNEL_COUNT) { - rxFlightChannelsValid = false; + if (!thisChannelValid) { + if (cmp32(currentTimeMs, validRxSignalTimeout[channel]) < 0) { + // HOLD PERIOD for any invalid channels + } else { + // STAGE 1 failsafe settings apply to any channels invalid for more than hold time + if (channel < NON_AUX_CHANNEL_COUNT) { + rxFlightChannelsValid = false; + sample = getRxfailValue(channel); + // affected RPYT values will be set as per Stage 1 Configurator settings + } else if (!failsafeAuxSwitch) { + // Aux channels as Set in Configurator, unless failsafe initiated by switch + sample = getRxfailValue(channel); + } } } } + #if defined(USE_PWM) || defined(USE_PPM) if (rxRuntimeState.rxProvider == RX_PROVIDER_PARALLEL_PWM || rxRuntimeState.rxProvider == RX_PROVIDER_PPM) { - // smooth output for PWM and PPM + // smooth output for PWM and PPM using moving average rcData[channel] = calculateChannelMovingAverage(channel, sample); } else #endif { + // set rcData to either clean raw incoming values, or failsafe values rcData[channel] = sample; } } - if (rxFlightChannelsValid && !IS_RC_MODE_ACTIVE(BOXFAILSAFE)) { + if (!rxFlightChannelsValid) { + // show RXLOSS in OSD if any RPYT channel, or any packets, are lost for more than hold time + setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); + } + + if (validRxPacket && rxFlightChannelsValid) { + // failsafe switches are off, packet is good, no invalid channel for more than hold time + // --> start the timer to exit stage 2 failsafe + unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); failsafeOnValidDataReceived(); } else { - rxIsInFailsafeMode = true; + // failsafe switch is on, or a bad packet, or at least one invalid channel for more than hold time + // -> start timer to enter stage2 failsafe + // note stage 2 onset will be delayed by hold time if we only have some bad channels in otherwise good packets failsafeOnValidDataFailed(); - for (int channel = 0; channel < rxChannelCount; channel++) { - rcData[channel] = getRxfailValue(channel); - } } + DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 3, rcData[THROTTLE]); } @@ -703,7 +737,6 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) } rxDataProcessingRequired = false; - rxNextUpdateAtUs = currentTimeUs + DELAY_15_HZ; // only proceed when no more samples to skip and suspend period is over if (skipRxSamples || currentTimeUs <= suspendRxSignalUntil) { diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 643b1a09ca..2939d3accb 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -177,7 +177,7 @@ extern rxRuntimeState_t rxRuntimeState; //!!TODO remove this extern, only needed void rxInit(void); void rxProcessPending(bool state); bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs); -void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs); +void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs, timeDelta_t anticipatedDeltaTime10thUs); bool rxIsReceivingSignal(void); bool rxAreFlightChannelsValid(void); bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs); diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index d14b79200c..35aef21060 100644 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -494,7 +494,7 @@ FAST_CODE void scheduler(void) } DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 0, clockCyclesTo10thMicros(cmpTimeCycles(nowCycles, lastTargetCycles))); #endif - currentTimeUs = clockCyclesToMicros(nowCycles); + currentTimeUs = micros(); taskExecutionTimeUs += schedulerExecuteTask(gyroTask, currentTimeUs); if (gyroFilterReady()) { @@ -504,15 +504,15 @@ FAST_CODE void scheduler(void) taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_PID), currentTimeUs); } if (rxFrameReady()) { - // Check to for incoming RX data. Don't do this in the checker as that is called repeatedly within + // Check for incoming RX data. Don't do this in the checker as that is called repeatedly within // a given gyro loop, and ELRS takes a long time to process this and so can only be safely processed // before the checkers - rxFrameCheck(currentTimeUs, cmpTimeUs(currentTimeUs, getTask(TASK_RX)->lastExecutedAtUs)); + rxFrameCheck(currentTimeUs, cmpTimeUs(currentTimeUs, getTask(TASK_RX)->lastExecutedAtUs), getTask(TASK_RX)->movingSumDeltaTime10thUs / TASK_STATS_MOVING_SUM_COUNT); } // Check for failsafe conditions without reliance on the RX task being well behaved if (cmp32(millis(), lastFailsafeCheckMs) > PERIOD_RXDATA_FAILURE) { - // This is very low cost taking less that 4us every 200ms + // This is very low cost taking less that 4us every 10ms failsafeCheckDataFailurePeriod(); failsafeUpdateState(); lastFailsafeCheckMs = millis(); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index a33cfd72eb..a9cb63db73 100644 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -28,7 +28,7 @@ #define TASK_PERIOD_MS(ms) ((ms) * 1000) #define TASK_PERIOD_US(us) (us) -#define TASK_STATS_MOVING_SUM_COUNT 64 +#define TASK_STATS_MOVING_SUM_COUNT 8 #define LOAD_PERCENTAGE_ONE 100 diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index c388f84bbb..32742dfc44 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -334,7 +334,7 @@ uint32_t millis(void) { return millis64() & 0xFFFFFFFF; } -uint32_t clockCyclesToMicros(uint32_t clockCycles) +int32_t clockCyclesToMicros(int32_t clockCycles) { return clockCycles; } diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index c5b0069617..7287b00ce5 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -183,7 +183,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndStartsLanding) failsafeOnValidDataReceived(); // set last valid sample at current time // when - for (sysTickUptime = 0; sysTickUptime < (uint32_t)(PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND); sysTickUptime++) { + for (sysTickUptime = 0; sysTickUptime < (uint32_t)(failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND); sysTickUptime++) { failsafeOnValidDataFailed(); failsafeUpdateState(); @@ -237,7 +237,7 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding) EXPECT_TRUE(isArmingDisabled()); // given - sysTickUptime += PERIOD_OF_30_SECONDS + 1; // adjust time to point just past the required additional recovery time + sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time failsafeOnValidDataReceived(); // when @@ -361,7 +361,7 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeKill) EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); // given - sysTickUptime += PERIOD_OF_1_SECONDS + 1; // adjust time to point just past the required additional recovery time + sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time failsafeOnValidDataReceived(); // when @@ -435,8 +435,7 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Land) failsafeUpdateState(); // should activate stage2 immediately // then - EXPECT_TRUE(failsafeIsActive()); - EXPECT_TRUE(isArmingDisabled()); + EXPECT_TRUE(failsafeIsActive()); // stick induced failsafe allows re-arming EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM)); EXPECT_EQ(FAILSAFE_LANDING, failsafePhase()); @@ -456,7 +455,7 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Land) EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); // given - sysTickUptime += PERIOD_OF_30_SECONDS + 1; // adjust time to point just past the required additional recovery time + sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time // and deactivateBoxFailsafe(); @@ -497,7 +496,7 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected sysTickUptime = 0; // restart time from 0 failsafeOnValidDataReceived(); // set last valid sample at current time - // when + // enter stage 1 failsafe for (sysTickUptime = 0; sysTickUptime < PERIOD_RXDATA_FAILURE; sysTickUptime++) { failsafeOnValidDataFailed(); @@ -520,10 +519,20 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected EXPECT_FALSE(failsafeIsActive()); EXPECT_EQ(FAILSAFE_IDLE, failsafePhase()); EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM)); - EXPECT_TRUE(isArmingDisabled()); + EXPECT_FALSE(isArmingDisabled()); // arming not blocked in stage 1 - // given - // enough valid data is received + // add enough time to enter stage 2 + sysTickUptime += failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND + 1; + failsafeOnValidDataFailed(); + + // when + failsafeUpdateState(); + + // then + EXPECT_TRUE(isArmingDisabled()); // arming blocked until recovery time + EXPECT_FALSE(failsafeIsActive()); // failsafe is still not active + + // given valid data is received for the recovery time, allow re-arming uint32_t sysTickTarget = sysTickUptime + PERIOD_RXDATA_RECOVERY; for (; sysTickUptime < sysTickTarget; sysTickUptime++) { failsafeOnValidDataReceived(); @@ -546,6 +555,7 @@ extern "C" { float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; float rcCommand[4]; int16_t debug[DEBUG16_VALUE_COUNT]; +uint8_t debugMode = 0; bool isUsingSticksToArm = true; PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0); diff --git a/src/test/unit/link_quality_unittest.cc b/src/test/unit/link_quality_unittest.cc index b0d7fc2d03..407462c551 100644 --- a/src/test/unit/link_quality_unittest.cc +++ b/src/test/unit/link_quality_unittest.cc @@ -45,6 +45,7 @@ extern "C" { #include "fc/rc_modes.h" #include "fc/runtime_config.h" + #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -86,6 +87,7 @@ extern "C" { PG_REGISTER(pilotConfig_t, pilotConfig, PG_PILOT_CONFIG, 0); PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0); PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); + PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0); timeUs_t simulationTime = 0; @@ -502,6 +504,7 @@ extern "C" { void persistentObjectWrite(persistentObjectId_e, uint32_t) {} void failsafeOnRxSuspend(uint32_t ) {} void failsafeOnRxResume(void) {} + uint32_t failsafeFailurePeriodMs(void) { return 400; } void featureDisableImmediate(uint32_t) { } bool rxMspFrameComplete(void) { return false; } bool isPPMDataBeingReceived(void) { return false; } diff --git a/src/test/unit/rx_ranges_unittest.cc b/src/test/unit/rx_ranges_unittest.cc index 95b7364497..8f1969dac2 100644 --- a/src/test/unit/rx_ranges_unittest.cc +++ b/src/test/unit/rx_ranges_unittest.cc @@ -25,11 +25,13 @@ extern "C" { #include "build/debug.h" #include "drivers/io.h" #include "common/maths.h" + #include "flight/failsafe.h" #include "pg/pg.h" #include "pg/pg_ids.h" #include "pg/rx.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" + #include "fc/runtime_config.h" #include "rx/rx.h" } @@ -39,10 +41,12 @@ extern "C" { extern "C" { PG_REGISTER(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0); +PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0); boxBitmask_t rcModeActivationMask; int16_t debug[DEBUG16_VALUE_COUNT]; uint8_t debugMode = 0; +uint8_t armingFlags = 0; extern float applyRxChannelRangeConfiguraton(float sample, const rxChannelRangeConfig_t *range); } @@ -107,8 +111,11 @@ extern "C" { void failsafeOnRxSuspend(uint32_t ) {} void failsafeOnRxResume(void) {} +bool failsafeIsActive(void) { return false; } bool failsafeIsReceivingRxData(void) { return true; } bool taskUpdateRxMainInProgress() { return true; } +void setArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); } +void unsetArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); } uint32_t micros(void) { return 0; } uint32_t millis(void) { return 0; } @@ -225,6 +232,11 @@ void failsafeOnValidDataFailed(void) { } +uint32_t failsafeFailurePeriodMs(void) +{ + return 400; +} + float pt1FilterGain(float f_cut, float dT) { UNUSED(f_cut); diff --git a/src/test/unit/rx_rx_unittest.cc b/src/test/unit/rx_rx_unittest.cc index b2ecb580f1..d4a5453084 100644 --- a/src/test/unit/rx_rx_unittest.cc +++ b/src/test/unit/rx_rx_unittest.cc @@ -27,6 +27,8 @@ extern "C" { #include "build/debug.h" #include "drivers/io.h" #include "fc/rc_controls.h" + #include "fc/runtime_config.h" + #include "flight/failsafe.h" #include "rx/rx.h" #include "fc/rc_modes.h" #include "common/maths.h" @@ -39,6 +41,7 @@ extern "C" { boxBitmask_t rcModeActivationMask; int16_t debug[DEBUG16_VALUE_COUNT]; uint8_t debugMode = 0; + uint8_t armingFlags = 0; bool isPulseValid(uint16_t pulseDuration); @@ -47,6 +50,7 @@ extern "C" { ); PG_REGISTER(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0); + PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0); } #include "unittest_macros.h" @@ -210,7 +214,9 @@ extern "C" { void failsafeOnRxSuspend(uint32_t ) {} void failsafeOnRxResume(void) {} + bool failsafeIsActive(void) { return false; } bool failsafeIsReceivingRxData(void) { return true; } + uint32_t failsafeFailurePeriodMs(void) { return 400; } uint32_t micros(void) { return 0; } uint32_t millis(void) { return 0; } @@ -236,6 +242,8 @@ extern "C" { void xBusInit(const rxConfig_t *, rxRuntimeState_t *) {} void rxMspInit(const rxConfig_t *, rxRuntimeState_t *) {} void rxPwmInit(const rxConfig_t *, rxRuntimeState_t *) {} + void setArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); } + void unsetArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); } bool taskUpdateRxMainInProgress() { return true; } float pt1FilterGain(float f_cut, float dT) { diff --git a/src/test/unit/scheduler_unittest.cc b/src/test/unit/scheduler_unittest.cc index e5fe2bbb5a..aea553e027 100644 --- a/src/test/unit/scheduler_unittest.cc +++ b/src/test/unit/scheduler_unittest.cc @@ -74,7 +74,7 @@ extern "C" { uint32_t simulatedTime = 0; uint32_t micros(void) { return simulatedTime; } uint32_t millis(void) { return simulatedTime/1000; } // Note simplistic mapping suitable only for short unit tests - uint32_t clockCyclesToMicros(uint32_t x) { return x/10;} + int32_t clockCyclesToMicros(int32_t x) { return x/10;} int32_t clockCyclesTo10thMicros(int32_t x) { return x;} uint32_t clockMicrosToCycles(uint32_t x) { return x*10;} uint32_t getCycleCounter(void) {return simulatedTime * 10;}