From b48d7c8ad4e674ca412f248b1265b72b0d01a933 Mon Sep 17 00:00:00 2001
From: Mr D - RC <darren@darrenlines.uk>
Date: Sun, 3 Nov 2024 17:11:31 +0000
Subject: [PATCH] Add function to set the RSSI from MSP_RC

---
 src/main/fc/fc_msp.c |  2 +-
 src/main/rx/rx.c     | 12 ++++++++++++
 src/main/rx/rx.h     |  1 +
 3 files changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 50146de8aac..20b30a71f1f 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -2920,7 +2920,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             uint8_t sublinkID = sbufReadU8(src); // Sublink ID
             sbufReadU8(src); // Valid link (Failsafe backup)
             if (sublinkID == 1) {
-                setRSSIFromMSP(sbufReadU8(src)); // RSSI %
+                setRSSIFromMSP_RC(sbufReadU8(src)); // RSSI %
                 rxLinkStatistics.uplinkRSSI = -sbufReadU16(src);
                 rxLinkStatistics.downlinkLQ = sbufReadU8(src);
                 rxLinkStatistics.uplinkLQ = sbufReadU8(src);
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index f941eaebe20..5743f514959 100755
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -572,6 +572,18 @@ static void setRSSIValue(uint16_t rssiValue, rssiSource_e source, bool filtered)
     rssi = constrain(scaleRange(rssi, rssiMin, rssiMax, 0, RSSI_MAX_VALUE), 0, RSSI_MAX_VALUE);
 }
 
+void setRSSIFromMSP_RC(uint8_t newMspRssi)
+{
+    if (activeRssiSource == RSSI_SOURCE_NONE && (rxConfig()->rssi_source == RSSI_SOURCE_MSP || rxConfig()->rssi_source == RSSI_SOURCE_AUTO)) {
+        activeRssiSource = RSSI_SOURCE_MSP;
+    }
+
+    if (activeRssiSource == RSSI_SOURCE_MSP) {
+        rssi = constrain(scaleRange(newMspRssi, 0, 100, 0, RSSI_MAX_VALUE), 0, RSSI_MAX_VALUE);
+        lastMspRssiUpdateUs = micros();
+    }
+}
+
 void setRSSIFromMSP(uint8_t newMspRssi)
 {
     if (activeRssiSource == RSSI_SOURCE_NONE && (rxConfig()->rssi_source == RSSI_SOURCE_MSP || rxConfig()->rssi_source == RSSI_SOURCE_AUTO)) {
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
index 4d8a76fecf5..64b97b172e2 100644
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@@ -216,6 +216,7 @@ bool isRxPulseValid(uint16_t pulseDuration);
 uint8_t calculateChannelRemapping(const uint8_t *channelMap, uint8_t channelMapEntryCount, uint8_t channelToRemap);
 void parseRcChannels(const char *input);
 
+void setRSSIFromMSP_RC(uint8_t newMspRssi);
 void setRSSIFromMSP(uint8_t newMspRssi);
 void updateRSSI(timeUs_t currentTimeUs);
 // Returns RSSI in [0, RSSI_MAX_VALUE] range.