allow sticks in GPS Rescue

This commit is contained in:
ctzsnooze 2022-04-01 18:11:40 +11:00
parent d8aeb89710
commit b360ccd60a
3 changed files with 24 additions and 6 deletions

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

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

View File

@ -642,6 +642,9 @@ void detectAndApplySignalLossBehaviour(void)
{
const uint32_t currentTimeMs = millis();
const bool failsafeAuxSwitch = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
#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
@ -651,28 +654,32 @@ void detectAndApplySignalLossBehaviour(void)
// if the packet is bad, we don't allow any channels to be good
if (thisChannelValid) {
// reset the 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)) {
// in STAGE 2 failsafe, 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) {
// in HOLD PERIOD of 300ms for invalid channels/packets
// HOLD PERIOD is MAX_INVALID_PULSE_TIME or 300ms for invalid channels/packets
} else {
// in STAGE 1 failsafe now that hold time has expired
if (channel < NON_AUX_CHANNEL_COUNT) {