From f6ea053f19d397480bba10bec556be3e277881df Mon Sep 17 00:00:00 2001
From: night-ghost <night_ghost@ykoctpa.ru>
Date: Mon, 7 May 2018 11:00:07 +0500
Subject: [PATCH] HAL_F4Light: fixed RSSI issue on OSD

---
 libraries/AP_HAL_F4Light/hardware/osd/osd.cpp | 58 ++++++++++++++++++-
 1 file changed, 57 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_F4Light/hardware/osd/osd.cpp b/libraries/AP_HAL_F4Light/hardware/osd/osd.cpp
index 1046740b7f..dff326646b 100644
--- a/libraries/AP_HAL_F4Light/hardware/osd/osd.cpp
+++ b/libraries/AP_HAL_F4Light/hardware/osd/osd.cpp
@@ -333,7 +333,7 @@ static bool diff_done;
 extern void heartBeat();
 extern void writePanels();
 
-void On100ms() {}
+void On100ms();
 void On20ms() {}
 void osd_loop();
 void vsync_ISR();
@@ -975,7 +975,63 @@ void osd_loop() {
     }
 }
 
+float avgRSSI(uint16_t d){
+    static uint8_t ind = -1;
+    static uint16_t RSSI_rawArray[8];
 
+    RSSI_rawArray[(++ind)%8] = d;
+    d=0;
+    for (uint8_t i=8;i!=0;)
+        d += RSSI_rawArray[--i];
+
+    return d/8.0;
+}
+
+void On100ms() {
+
+        byte ch = sets.RSSI_raw / 2;
+
+        uint16_t d;
+
+
+//DBG_PRINTF("\n RSSI ch=%d ", ch);
+
+        switch(ch) {
+
+        case 1: // analog RSSI from pin
+        case 2: // digital RSSI from pin
+
+        case 0:
+            d=osd_rssi; // mavlink
+            goto case_4;
+
+        case 3: // 3dr modem rssi
+            d=telem_rssi;
+            goto case_4;
+
+        case 4:
+        default:
+            d = chan_raw[7]; // ch 8
+
+//DBG_PRINTF("ch8_rssi=%d\n", d );
+
+case_4:
+            rssi_in = avgRSSI(d);
+
+// RSSI source is not pin so we can read it for sensor
+#if defined(USE_SENSORS)
+            if(SENSOR4_ON) {
+                if(fPulseSensor[3])
+                    d=pulseIn(RssiPin,HIGH,10000);
+                else
+                    d=analogRead(RssiPin);
+
+                sensorData[3] = (sensorData[3]*7 + d) /8;
+            }
+#endif
+            break;
+        }
+}
 
 
 void vsync_ISR(){