AP_MSP: support AP_Periph MSP send

libraries/AP_MSP/AP_MSP_Telem_Backend.cpp

@@ -310,6 +310,7 @@ void AP_MSP_Telem_Backend::update_flight_mode_str(char *flight_mode_str, bool wi
                 MANU [S]
                 MANU [SS]
         */
+#ifndef HAL_NO_GCS
         const bool simple_mode = gcs().simple_input_active();
         const bool supersimple_mode = gcs().supersimple_input_active();
         const char* simple_mode_str = simple_mode ? " [S]" : (supersimple_mode ? " [SS]" : "");
@@ -320,6 +321,7 @@ void AP_MSP_Telem_Backend::update_flight_mode_str(char *flight_mode_str, bool wi
         // left pad
         uint8_t left_padded_len = MSP_TXT_VISIBLE_CHARS - (MSP_TXT_VISIBLE_CHARS - used)/2;
         snprintf(flight_mode_str, MSP_TXT_BUFFER_SIZE, "%*s", left_padded_len, buffer);
+#endif
     }
 }
 

libraries/AP_MSP/msp.cpp

@@ -48,14 +48,22 @@ uint32_t MSP::msp_serial_send_frame(msp_port_t *msp, const uint8_t * hdr, uint32
 /*
  ported from betaflight/src/main/msp/msp_serial.c
  */
-uint32_t MSP::msp_serial_encode(msp_port_t *msp, msp_packet_t *packet, msp_version_e msp_version)
+uint32_t MSP::msp_serial_encode(msp_port_t *msp, msp_packet_t *packet, msp_version_e msp_version, bool is_request)
 {
     static const uint8_t msp_magic[MSP_VERSION_COUNT] = MSP_VERSION_MAGIC_INITIALIZER;
     /*
         Note: after calling sbuf_switch_to_reader() sbuf_bytes_remaining() returns the size of the packet
     */
     const uint16_t data_len = sbuf_bytes_remaining(&packet->buf);
-    const uint8_t hdr_buf[16] = { '$', msp_magic[msp_version], static_cast<uint8_t>(packet->result == MSP_RESULT_ERROR ? '!' : '>')};
+    uint8_t code;
+    if (is_request) {
+        code = '<';
+    } else if (packet->result == MSP_RESULT_ERROR) {
+        code = '!';
+    } else {
+        code = '>';
+    }
+    const uint8_t hdr_buf[16] = { '$', msp_magic[msp_version], code };
     uint8_t crc_buf[2];
     uint32_t hdr_len = 3;
     uint32_t crc_len = 0;
@@ -310,4 +318,4 @@ bool MSP::msp_parse_received_data(msp_port_t *msp, uint8_t c)
     return true;
 }
 
-#endif //HAL_MSP_ENABLED
+#endif //HAL_MSP_ENABLED

libraries/AP_MSP/msp.h

@@ -130,8 +130,8 @@ typedef enum : uint8_t {
 
 uint8_t msp_serial_checksum_buf(uint8_t checksum, const uint8_t *data, uint32_t len);
 uint32_t msp_serial_send_frame(msp_port_t *msp, const uint8_t * hdr, uint32_t hdr_len, const uint8_t * data, uint32_t data_len, const uint8_t * crc, uint32_t crc_len);
-uint32_t msp_serial_encode(msp_port_t *msp, msp_packet_t *packet, msp_version_e msp_version);
+uint32_t msp_serial_encode(msp_port_t *msp, msp_packet_t *packet, msp_version_e msp_version, bool is_request=false);
 bool msp_parse_received_data(msp_port_t *msp, uint8_t c);
 }
 
-#endif //HAL_MSP_ENABLED
+#endif //HAL_MSP_ENABLED

libraries/AP_MSP/msp_protocol.h

@@ -342,3 +342,9 @@
 #define MSP_RTC                  247    //out message         Gets the RTC clock
 #define MSP_SET_BOARD_INFO       248    //in message          Sets the board information for this board
 #define MSP_SET_SIGNATURE        249    //in message          Sets the signature of the board and serial number
+
+// V2 commands
+#define MSP2_SENSOR_OPTIC_FLOW      0x1F02
+#define MSP2_SENSOR_GPS             0x1F03
+#define MSP2_SENSOR_COMPASS         0x1F04
+#define MSP2_SENSOR_BAROMETER       0x1F05