rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #11497 from ctzsnooze/repair-commit-for-PR-11459 

fix for failsafe to apply throttle, aux and recovery delay settings
This commit is contained in:
haslinghuis 2022-04-02 01:40:00 +02:00 committed by GitHub
parent bef4c9a254 b360ccd60a
commit 7fcf16aad8
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 72 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/main/flight/failsafe.c
View File

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

3
src/main/flight/failsafe.h
View File

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

103
src/main/rx/rx.c
View File

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

6
src/test/unit/flight_failsafe_unittest.cc
View File

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