AP_Periph: Add MSP sensor output

Tools/AP_Periph/AP_Periph.cpp

@@ -140,6 +140,10 @@ void AP_Periph_FW::init()
 #ifdef HAL_PERIPH_ENABLE_HWESC
     hwesc_telem.init(hal.uartB);
 #endif
+
+#ifdef HAL_PERIPH_ENABLE_MSP
+    msp_init(hal.uartD);
+#endif
 
     start_ms = AP_HAL::millis();
 }

Tools/AP_Periph/AP_Periph.h

@@ -5,6 +5,8 @@
 #include <AP_Baro/AP_Baro.h>
 #include <AP_Airspeed/AP_Airspeed.h>
 #include <AP_RangeFinder/AP_RangeFinder.h>
+#include <AP_MSP/AP_MSP.h>
+#include <AP_MSP/msp.h>
 #include "../AP_Bootloader/app_comms.h"
 #include "hwing_esc.h"
 
@@ -51,6 +53,16 @@ class AP_Periph_FW {
     AP_Baro baro;
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_MSP
+    struct {
+        AP_MSP msp;
+        MSP::msp_port_t port;
+        uint32_t last_gps_ms;
+    } msp;
+    void msp_init(AP_HAL::UARTDriver *_uart);
+    void can_msp_update(void);
+#endif
+
 #ifdef HAL_PERIPH_ENABLE_ADSB
     void adsb_init();
     void adsb_update();

Tools/AP_Periph/can.cpp

@@ -1180,6 +1180,9 @@ void AP_Periph_FW::can_update()
 #ifdef HAL_PERIPH_ENABLE_HWESC
     hwesc_telem_update();
 #endif
+#ifdef HAL_PERIPH_ENABLE_MSP
+    can_msp_update();
+#endif
 
     processTx();
     processRx();

Tools/AP_Periph/msp.cpp

@@ -0,0 +1,106 @@
+/*
+  output MSP protocol from AP_Periph for ArduPilot and INAV
+  Thanks to input from Konstantin Sharlaimov
+ */
+
+#include "AP_Periph.h"
+
+#ifdef HAL_PERIPH_ENABLE_MSP
+
+void AP_Periph_FW::msp_init(AP_HAL::UARTDriver *_uart)
+{
+    msp.port.uart = _uart;
+    msp.port.msp_version = MSP::MSP_V2_NATIVE;
+    _uart->begin(115200, 512, 512);
+}
+
+void AP_Periph_FW::can_msp_update(void)
+{
+    struct PACKED {
+        uint32_t msTOW;
+        uint8_t  fixType;
+        uint8_t  satellitesInView;
+        uint16_t horizontalPosAccuracy;     // [cm]
+        uint16_t verticalPosAccuracy;       // [cm]
+        uint16_t horizontalVelAccuracy;     // [cm/s]
+        uint16_t hdop;
+        int32_t  longitude;
+        int32_t  latitude;
+        int32_t  mslAltitude;       // cm
+        int32_t  nedVelNorth;       // cm/s
+        int32_t  nedVelEast;
+        int32_t  nedVelDown;
+        int16_t  groundCourse;      // deg * 100
+        int16_t  trueYaw;           // deg * 100, values of 0..36000 are valid. 65535 = no data available
+        uint16_t year;
+        uint8_t  month;
+        uint8_t  day;
+        uint8_t  hour;
+        uint8_t  min;
+        uint8_t  sec;
+    } p;
+
+    if (gps.get_type(0) == AP_GPS::GPS_Type::GPS_TYPE_NONE) {
+        return;
+    }
+    gps.update();
+    if (msp.last_gps_ms == gps.last_message_time_ms(0)) {
+        return;
+    }
+    msp.last_gps_ms = gps.last_message_time_ms(0);
+
+    const Location &loc = gps.location(0);
+    const Vector3f &vel = gps.velocity(0);
+
+    p.msTOW = gps.get_itow(0);
+    p.fixType = uint8_t(gps.status(0));
+    p.satellitesInView = gps.num_sats(0);
+
+    float hacc=0, vacc=0, sacc=0;
+    gps.horizontal_accuracy(0, hacc);
+    gps.vertical_accuracy(0, vacc);
+    gps.speed_accuracy(0, sacc);
+
+    p.horizontalVelAccuracy = sacc*100;
+    p.horizontalPosAccuracy = hacc*100;
+    p.verticalPosAccuracy = vacc*100;
+    p.hdop = gps.get_hdop(0);
+    p.longitude = loc.lng;
+    p.latitude = loc.lat;
+    p.mslAltitude = loc.alt;
+    p.nedVelNorth = vel.x*100;
+    p.nedVelEast = vel.y*100;
+    p.nedVelDown = vel.z*100;
+    p.groundCourse = gps.ground_course(0);
+    float yaw_deg=0, acc;
+    if (gps.gps_yaw_deg(0, yaw_deg, acc)) {
+        p.trueYaw = yaw_deg*100;
+    } else {
+        p.trueYaw = 65535; // unknown
+    }
+    uint64_t tepoch_us = gps.time_epoch_usec(0);
+    time_t utc_sec = tepoch_us / (1000U * 1000U);
+    struct tm* tm = gmtime(&utc_sec);
+
+    p.year = tm->tm_year+1900;
+    p.month = tm->tm_mon;
+    p.day = tm->tm_mday;
+    p.hour = tm->tm_hour;
+    p.min = tm->tm_min;
+    p.sec = tm->tm_sec;
+
+    uint8_t out_buf[MSP_PORT_OUTBUF_SIZE] {};
+    MSP::msp_packet_t pkt = {
+        .buf = { .ptr = out_buf, .end = MSP_ARRAYEND(out_buf), },
+        .cmd = (int16_t)MSP2_SENSOR_GPS,
+        .flags = 0,
+        .result = 0,
+    };
+
+    sbuf_write_data(&pkt.buf, &p, sizeof(p));
+    sbuf_switch_to_reader(&pkt.buf, &out_buf[0]);
+
+    MSP::msp_serial_encode(&msp.port, &pkt, MSP::MSP_V2_NATIVE, true);
+}
+
+#endif // HAL_PERIPH_ENABLE_MSP

Tools/AP_Periph/wscript

@@ -34,6 +34,7 @@ def build(bld):
         'AP_Airspeed',
         'AP_RangeFinder',
         'AP_ROMFS',
+        'AP_MSP',
         ],
         exclude_src=[
             'libraries/AP_HAL_ChibiOS/Storage.cpp'

libraries/AP_GPS/AP_GPS.cpp

@@ -1795,6 +1795,15 @@ bool AP_GPS::logging_failed(void) const {
     return false;
 }
 
+// get iTOW, if supported, zero otherwie
+uint32_t AP_GPS::get_itow(uint8_t instance) const
+{
+    if (instance >= GPS_MAX_RECEIVERS || drivers[instance] == nullptr) {
+        return 0;
+    }
+    return drivers[instance]->get_last_itow();
+}
+
 namespace AP {
 
 AP_GPS &gps()

libraries/AP_GPS/AP_GPS.h

@@ -482,6 +482,9 @@ class AP_GPS
         return instance>=GPS_MAX_RECEIVERS? GPS_Type::GPS_TYPE_NONE : GPS_Type(_type[instance].get());
     }
 
+    // get iTOW, if supported, zero otherwie
+    uint32_t get_itow(uint8_t instance) const;
+
 protected:
 
     // configuration parameters

libraries/AP_GPS/GPS_Backend.h

@@ -71,6 +71,11 @@ class AP_GPS_Backend
     virtual bool get_RTCMV3(const uint8_t *&bytes, uint16_t &len) { return false; }
     virtual void clear_RTCMV3(void) {};
 
+    // return iTOW of last message, or zero if not supported
+    uint32_t get_last_itow(void) const {
+        return _last_itow;
+    }
+
 protected:
     AP_HAL::UARTDriver *port;           ///< UART we are attached to
     AP_GPS &gps;                        ///< access to frontend (for parameters)

libraries/AP_HAL_ChibiOS/hwdef/f303-MatekGPS/hwdef.dat

@@ -130,8 +130,8 @@ PA3 USART2_RX USART2 SPEED_HIGH NODMA
 PB10  USART3_TX USART3 SPEED_HIGH NODMA
 PB11  USART3_RX USART3 SPEED_HIGH NODMA
 
-# use DNA
-define HAL_CAN_DEFAULT_NODE_ID 0
+# use a default node ID so this works with MSP only
+define HAL_CAN_DEFAULT_NODE_ID 112
 
 define CAN_APP_NODE_NAME "org.ardupilot.f303_MatekGPS"
 
@@ -148,8 +148,11 @@ define HAL_PERIPH_ENABLE_MAG
 define HAL_PERIPH_ENABLE_BARO
 define HAL_PERIPH_ENABLE_AIRSPEED
 define HAL_PERIPH_ENABLE_RANGEFINDER
+define HAL_PERIPH_ENABLE_MSP
 
 # allow for rangefinder to be plugged in on "MSP" port
 define RANGEFINDER_MAX_INSTANCES 1
 define ADSB_PORT hal.uartC
 define HAL_PERIPH_ENABLE_ADSB
+
+define HAL_MSP_ENABLED 1

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