diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 1698dabebf..d329817bd3 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -125,6 +125,13 @@ bool failsafeIsActive(void) return failsafeState.active; } +#ifdef USE_GPS_RESCUE +bool failsafePhaseIsGpsRescue(void) +{ + return failsafeState.phase == FAILSAFE_GPS_RESCUE; +} +#endif + void failsafeStartMonitoring(void) { failsafeState.monitoring = true; @@ -255,10 +262,13 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) && failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_GPS_RESCUE #endif ) { - // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds + // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds before failsafe + // protects against false arming when the Tx is powered up after the quad failsafeActivate(); - failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure - failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData + // skip auto-landing procedure + failsafeState.phase = FAILSAFE_LANDED; + // re-arm at rxDataRecoveryPeriod - default is 1.0 seconds + failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod; } else { failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED; } @@ -289,6 +299,8 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) case FAILSAFE_PROCEDURE_DROP_IT: // Drop the craft failsafeActivate(); + // re-arm at rxDataRecoveryPeriod - default is 1.0 seconds + failsafeState.receivingRxDataPeriodPreset = failsafeState.rxDataRecoveryPeriod; // skip auto-landing procedure failsafeState.phase = FAILSAFE_LANDED; break; @@ -323,6 +335,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void) case FAILSAFE_GPS_RESCUE: if (receivingRxData) { if (areSticksActive(failsafeConfig()->failsafe_stick_threshold)) { + // hence we must allow stick inputs during FAILSAFE_GPS_RESCUE see PR #7936 for rationale failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; reprocessState = true; } diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 31c5b01b7e..3637782b39 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -107,3 +107,6 @@ void failsafeOnRxSuspend(uint32_t suspendPeriod); void failsafeOnRxResume(void); void failsafeOnValidDataReceived(void); void failsafeOnValidDataFailed(void); +#ifdef USE_GPS_RESCUE +bool failsafePhaseIsGpsRescue(void); +#endif diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index ac3a71bcf3..b607c3431a 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -103,7 +103,6 @@ static bool auxiliaryProcessingRequired = false; static bool rxSignalReceived = false; static bool rxFlightChannelsValid = false; -static bool rxIsInFailsafeMode = true; static uint8_t rxChannelCount; static timeUs_t needRxSignalBefore = 0; @@ -477,13 +476,13 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current { bool signalReceived = false; bool useDataDrivenProcessing = true; - // Need the next packet in 2.5 x the anticipated time + // 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 + // no need to check for new data as a packet is being processed already return; } @@ -495,7 +494,6 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current case RX_PROVIDER_PPM: if (isPPMDataBeingReceived()) { signalReceived = true; - rxIsInFailsafeMode = false; resetPPMDataReceivedState(); } @@ -503,7 +501,6 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current case RX_PROVIDER_PARALLEL_PWM: if (isPWMDataBeingReceived()) { signalReceived = true; - rxIsInFailsafeMode = false; useDataDrivenProcessing = false; } @@ -514,36 +511,34 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current case RX_PROVIDER_SPI: { const uint8_t frameStatus = rxRuntimeState.rcFrameStatusFn(&rxRuntimeState); - if (frameStatus & RX_FRAME_COMPLETE) { - rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0; - bool rxFrameDropped = (frameStatus & RX_FRAME_DROPPED) != 0; - signalReceived = !(rxIsInFailsafeMode || rxFrameDropped); - setLinkQuality(signalReceived, currentDeltaTimeUs); - } - if (frameStatus & RX_FRAME_PROCESSING_REQUIRED) { - auxiliaryProcessingRequired = true; - } + DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 1, (frameStatus & RX_FRAME_FAILSAFE)); + signalReceived = (frameStatus & RX_FRAME_COMPLETE) && !((frameStatus & RX_FRAME_FAILSAFE) || (frameStatus & RX_FRAME_DROPPED)); + setLinkQuality(signalReceived, currentDeltaTimeUs); + auxiliaryProcessingRequired |= (frameStatus & RX_FRAME_PROCESSING_REQUIRED); } break; } if (signalReceived) { + // true only when a new packet arrives 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) { + // watch for next packet + if (cmpTimeUs(currentTimeUs, needRxSignalBefore) > 0) { + // initial time to signalReceived failure is 2.5 packets then every 100ms rxSignalReceived = false; - // rcData[] needs to be processed to rcCommand - rxDataProcessingRequired = true; needRxSignalBefore = currentTimeUs + (timeDelta_t)DELAY_10_HZ; + // review rcData values every 100ms in failsafe changed them + rxDataProcessingRequired = true; } } - if (signalReceived && useDataDrivenProcessing) { - rxDataProcessingRequired = true; - } + DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 0, rxSignalReceived); + // process the new Rx packet when it arrives + rxDataProcessingRequired |= (signalReceived && useDataDrivenProcessing); + } #if defined(USE_PWM) || defined(USE_PPM) @@ -643,51 +638,57 @@ static void readRxChannelsApplyRanges(void) } } -static void detectAndApplySignalLossBehaviour(void) +void detectAndApplySignalLossBehaviour(void) { const uint32_t currentTimeMs = millis(); 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); +#ifdef USE_GPS_RESCUE + const bool gpsRescue = failsafePhaseIsGpsRescue(); +#endif + rxFlightChannelsValid = rxSignalReceived && !failsafeAuxSwitch; + // set rxFlightChannelsValid false when a packet is bad or we use a failsafe switch 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 thisChannelValid = rxFlightChannelsValid && isPulseValid(sample); + // if the packet is bad, we don't allow any channels to be good if (thisChannelValid) { - // reset invalid pulse period timer for each good-channel + // reset the invalid pulse period timer for every 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 (ARMING_FLAG(ARMED) && failsafeIsActive()) { + // apply failsafe values, until failsafe ends, or disarmed, unless in GPS Return if (channel < NON_AUX_CHANNEL_COUNT) { +#ifdef USE_GPS_RESCUE + if (gpsRescue) { + continue; + } +#endif 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 + // aux channels as Set in Configurator, unless failsafe initiated by switch sample = getRxfailValue(channel); } } else { + // in STAGE 1 failsafe or HOLD period. if (!thisChannelValid) { if (cmp32(currentTimeMs, validRxSignalTimeout[channel]) < 0) { - // HOLD PERIOD for any invalid channels + // HOLD PERIOD is MAX_INVALID_PULSE_TIME or 300ms for invalid channels/packets } else { - // STAGE 1 failsafe settings apply to any channels invalid for more than hold time + // in STAGE 1 failsafe now that hold time has expired if (channel < NON_AUX_CHANNEL_COUNT) { rxFlightChannelsValid = false; + // some flight channels were bad, so flag them sample = getRxfailValue(channel); - // affected RPYT values will be set as per Stage 1 Configurator settings + // affected RPYT values will get Stage 1 values } else if (!failsafeAuxSwitch) { - // Aux channels as Set in Configurator, unless failsafe initiated by switch + // for switch initiated failsafe, only the flight channels get Stage 1 values sample = getRxfailValue(channel); } } @@ -700,26 +701,22 @@ static void detectAndApplySignalLossBehaviour(void) rcData[channel] = calculateChannelMovingAverage(channel, sample); } else #endif + { - // set rcData to either clean raw incoming values, or failsafe values + // set rcData to either clean raw incoming values, or failsafe-modified values rcData[channel] = sample; } } - 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 + if (rxFlightChannelsValid) { + // clear RXLOSS in OSD unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); + // --> start the timer to exit stage 2 failsafe failsafeOnValidDataReceived(); } else { - // failsafe switch is on, or a bad packet, or at least one invalid channel for more than hold time + // show RXLOSS in OSD (may be a little aggressive ? if so move up to Stage 1) + setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); // -> 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(); } @@ -747,8 +744,8 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs) return true; } - readRxChannelsApplyRanges(); - detectAndApplySignalLossBehaviour(); + readRxChannelsApplyRanges(); // returns rcRaw + detectAndApplySignalLossBehaviour(); // returns rcData rcSampleIndex++; @@ -973,4 +970,4 @@ timeDelta_t rxGetFrameDelta(timeDelta_t *frameAgeUs) timeUs_t rxFrameTimeUs(void) { return rxRuntimeState.lastRcFrameTimeUs; -} +} \ No newline at end of file diff --git a/src/test/unit/flight_failsafe_unittest.cc b/src/test/unit/flight_failsafe_unittest.cc index 7287b00ce5..b8ebfcc24c 100644 --- a/src/test/unit/flight_failsafe_unittest.cc +++ b/src/test/unit/flight_failsafe_unittest.cc @@ -290,7 +290,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms) // given failsafeOnValidDataFailed(); // set last invalid sample at current time - sysTickUptime += PERIOD_RXDATA_RECOVERY + 1; // adjust time to point just past the recovery time to + sysTickUptime += PERIOD_RXDATA_RECOVERY; // adjust time to point just past the recovery time to failsafeOnValidDataReceived(); // cause a recovered link // when @@ -303,7 +303,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndJustDisarms) EXPECT_TRUE(isArmingDisabled()); // given - sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time + sysTickUptime += PERIOD_OF_1_SECONDS + 1; // adjust time to point just past the required additional recovery time failsafeOnValidDataReceived(); // when @@ -401,7 +401,7 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Drop) EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); // given - sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time + sysTickUptime += PERIOD_OF_1_SECONDS + 1; // adjust time to point just past the required additional recovery time deactivateBoxFailsafe(); failsafeOnValidDataReceived(); // inactive box failsafe gives valid data