From 3830007fd2b271a117e6a407a1f0d309097a1f33 Mon Sep 17 00:00:00 2001
From: Bertrand Songis <bsongis@gmail.com>
Date: Tue, 16 Apr 2019 19:18:04 +0200
Subject: [PATCH] SWR fixes

---
 radio/src/lua/api_general.cpp           |  2 +-
 radio/src/telemetry/frsky.h             | 36 ++++++++++++---
 radio/src/telemetry/frsky_sport.cpp     |  4 +-
 radio/src/telemetry/telemetry.cpp       | 59 +++----------------------
 radio/src/telemetry/telemetry_holders.h |  4 ++
 5 files changed, 42 insertions(+), 63 deletions(-)

diff --git a/radio/src/lua/api_general.cpp b/radio/src/lua/api_general.cpp
index 97ec2467a..60a4efe30 100644
--- a/radio/src/lua/api_general.cpp
+++ b/radio/src/lua/api_general.cpp
@@ -672,7 +672,7 @@ This is just a hardware pass/fail measure and does not represent the quality of
 */
 static int luaGetRAS(lua_State * L)
 {
-  if (!IS_RAS_VALUE_VALID(telemetryData.xjtVersion)) {
+  if (isRasValueValid()) {
     lua_pushinteger(L, telemetryData.swrInternal.value);
   }
   else {
diff --git a/radio/src/telemetry/frsky.h b/radio/src/telemetry/frsky.h
index 2f21ea88c..0d928a08c 100644
--- a/radio/src/telemetry/frsky.h
+++ b/radio/src/telemetry/frsky.h
@@ -187,12 +187,12 @@ enum FrSkyDataState {
 #define RSSI_ID                   0xf101
 #define ADC1_ID                   0xf102
 #define ADC2_ID                   0xf103
-#define SP2UART_A_ID              0xfd00
-#define SP2UART_B_ID              0xfd01
 #define BATT_ID                   0xf104
 #define RAS_ID                    0xf105
 #define XJT_VERSION_ID            0xf106
 #define R9_PWR_ID                 0xf107
+#define SP2UART_A_ID              0xfd00
+#define SP2UART_B_ID              0xfd01
 #define FUEL_QTY_FIRST_ID         0x0a10
 #define FUEL_QTY_LAST_ID          0x0a1f
 
@@ -291,9 +291,9 @@ class TelemetryData {
     void setSwr(uint8_t origin, uint8_t value)
     {
       if (origin & 0x80)
-        swrExternal.set(0x00);
+        swrExternal.set(value);
       else
-        swrInternal.set(0x00);
+        swrInternal.set(value);
     }
 };
 
@@ -302,9 +302,33 @@ extern TelemetryData telemetryData;
 bool pushFrskyTelemetryData(uint8_t data); // returns true when end of frame detected
 void processFrskyTelemetryData(uint8_t data);
 
-inline bool IS_RAS_VALUE_VALID(uint16_t version)
+#if defined(NO_RAS)
+inline bool isRasValueValid()
 {
-  return version != 0x0000 && version != 0x00ff;
+  return false;
 }
+#elif defined(PXX2)
+inline bool isRasValueValid()
+{
+  return true;
+}
+#elif defined(PCBX10)
+inline bool isRasValueValid()
+{
+  return false;
+}
+#elif defined(PCBX9DP) || defined(PCBX9E)
+inline bool isRasValueValid()
+{
+  return telemetryData.xjtVersion != 0x0000 && telemetryData.xjtVersion != 0x00ff;
+}
+#else
+inline bool isRasValueValid()
+{
+  return false;
+}
+#endif
+
+constexpr uint8_t FRSKY_BAD_ANTENNA_THRESHOLD = 0x33;
 
 #endif // _FRSKY_H_
diff --git a/radio/src/telemetry/frsky_sport.cpp b/radio/src/telemetry/frsky_sport.cpp
index 0632634d9..c1f3675de 100644
--- a/radio/src/telemetry/frsky_sport.cpp
+++ b/radio/src/telemetry/frsky_sport.cpp
@@ -184,12 +184,12 @@ void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packe
     }
     else if (dataId == XJT_VERSION_ID) {
       telemetryData.xjtVersion = HUB_DATA_U16(packet);
-      if (!IS_RAS_VALUE_VALID(telemetryData.xjtVersion)) {
+      if (!isRasValueValid()) {
         telemetryData.setSwr(origin, 0);
       }
     }
     else if (dataId == RAS_ID) {
-      if (!IS_RAS_VALUE_VALID(telemetryData.xjtVersion)) {
+      if (isRasValueValid()) {
         telemetryData.setSwr(origin, SPORT_DATA_U8(packet));
       }
     }
diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp
index 530e30cf5..0341c0ebd 100644
--- a/radio/src/telemetry/telemetry.cpp
+++ b/radio/src/telemetry/telemetry.cpp
@@ -66,64 +66,15 @@ void processTelemetryData(uint8_t data)
   processFrskyTelemetryData(data);
 }
 
-#if defined(NO_RAS)
-inline bool IS_INTERNAL_RAS_VALUE_VALID()
-{
-  return false;
-}
-
-inline bool IS_EXTERNAL_RAS_VALUE_VALID()
-{
-  return false;
-}
-#elif defined(PXX2)
-inline bool IS_INTERNAL_RAS_VALUE_VALID()
-{
-  return get_tmr10ms() < telemetryData.swrInternal.expirationTime;
-}
-
-inline bool IS_EXTERNAL_RAS_VALUE_VALID()
-{
-  return get_tmr10ms() < telemetryData.swrExternal.expirationTime;
-}
-#elif defined(PCBX10)
-inline bool IS_INTERNAL_RAS_VALUE_VALID()
-{
-  return false;
-}
-
-inline bool IS_EXTERNAL_RAS_VALUE_VALID()
-{
-  return false;
-}
-#elif defined(PCBX9DP) || defined(PCBX9E)
-inline bool IS_INTERNAL_RAS_VALUE_VALID()
-{
-  return IS_RAS_VALUE_VALID(telemetryData.xjtVersion);
-}
-
-inline bool IS_EXTERNAL_RAS_VALUE_VALID()
-{
-  return IS_RAS_VALUE_VALID(telemetryData.xjtVersion);
-}
-#else
-inline bool IS_INTERNAL_RAS_VALUE_VALID()
-{
-  return false;
-}
-
-inline bool IS_EXTERNAL_RAS_VALUE_VALID()
-{
-  return false;
-}
-#endif
-
 inline bool isBadAntennaDetected()
 {
-  if (IS_INTERNAL_RAS_VALUE_VALID() && telemetryData.swrInternal.value > 0x33)
+  if (!isRasValueValid())
+    return false;
+
+  if (telemetryData.swrInternal.isFresh() && telemetryData.swrInternal.value > FRSKY_BAD_ANTENNA_THRESHOLD)
     return true;
 
-  if (IS_EXTERNAL_RAS_VALUE_VALID() && telemetryData.swrExternal.value > 0x33)
+  if (telemetryData.swrExternal.isFresh() && telemetryData.swrExternal.value > FRSKY_BAD_ANTENNA_THRESHOLD)
     return true;
 
   return false;
diff --git a/radio/src/telemetry/telemetry_holders.h b/radio/src/telemetry/telemetry_holders.h
index a08302e11..2993e9a3b 100644
--- a/radio/src/telemetry/telemetry_holders.h
+++ b/radio/src/telemetry/telemetry_holders.h
@@ -83,6 +83,10 @@ class TelemetryExpiringDecorator: public T {
     {
       memclear(this, sizeof(*this));
     }
+    bool isFresh()
+    {
+      return get_tmr10ms() < expirationTime;
+    }
 };
 
 template <class T>