AP_MSP: added support for MSP sensors

libraries/AP_MSP/AP_MSP_Telem_Backend.cpp

@@ -468,13 +468,28 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_sensor_command(uint16_t cmd_m
 
     switch (cmd_msp) {
     case MSP2_SENSOR_RANGEFINDER: {
-        const MSP::msp_rangefinder_sensor_t pkt = *(const MSP::msp_rangefinder_sensor_t *)src->ptr;
+        const MSP::msp_rangefinder_sensor_t *pkt = (const MSP::msp_rangefinder_sensor_t *)src->ptr;
-        msp_handle_rangefinder(pkt);
+        msp_handle_rangefinder(*pkt);
     }
     break;
     case MSP2_SENSOR_OPTIC_FLOW: {
-        const MSP::msp_opflow_sensor_t pkt = *(const MSP::msp_opflow_sensor_t *)src->ptr;
+        const MSP::msp_opflow_sensor_t *pkt = (const MSP::msp_opflow_sensor_t *)src->ptr;
-        msp_handle_opflow(pkt);
+        msp_handle_opflow(*pkt);
+    }
+    break;
+    case MSP2_SENSOR_GPS: {
+        const MSP::msp_gps_data_message_t *pkt = (const MSP::msp_gps_data_message_t *)src->ptr;
+        msp_handle_gps(*pkt);
+    }
+    break;
+    case MSP2_SENSOR_COMPASS: {
+        const MSP::msp_compass_data_message_t *pkt = (const MSP::msp_compass_data_message_t *)src->ptr;
+        msp_handle_compass(*pkt);
+    }
+    break;
+    case MSP2_SENSOR_BAROMETER: {
+        const MSP::msp_baro_data_message_t *pkt = (const MSP::msp_baro_data_message_t *)src->ptr;
+        msp_handle_baro(*pkt);
     }
     break;
     }
@@ -504,6 +519,27 @@ void AP_MSP_Telem_Backend::msp_handle_rangefinder(const MSP::msp_rangefinder_sen
 #endif
 }
 
+void AP_MSP_Telem_Backend::msp_handle_gps(const MSP::msp_gps_data_message_t &pkt)
+{
+#if HAL_MSP_GPS_ENABLED
+    AP::gps().handle_msp(pkt);
+#endif
+}
+
+void AP_MSP_Telem_Backend::msp_handle_compass(const MSP::msp_compass_data_message_t &pkt)
+{
+#if HAL_MSP_COMPASS_ENABLED
+    AP::compass().handle_msp(pkt);
+#endif
+}
+
+void AP_MSP_Telem_Backend::msp_handle_baro(const MSP::msp_baro_data_message_t &pkt)
+{
+#if HAL_MSP_BARO_ENABLED
+    AP::baro().handle_msp(pkt);
+#endif
+}
+
 MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_raw_gps(sbuf_t *dst)
 {
 #if OSD_ENABLED

libraries/AP_MSP/AP_MSP_Telem_Backend.h

@@ -161,6 +161,9 @@ protected:
     // MSP sensor command processing
     void msp_handle_opflow(const MSP::msp_opflow_sensor_t &pkt);
     void msp_handle_rangefinder(const MSP::msp_rangefinder_sensor_t &pkt);
+    void msp_handle_gps(const MSP::msp_gps_data_message_t &pkt);
+    void msp_handle_compass(const MSP::msp_compass_data_message_t &pkt);
+    void msp_handle_baro(const MSP::msp_baro_data_message_t &pkt);
 
     // implementation specific helpers
     // custom masks are needed for vendor specific settings

libraries/AP_MSP/msp.h

@@ -11,6 +11,7 @@
 #include "msp_protocol.h"
 #include "msp_sbuf.h"
 #include "msp_version.h"
+#include "msp_sensors.h"
 
 #if HAL_MSP_ENABLED
 
@@ -25,10 +26,8 @@
 #define MSP_PORT_INBUF_SIZE 192
 #define MSP_PORT_OUTBUF_SIZE 512
 #define MSP_MAX_HEADER_SIZE     9
-// betaflight/src/main/msp/msp_protocol_v2_sensor.h
+// inav/src/main/msp/msp_protocol_v2_sensor.h
 #define MSP2_IS_SENSOR_MESSAGE(x)   ((x) >= 0x1F00U && (x) <= 0x1FFFU)
-#define MSP2_SENSOR_RANGEFINDER     0x1F01
-#define MSP2_SENSOR_OPTIC_FLOW      0x1F02
 
 class AP_MSP_Telem_Backend;
 
@@ -108,17 +107,6 @@ typedef struct msp_port_s {
     uint8_t checksum2;
 } msp_port_t;
 
-typedef struct PACKED {
-    uint8_t quality;    // [0;255]
-    int32_t distance_mm; // Negative value for out of range
-} msp_rangefinder_sensor_t;
-
-typedef struct PACKED {
-    uint8_t quality;    // [0;255]
-    int32_t motion_x;
-    int32_t motion_y;
-} msp_opflow_sensor_t;
-
 // betaflight/src/main/sensors/battery.h
 typedef enum : uint8_t {
     MSP_BATTERY_OK = 0,

libraries/AP_MSP/msp_protocol.h

@@ -344,6 +344,7 @@
 #define MSP_SET_SIGNATURE        249    //in message          Sets the signature of the board and serial number
 
 // V2 commands
+#define MSP2_SENSOR_RANGEFINDER     0x1F01
 #define MSP2_SENSOR_OPTIC_FLOW      0x1F02
 #define MSP2_SENSOR_GPS             0x1F03
 #define MSP2_SENSOR_COMPASS         0x1F04

libraries/AP_MSP/msp_sensors.h

@@ -2,7 +2,7 @@
 
 namespace MSP
 {
-// inav/src/main/io/rangefinder_msp.c    
+// inav/src/main/io/rangefinder_msp.c
 typedef struct PACKED {
     uint8_t quality;    // [0;255]
     int32_t distance_mm; // Negative value for out of range