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

Simplified logic for recovery delay

Browse source
This commit is contained in:
Tony Cabello 2019-04-16 04:27:23 +02:00
parent 0a38ef67b4
commit b9360eef4e
2 changed files with 13 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -76,9 +76,9 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = { const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = {
"AUTO-LAND", "AUTO-LAND",
"DROP" "DROP",
#ifdef USE_GPS_RESCUE #ifdef USE_GPS_RESCUE
, "GPS-RESCUE" "GPS-RESCUE",
#endif #endif
}; };
@ -88,6 +88,7 @@ const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = {
void failsafeReset(void) void failsafeReset(void)
{ {
failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND; 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.validRxDataReceivedAt = 0; failsafeState.validRxDataReceivedAt = 0;
failsafeState.validRxDataFailedAt = 0; failsafeState.validRxDataFailedAt = 0;
failsafeState.throttleLowPeriod = 0; failsafeState.throttleLowPeriod = 0;
@ -177,7 +178,7 @@ void failsafeOnRxResume(void)
void failsafeOnValidDataReceived(void) void failsafeOnValidDataReceived(void)
{ {
failsafeState.validRxDataReceivedAt = millis(); failsafeState.validRxDataReceivedAt = millis();
if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > PERIOD_RXDATA_RECOVERY) { if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > failsafeState.rxDataRecoveryPeriod) {
failsafeState.rxLinkState = FAILSAFE_RXLINK_UP; failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;
unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
} }
@ -286,10 +287,8 @@ void failsafeUpdateState(void)
case FAILSAFE_LANDING: case FAILSAFE_LANDING:
if (receivingRxData) { if (receivingRxData) {
if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > (failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_RECOVERY)) { failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; reprocessState = true;
reprocessState = true;
}
} }
if (armed) { if (armed) {
failsafeApplyControlInput(); failsafeApplyControlInput();
@ -304,11 +303,9 @@ void failsafeUpdateState(void)
#ifdef USE_GPS_RESCUE #ifdef USE_GPS_RESCUE
case FAILSAFE_GPS_RESCUE: case FAILSAFE_GPS_RESCUE:
if (receivingRxData) { if (receivingRxData) {
if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > (failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_RECOVERY)) { if (areSticksActive(failsafeConfig()->failsafe_stick_threshold)) {
if (areSticksActive(failsafeConfig()->failsafe_stick_threshold)) { failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; reprocessState = true;
reprocessState = true;
}
} }
} }
if (armed) { if (armed) {

7
src/main/flight/failsafe.h
View file

@ -81,9 +81,10 @@ typedef struct failsafeState_s {
int16_t events; int16_t events;
bool monitoring; bool monitoring;
bool active; bool active;
int32_t rxDataFailurePeriod; uint32_t rxDataFailurePeriod;
int32_t validRxDataReceivedAt; uint32_t rxDataRecoveryPeriod;
int32_t validRxDataFailedAt; uint32_t validRxDataReceivedAt;
uint32_t validRxDataFailedAt;
uint32_t throttleLowPeriod; // throttle stick must have been below 'min_check' for this period uint32_t throttleLowPeriod; // throttle stick must have been below 'min_check' for this period
uint32_t landingShouldBeFinishedAt; uint32_t landingShouldBeFinishedAt;
uint32_t receivingRxDataPeriod; // period for the required period of valid rxData uint32_t receivingRxDataPeriod; // period for the required period of valid rxData