From c6b88e5d04a9385150d022dffc2ac15364af04db Mon Sep 17 00:00:00 2001
From: mikeller <github@ike.ch>
Date: Sat, 11 Nov 2017 16:13:37 +1300
Subject: [PATCH] Cleaned up handling of RSSI, added resetting to 0 for RSSI
 over MSP.

---
 src/main/blackbox/blackbox.c       |  2 +-
 src/main/fc/fc_msp.c               | 13 ++----
 src/main/io/ledstrip.c             |  4 +-
 src/main/io/osd.c                  |  4 +-
 src/main/rx/flysky.c               |  4 +-
 src/main/rx/fport.c                |  2 +-
 src/main/rx/frsky_d.c              |  2 +-
 src/main/rx/rx.c                   | 65 ++++++++++++++++++++----------
 src/main/rx/rx.h                   |  8 ++--
 src/main/telemetry/ltm.c           |  2 +-
 src/main/telemetry/mavlink.c       |  2 +-
 src/test/unit/ledstrip_unittest.cc |  3 +-
 src/test/unit/osd_unittest.cc      |  2 +
 13 files changed, 67 insertions(+), 46 deletions(-)

diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 0719ee6176..39db6c9f3e 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1027,7 +1027,7 @@ static void loadMainState(timeUs_t currentTimeUs)
     blackboxCurrent->sonarRaw = sonarRead();
 #endif
 
-    blackboxCurrent->rssi = rssi;
+    blackboxCurrent->rssi = getRssi();
 
 #ifdef USE_SERVOS
     //Tail servo for tricopters
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 2189b5c857..c2d26e8525 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -442,7 +442,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce
 #ifdef USE_OSD_SLAVE
         sbufWriteU16(dst, 0); // rssi
 #else
-        sbufWriteU16(dst, rssi);
+        sbufWriteU16(dst, getRssi());
 #endif
         sbufWriteU16(dst, (int16_t)constrain(getAmperage(), -0x8000, 0x7FFF)); // send current in 0.01 A steps, range is -320A to 320A
         break;
@@ -1954,16 +1954,9 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
 #endif
 
     case MSP_TX_INFO:
-    {
-        uint8_t rssi_tx = sbufReadU8(src);
-        // Ignore rssi from MSP when RSSI channel or RSSIPWM feature is enabled
-        if (rxConfig()->rssi_channel == 0 && !featureConfigured(FEATURE_RSSI_ADC)) {
-            // Range of rssi_tx is [1;100]. rssi should be in [0;1023];
-            rssi = (uint16_t)((rssi_tx / 100.0f) * 1023.0f);
-        }
-    }
-    break;
+        setRssiMsp(sbufReadU8(src));
 
+        break;
     default:
         // we do not know how to handle the (valid) message, indicate error MSP $M!
         return MSP_RESULT_ERROR;
diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c
index 51cbfea728..7ee1373a0c 100644
--- a/src/main/io/ledstrip.c
+++ b/src/main/io/ledstrip.c
@@ -495,7 +495,7 @@ static void applyLedFixedLayers(void)
 
         case LED_FUNCTION_RSSI:
             color = HSV(RED);
-            hOffset += scaleRange(rssi * 100, 0, 1023, -30, 120);
+            hOffset += scaleRange(getRssi() * 100, 0, 1023, -30, 120);
             break;
 
         default:
@@ -713,7 +713,7 @@ static void applyLedRssiLayer(bool updateNow, timeUs_t *timer)
     int timerDelay = HZ_TO_US(1);
 
     if (updateNow) {
-        int state = (rssi * 100) / 1023;
+        int state = (getRssi() * 100) / 1023;
 
         if (state > 50) {
             flash = true;
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 61ea8b18a6..a58c9c7331 100755
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -323,7 +323,7 @@ static void osdDrawSingleElement(uint8_t item)
     switch (item) {
     case OSD_RSSI_VALUE:
         {
-            uint16_t osdRssi = rssi * 100 / 1024; // change range
+            uint16_t osdRssi = getRssi() * 100 / 1024; // change range
             if (osdRssi >= 100)
                 osdRssi = 99;
 
@@ -1148,7 +1148,7 @@ STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
         armState = ARMING_FLAG(ARMED);
     }
 
-    statRssi = scaleRange(rssi, 0, 1024, 0, 100);
+    statRssi = scaleRange(getRssi(), 0, 1024, 0, 100);
 
     osdUpdateStats();
 
diff --git a/src/main/rx/flysky.c b/src/main/rx/flysky.c
index f0a5630339..a10a4b701a 100644
--- a/src/main/rx/flysky.c
+++ b/src/main/rx/flysky.c
@@ -173,7 +173,7 @@ static void checkTimeout (void)
 
         if(countTimeout > 31) {
             timeout = timings->syncPacket;
-            rssi = 0;
+            setRssiFiltered(0);
         } else {
             timeout = timings->packet;
             countTimeout++;
@@ -197,7 +197,7 @@ static void checkRSSI (void)
     rssi_dBm = 50 + sum / (3 * FLYSKY_RSSI_SAMPLE_COUNT); // range about [95...52], -dBm
 
     int16_t tmp = 2280 - 24 * rssi_dBm;// convert to [0...1023]
-    rssi = (uint16_t) constrain(tmp, 0, 1023);// external variable from "rx/rx.h"
+    setRssiFiltered(constrain(tmp, 0, 1023));
 }
 
 static bool isValidPacket (const uint8_t *packet) {
diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c
index 819f1033a4..9bad0cc975 100644
--- a/src/main/rx/fport.c
+++ b/src/main/rx/fport.c
@@ -260,7 +260,7 @@ static uint8_t fportFrameStatus(void)
                     } else {
                         result = sbusChannelsDecode(&frame->data.controlData.channels);
 
-                        processRssi(constrain(frame->data.controlData.rssi, 0, 100));
+                        setRssiUnfiltered(scaleRange(frame->data.controlData.rssi, 0, 100, 0, 1024));
                     }
 
                     break;
diff --git a/src/main/rx/frsky_d.c b/src/main/rx/frsky_d.c
index af2585f659..107704d4c2 100644
--- a/src/main/rx/frsky_d.c
+++ b/src/main/rx/frsky_d.c
@@ -133,7 +133,7 @@ static void compute_RSSIdbm(uint8_t *packet)
         RSSI_dBm = ((((uint16_t)packet[18]) * 18) >> 5) + 65;
     }
 
-    processRssi(constrain((RSSI_dBm << 3) / 10, 0, 100));
+    setRssiUnfiltered(constrain(RSSI_dBm << 3, 0, 1024));
 }
 
 #if defined(USE_TELEMETRY_FRSKY)
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index 091ecdb179..26d70899a2 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -67,7 +67,11 @@
 
 const char rcChannelLetters[] = "AERT12345678abcdefgh";
 
-uint16_t rssi = 0;                  // range: [0;1023]
+static uint16_t rssi = 0;                  // range: [0;1023]
+static bool useMspRssi = true;
+static timeUs_t lastMspRssiUpdateUs = 0;
+
+#define MSP_RSSI_TIMEOUT_US 1500000   // 1.5 sec
 
 static bool rxDataReceived = false;
 static bool rxSignalReceived = false;
@@ -608,6 +612,12 @@ void parseRcChannels(const char *input, rxConfig_t *rxConfig)
     }
 }
 
+void setRssiFiltered(uint16_t newRssi)
+{
+    rssi = newRssi;
+    useMspRssi = false;
+}
+
 static void updateRSSIPWM(void)
 {
     // Read value of AUX channel as rssi
@@ -619,7 +629,7 @@ static void updateRSSIPWM(void)
     }
 
     // Range of rawPwmRssi is [1000;2000]. rssi should be in [0;1023];
-    rssi = (uint16_t)((constrain(pwmRssi - 1000, 0, 1000) / 1000.0f) * 1023.0f);
+    setRssiFiltered((uint16_t)((constrain(pwmRssi - 1000, 0, 1000) / 1000.0f) * 1023.0f));
 }
 
 static void updateRSSIADC(timeUs_t currentTimeUs)
@@ -635,38 +645,42 @@ static void updateRSSIADC(timeUs_t currentTimeUs)
     rssiUpdateAt = currentTimeUs + DELAY_50_HZ;
 
     const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI);
-    const uint8_t rssiPercentage = adcRssiSample / rxConfig()->rssi_scale;
+    unsigned rssiValue = (1024 * adcRssiSample) / (rxConfig()->rssi_scale * 100);
+    rssiValue = constrain(rssiValue, 0, 1024);
 
-    processRssi(rssiPercentage);
+    // RSSI_Invert option
+    if (rxConfig()->rssi_invert) {
+        rssiValue = 1024 - rssiValue;
+    }
+
+    setRssiUnfiltered((uint16_t)rssiValue);
 #endif
 }
 
 #define RSSI_SAMPLE_COUNT 16
 
-void processRssi(uint8_t rssiPercentage)
+void setRssiUnfiltered(uint16_t rssiValue)
 {
-    static uint8_t rssiSamples[RSSI_SAMPLE_COUNT];
+    static uint16_t rssiSamples[RSSI_SAMPLE_COUNT];
     static uint8_t rssiSampleIndex = 0;
+    static unsigned sum = 0;
 
+    sum = sum + rssiValue;
+    sum = sum - rssiSamples[rssiSampleIndex];
+    rssiSamples[rssiSampleIndex] = rssiValue;
     rssiSampleIndex = (rssiSampleIndex + 1) % RSSI_SAMPLE_COUNT;
 
-    rssiSamples[rssiSampleIndex] = rssiPercentage;
+    int16_t rssiMean = sum / RSSI_SAMPLE_COUNT;
 
-    int16_t rssiMean = 0;
-    for (int sampleIndex = 0; sampleIndex < RSSI_SAMPLE_COUNT; sampleIndex++) {
-        rssiMean += rssiSamples[sampleIndex];
+    setRssiFiltered((uint16_t)((rssiMean / 100.0f) * 1023.0f));
+}
+
+void setRssiMsp(uint8_t newMspRssi)
+{
+    if (useMspRssi) {
+        rssi = ((uint16_t)newMspRssi) << 2;
+        lastMspRssiUpdateUs = micros();
     }
-
-    rssiMean = rssiMean / RSSI_SAMPLE_COUNT;
-
-    rssiMean=constrain(rssiMean, 0, 100);
-
-    // RSSI_Invert option
-    if (rxConfig()->rssi_invert) {
-        rssiMean = 100 - rssiMean;
-    }
-
-    rssi = (uint16_t)((rssiMean / 100.0f) * 1023.0f);
 }
 
 void updateRSSI(timeUs_t currentTimeUs)
@@ -676,9 +690,18 @@ void updateRSSI(timeUs_t currentTimeUs)
         updateRSSIPWM();
     } else if (feature(FEATURE_RSSI_ADC)) {
         updateRSSIADC(currentTimeUs);
+    } else if (useMspRssi) {
+        if (cmpTimeUs(micros(), lastMspRssiUpdateUs) > MSP_RSSI_TIMEOUT_US) {
+            rssi = 0;
+        }
     }
 }
 
+uint16_t getRssi(void)
+{
+    return rssi;
+}
+
 uint16_t rxGetRefreshRate(void)
 {
     return rxRuntimeConfig.rxRefreshRate;
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
index f2c2c84cbf..7667a66c5e 100644
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@@ -76,8 +76,6 @@ typedef enum {
 #define MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT
 #endif
 
-extern uint16_t rssi;
-
 extern const char rcChannelLetters[];
 
 extern int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];       // interval [1000;2000]
@@ -171,8 +169,11 @@ void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs);
 
 void parseRcChannels(const char *input, rxConfig_t *rxConfig);
 
+void setRssiFiltered(uint16_t newRssi);
+void setRssiUnfiltered(uint16_t rssiValue);
+void setRssiMsp(uint8_t newMspRssi);
 void updateRSSI(timeUs_t currentTimeUs);
-void processRssi(uint8_t rssiPercentage);
+uint16_t getRssi(void);
 
 void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRangeConfig);
 
@@ -180,3 +181,4 @@ void suspendRxSignal(void);
 void resumeRxSignal(void);
 
 uint16_t rxGetRefreshRate(void);
+
diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c
index dddbd4e1e1..b378cf0ba8 100644
--- a/src/main/telemetry/ltm.c
+++ b/src/main/telemetry/ltm.c
@@ -192,7 +192,7 @@ static void ltm_sframe(void)
     ltm_initialise_packet('S');
     ltm_serialise_16(getBatteryVoltage() * 100);    //vbat converted to mv
     ltm_serialise_16(0);             //  current, not implemented
-    ltm_serialise_8((uint8_t)((rssi * 254) / 1023));        // scaled RSSI (uchar)
+    ltm_serialise_8((uint8_t)((getRssi() * 254) / 1023));        // scaled RSSI (uchar)
     ltm_serialise_8(0);              // no airspeed
     ltm_serialise_8((lt_flightmode << 2) | lt_statemode);
     ltm_finalise();
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index b69c01adaa..bc8fe86702 100755
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -268,7 +268,7 @@ void mavlinkSendRCChannelsAndRSSI(void)
         // chan8_raw RC channel 8 value, in microseconds
         (rxRuntimeConfig.channelCount >= 8) ? rcData[7] : 0,
         // rssi Receive signal strength indicator, 0: 0%, 255: 100%
-        scaleRange(rssi, 0, 1023, 0, 255));
+        scaleRange(getRssi(), 0, 1023, 0, 255));
     msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
     mavlinkSerialWrite(mavBuffer, msgLength);
 }
diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc
index 7bb59caaf4..44abc37372 100644
--- a/src/test/unit/ledstrip_unittest.cc
+++ b/src/test/unit/ledstrip_unittest.cc
@@ -298,7 +298,6 @@ uint16_t flightModeFlags = 0;
 float rcCommand[4];
 int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 uint32_t rcModeActivationMask;
-uint16_t rssi = 0;
 gpsSolutionData_t gpsSol;
 
 batteryState_e getBatteryState(void) {
@@ -384,4 +383,6 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {};
 
 bool isArmingDisabled(void) { return false; }
 
+uint16_t getRssi(void) { return 0; }
+
 }
diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc
index 59ef21c160..b807fe5a86 100644
--- a/src/test/unit/osd_unittest.cc
+++ b/src/test/unit/osd_unittest.cc
@@ -932,4 +932,6 @@ extern "C" {
     bool cmsDisplayPortRegister(displayPort_t *) {
         return false;
     }
+
+    uint16_t getRssi(void) { return rssi; }
 }