From a9f327fae094b18ffb30d77a8a7eb721cb524258 Mon Sep 17 00:00:00 2001
From: Mr D - RC <darren@darrenlines.uk>
Date: Sun, 3 Nov 2024 12:21:41 +0000
Subject: [PATCH] Update fc_msp.c

- Add RSSI
- Allow extended versions of future MSP2_COMMON_SET_MSP_RC_LINK_STATS and MSP2_COMMON_SET_MSP_RC_INFO still work with older (from now) INAV versions. Except for the new data.
---
 src/main/fc/fc_msp.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index 8bbf880b790..ab8a3b2061c 100644
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -2916,11 +2916,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 
 #ifdef USE_RX_MSP
     case MSP2_COMMON_SET_MSP_RC_LINK_STATS:
-        if (dataSize == 7) {
+        if (dataSize >= 7) {
             uint8_t sublinkID = sbufReadU8(src); // Sublink ID
             sbufReadU8(src); // Valid link (Failsafe backup)
             if (sublinkID == 1) {
-                sbufReadU8(src); // RSSI %
+                setRSSIFromMSP(sbufReadU8(src)); // RSSI %
                 rxLinkStatistics.uplinkRSSI = -sbufReadU16(src);
                 rxLinkStatistics.downlinkLQ = sbufReadU8(src);
                 rxLinkStatistics.uplinkLQ = sbufReadU8(src);
@@ -2931,7 +2931,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         break;
 
     case MSP2_COMMON_SET_MSP_RC_INFO:
-        if (dataSize == 15) {
+        if (dataSize >= 15) {
             uint8_t sublinkID = sbufReadU8(src);
 
             if (sublinkID == 1) {