From 5ca5c2d161b23c2cd83a284dbc7367e9c3e2d61e Mon Sep 17 00:00:00 2001
From: ProDrone <me@prodrone.com>
Date: Sun, 6 Sep 2015 19:53:12 +0200
Subject: [PATCH] Adapted RX suspend & resume on EEPROM read & write

Modified to make mechanism use time instead of counters for call
frequency independency.
Skipping RC samples uses a counter for time independency.

Also LED and beeper patterns were changed by previous implementation.
Corrected with RX supend/resume feedback to failsafe system.
---
 src/main/flight/failsafe.c | 11 +++++++++++
 src/main/flight/failsafe.h |  2 ++
 src/main/rx/rx.c           | 18 ++++++++++++++----
 3 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c
index f02b318f8a..037c424140 100644
--- a/src/main/flight/failsafe.c
+++ b/src/main/flight/failsafe.c
@@ -132,6 +132,17 @@ bool failsafeIsReceivingRxData(void)
     return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP);
 }
 
+void failsafeOnRxSuspend(uint32_t usSuspendPeriod)
+{
+    failsafeState.validRxDataReceivedAt += (usSuspendPeriod / 1000);    // / 1000 to convert micros to millis
+}
+
+void failsafeOnRxResume(void)
+{
+    failsafeState.validRxDataReceivedAt = millis();                     // prevent RX link down trigger, restart rx link up
+    failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;                     // do so while rx link is up
+}
+
 void failsafeOnValidDataReceived(void)
 {
     failsafeState.validRxDataReceivedAt = millis();
diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h
index 4e2c4b52d6..7d841dd111 100644
--- a/src/main/flight/failsafe.h
+++ b/src/main/flight/failsafe.h
@@ -73,6 +73,8 @@ failsafePhase_e failsafePhase();
 bool failsafeIsMonitoring(void);
 bool failsafeIsActive(void);
 bool failsafeIsReceivingRxData(void);
+void failsafeOnRxSuspend(uint32_t suspendPeriod);
+void failsafeOnRxResume(void);
 
 void failsafeOnValidDataReceived(void);
 void failsafeOnValidDataFailed(void);
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index ffd5b1754c..67c184f9ff 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -40,6 +40,7 @@
 #include "drivers/gpio.h"
 #include "drivers/timer.h"
 #include "drivers/pwm_rx.h"
+#include "drivers/system.h"
 #include "rx/pwm.h"
 #include "rx/sbus.h"
 #include "rx/spektrum.h"
@@ -50,6 +51,7 @@
 
 #include "rx/rx.h"
 
+
 //#define DEBUG_RX_SIGNAL_LOSS
 
 void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
@@ -71,6 +73,7 @@ static bool rxFlightChannelsValid = false;
 
 static uint32_t rxUpdateAt = 0;
 static uint32_t needRxSignalBefore = 0;
+static uint32_t suspendRxSignalUntil = 0;
 static uint8_t  skipRxSamples = 0;
 
 int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
@@ -80,8 +83,8 @@ int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
 
 #define DELAY_50_HZ (1000000 / 50)
 #define DELAY_10_HZ (1000000 / 10)
-#define SKIP_RC_SAMPLES_ON_SUSPEND 75               // approx. 1.5 seconds of samples at 50Hz
-#define SKIP_RC_SAMPLES_ON_RESUME  2                // flush 2 samples to drop wrong measurements
+#define SKIP_RC_ON_SUSPEND_PERIOD 1500000           // 1.5 second period in usec (call frequency independent)
+#define SKIP_RC_SAMPLES_ON_RESUME  2                // flush 2 samples to drop wrong measurements (timing independent)
 
 rxRuntimeConfig_t rxRuntimeConfig;
 static rxConfig_t *rxConfig;
@@ -266,12 +269,16 @@ static void resetRxSignalReceivedFlagIfNeeded(uint32_t currentTime)
 
 void suspendRxSignal(void)
 {
-    skipRxSamples = SKIP_RC_SAMPLES_ON_SUSPEND;
+    suspendRxSignalUntil = micros() + SKIP_RC_ON_SUSPEND_PERIOD;
+    skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
+    failsafeOnRxSuspend(SKIP_RC_ON_SUSPEND_PERIOD);
 }
 
 void resumeRxSignal(void)
 {
+    suspendRxSignalUntil = micros();
     skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
+    failsafeOnRxResume();
 }
 
 void updateRx(uint32_t currentTime)
@@ -481,8 +488,11 @@ void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime)
         }
     }
 
+    // only proceed when no more samples to skip and suspend period is over
     if (skipRxSamples) {
-        skipRxSamples--;
+        if (currentTime > suspendRxSignalUntil) {
+            skipRxSamples--;
+        }
         return;
     }