From b9360eef4e97e39cf66c32cadea8821f7e9fd9f2 Mon Sep 17 00:00:00 2001
From: Tony Cabello <tcabello@blitworks.com>
Date: Tue, 16 Apr 2019 04:27:23 +0200
Subject: [PATCH] Simplified logic for recovery delay

---
 src/main/flight/failsafe.c | 21 +++++++++------------
 src/main/flight/failsafe.h |  7 ++++---
 2 files changed, 13 insertions(+), 15 deletions(-)

diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c
index 58991c6e63..0a3b804e56 100644
--- a/src/main/flight/failsafe.c
+++ b/src/main/flight/failsafe.c
@@ -76,9 +76,9 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
 
 const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = {
     "AUTO-LAND",
-    "DROP"
+    "DROP",
 #ifdef USE_GPS_RESCUE
-    , "GPS-RESCUE"
+    "GPS-RESCUE",
 #endif
 };
 
@@ -88,6 +88,7 @@ const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = {
 void failsafeReset(void)
 {
     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.validRxDataFailedAt = 0;
     failsafeState.throttleLowPeriod = 0;
@@ -177,7 +178,7 @@ void failsafeOnRxResume(void)
 void failsafeOnValidDataReceived(void)
 {
     failsafeState.validRxDataReceivedAt = millis();
-    if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > PERIOD_RXDATA_RECOVERY) {
+    if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > failsafeState.rxDataRecoveryPeriod) {
         failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;
         unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
     }
@@ -286,10 +287,8 @@ void failsafeUpdateState(void)
 
             case FAILSAFE_LANDING:
                 if (receivingRxData) {
-                    if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > (failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_RECOVERY)) {
-                        failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
-                        reprocessState = true;
-                    }
+                    failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
+                    reprocessState = true;
                 }
                 if (armed) {
                     failsafeApplyControlInput();
@@ -304,11 +303,9 @@ void failsafeUpdateState(void)
 #ifdef USE_GPS_RESCUE
             case FAILSAFE_GPS_RESCUE:
                 if (receivingRxData) {
-                    if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > (failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND + PERIOD_RXDATA_RECOVERY)) {
-                        if (areSticksActive(failsafeConfig()->failsafe_stick_threshold)) {
-                            failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
-                            reprocessState = true;
-                        }
+                    if (areSticksActive(failsafeConfig()->failsafe_stick_threshold)) {
+                        failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
+                        reprocessState = true;
                     }
                 }
                 if (armed) {
diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h
index da32184878..739fed6117 100644
--- a/src/main/flight/failsafe.h
+++ b/src/main/flight/failsafe.h
@@ -81,9 +81,10 @@ typedef struct failsafeState_s {
     int16_t events;
     bool monitoring;
     bool active;
-    int32_t rxDataFailurePeriod;
-    int32_t validRxDataReceivedAt;
-    int32_t validRxDataFailedAt;
+    uint32_t rxDataFailurePeriod;
+    uint32_t rxDataRecoveryPeriod; 
+    uint32_t validRxDataReceivedAt;
+    uint32_t validRxDataFailedAt;
     uint32_t throttleLowPeriod;             // throttle stick must have been below 'min_check' for this period
     uint32_t landingShouldBeFinishedAt;
     uint32_t receivingRxDataPeriod;         // period for the required period of valid rxData