Merge pull request #1 from iNavFlight/MrD_mLRS-MSP-message

Mr d m lrs msp message
b14ckyy · Nov 3, 2024 · 20320fe · 20320fe
2 parents 625a3a0 + 66e9be4
commit 20320fe
Show file tree

Hide file tree

Showing 3 changed files with 67 additions and 23 deletions.
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
@@ -2914,6 +2914,43 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             return MSP_RESULT_ERROR;
         break;
 
+#ifdef USE_RX_MSP
+    case MSP2_COMMON_SET_MSP_RC_LINK_STATS:
+        if (dataSize == 48) {
+            uint8_t sublinkID = sbufReadU8(src); // Sublink ID
+            sbufReadU8(src); // Valid link (Failsafe backup)
+            if (sublinkID == 1) {
+                // RSSI %
+                rxLinkStatistics.uplinkRSSI = -sbufReadU8(src);
+                rxLinkStatistics.downlinkLQ = sbufReadU8(src);
+                rxLinkStatistics.uplinkLQ = sbufReadU8(src);
+                rxLinkStatistics.uplinkSNR = sbufReadU8(src);
+            }
+        } else
+            return MSP_RESULT_ERROR;
+        break;
+
+    case MSP2_COMMON_SET_MSP_RC_INFO:
+        if (dataSize == 104) {
+            uint8_t sublinkID = sbufReadU8(src);
+
+            if (sublinkID == 1) {
+                rxLinkStatistics.uplinkTXPower = sbufReadU8(src);
+                rxLinkStatistics.downlinkTXPower = sbufReadU8(src);
+
+                for (int i = 0; i < 4 - 1; i++) {
+                    rxLinkStatistics.band[i] = sbufReadU8(src);
+                }
+
+                for (int i = 0; i < 6 - 1; i++) {
+                    rxLinkStatistics.mode[i] = sbufReadU8(src);
+                }
+            }
+        } else
+            return MSP_RESULT_ERROR;
+        break;
+#endif
+
     case MSP_SET_FAILSAFE_CONFIG:
         if (dataSize == 20) {
             failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);

diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h
@@ -15,22 +15,25 @@
  * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#define MSP2_COMMON_TZ                  0x1001  //out message       Gets the TZ offset for the local time (returns: minutes(i16))
-#define MSP2_COMMON_SET_TZ              0x1002  //in message        Sets the TZ offset for the local time (args: minutes(i16))
-#define MSP2_COMMON_SETTING             0x1003  //in/out message    Returns the value for a setting
-#define MSP2_COMMON_SET_SETTING         0x1004  //in message        Sets the value for a setting
+#define MSP2_COMMON_TZ                      0x1001  //out message       Gets the TZ offset for the local time (returns: minutes(i16))
+#define MSP2_COMMON_SET_TZ                  0x1002  //in message        Sets the TZ offset for the local time (args: minutes(i16))
+#define MSP2_COMMON_SETTING                 0x1003  //in/out message    Returns the value for a setting
+#define MSP2_COMMON_SET_SETTING             0x1004  //in message        Sets the value for a setting
+
+#define MSP2_COMMON_MOTOR_MIXER             0x1005
+#define MSP2_COMMON_SET_MOTOR_MIXER         0x1006
+
+#define MSP2_COMMON_SETTING_INFO            0x1007  //in/out message    Returns info about a setting (PG, type, flags, min/max, etc..).
+#define MSP2_COMMON_PG_LIST                 0x1008  //in/out message    Returns a list of the PG ids used by the settings
+
+#define MSP2_COMMON_SERIAL_CONFIG           0x1009
+#define MSP2_COMMON_SET_SERIAL_CONFIG       0x100A
+
+// radar commands   
+#define MSP2_COMMON_SET_RADAR_POS           0x100B //SET radar position information
+#define MSP2_COMMON_SET_RADAR_ITD           0x100C //SET radar information to display
 
-#define MSP2_COMMON_MOTOR_MIXER         0x1005
-#define MSP2_COMMON_SET_MOTOR_MIXER     0x1006
+#define MSP2_COMMON_SET_MSP_RC_LINK_STATS   0x100D //in message        Sets the MSP RC stats
+#define MSP2_COMMON_SET_MSP_RC_INFO         0x100E //in message        Sets the MSP RC info
 
-#define MSP2_COMMON_SETTING_INFO        0x1007  //in/out message    Returns info about a setting (PG, type, flags, min/max, etc..).
-#define MSP2_COMMON_PG_LIST             0x1008  //in/out message    Returns a list of the PG ids used by the settings
-
-#define MSP2_COMMON_SERIAL_CONFIG       0x1009
-#define MSP2_COMMON_SET_SERIAL_CONFIG   0x100A
-
-// radar commands
-#define MSP2_COMMON_SET_RADAR_POS       0x100B //SET radar position information
-#define MSP2_COMMON_SET_RADAR_ITD       0x100C //SET radar information to display
-
-#define MSP2_BETAFLIGHT_BIND            0x3000
+#define MSP2_BETAFLIGHT_BIND                0x3000
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
@@ -181,12 +181,16 @@ typedef enum {
 } rssiSource_e;
 
 typedef struct rxLinkStatistics_s {
-    int16_t uplinkRSSI;     // RSSI value in dBm
-    uint8_t uplinkLQ;       // A protocol specific measure of the link quality in [0..100]
-    int8_t uplinkSNR;       // The SNR of the uplink in dB
-    uint8_t rfMode;         // A protocol specific measure of the transmission bandwidth [2 = 150Hz, 1 = 50Hz, 0 = 4Hz]
-    uint16_t uplinkTXPower; // power in mW
-    uint8_t activeAntenna;
+    int16_t     uplinkRSSI;         // RSSI value in dBm
+    uint8_t     uplinkLQ;           // A protocol specific measure of the link quality in [0..100]
+    uint8_t     downlinkLQ;         // A protocol specific measure of the link quality in [0..100]
+    int8_t      uplinkSNR;          // The SNR of the uplink in dB
+    uint8_t     rfMode;             // A protocol specific measure of the transmission bandwidth [2 = 150Hz, 1 = 50Hz, 0 = 4Hz]
+    uint16_t    uplinkTXPower;      // power in mW
+    uint16_t    downlinkTXPower;    // power in mW
+    uint8_t     activeAntenna;
+    char        band[4];
+    char        mode[6];
 } rxLinkStatistics_t;
 
 typedef uint16_t (*rcReadRawDataFnPtr)(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data