Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

add gps rescue mode

Browse source
This commit is contained in:
s0up 2018-05-15 14:32:03 -07:00 • committed by Diego Basch
parent 43c706fc95
commit ac6b8088c9
24 changed files with 653 additions and 32 deletions
Download patch file Download diff file Expand all files Collapse all files

39
src/main/flight/failsafe.c
View file

@ -123,7 +123,9 @@ static bool failsafeShouldHaveCausedLandingByNow(void)
static void failsafeActivate(void)
{
failsafeState.active = true;
failsafeState.phase = FAILSAFE_LANDING;
ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
@ -132,6 +134,12 @@ static void failsafeActivate(void)
static void failsafeApplyControlInput(void)
{
if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE) {
ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
return;
}
for (int i = 0; i < 3; i++) {
rcData[i] = rxConfig()->midrc;
}
@ -208,14 +216,8 @@ void failsafeUpdateState(void)
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_1_SECONDS; // require 1 seconds of valid rxData
reprocessState = true;
} else if (!receivingRxData) {
if (millis() > failsafeState.throttleLowPeriod) {
// JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds
failsafeActivate();
failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData
} else {
failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
}
failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
reprocessState = true;
}
} else {
@ -235,7 +237,6 @@ void failsafeUpdateState(void)
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
} else {
switch (failsafeConfig()->failsafe_procedure) {
default:
case FAILSAFE_PROCEDURE_AUTO_LANDING:
// Stabilize, and set Throttle to specified level
failsafeActivate();
@ -247,6 +248,11 @@ void failsafeUpdateState(void)
failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData
break;
case FAILSAFE_PROCEDURE_GPS_RESCUE:
failsafeActivate();
failsafeState.phase = FAILSAFE_GPS_RESCUE;
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS;
break;
}
}
reprocessState = true;
@ -267,7 +273,20 @@ void failsafeUpdateState(void)
reprocessState = true;
}
break;
case FAILSAFE_GPS_RESCUE:
if (receivingRxData) {
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;
}
break;
case FAILSAFE_LANDED:
setArmingDisabled(ARMING_DISABLED_FAILSAFE); // To prevent accidently rearming by an intermittent rx link
disarm();