AP_MSP added initial support for MSP gps, compass and baro.

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,22 @@ 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;
+        uint32_t last_baro_ms;
+        uint32_t last_mag_ms;
+    } msp;
+    void msp_init(AP_HAL::UARTDriver *_uart);
+    void msp_sensor_update(void);
+    void send_msp_packet(uint16_t cmd, void *p, uint16_t size);
+    void send_msp_GPS(void);
+    void send_msp_compass(void);
+    void send_msp_baro(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
+    msp_sensor_update();
+#endif
 
     processTx();
     processRx();

Tools/AP_Periph/msp.cpp

@@ -0,0 +1,163 @@
+/*
+  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);
+}
+
+
+/*
+  send a MSP packet
+ */
+void AP_Periph_FW::send_msp_packet(uint16_t cmd, void *p, uint16_t size)
+{
+    uint8_t out_buf[size+16] {};
+    MSP::msp_packet_t pkt = {
+        .buf = { .ptr = out_buf, .end = MSP_ARRAYEND(out_buf), },
+        .cmd = (int16_t)cmd,
+        .flags = 0,
+        .result = 0,
+    };
+
+    sbuf_write_data(&pkt.buf, p, size);
+    sbuf_switch_to_reader(&pkt.buf, &out_buf[0]);
+
+    MSP::msp_serial_encode(&msp.port, &pkt, MSP::MSP_V2_NATIVE, true);
+}
+
+/*
+  update MSP sensors
+ */
+void AP_Periph_FW::msp_sensor_update(void)
+{
+#ifdef HAL_PERIPH_ENABLE_GPS
+    send_msp_GPS();
+#endif
+#ifdef HAL_PERIPH_ENABLE_BARO
+    send_msp_baro();
+#endif
+#ifdef HAL_PERIPH_ENABLE_MAG
+    send_msp_compass();
+#endif
+}
+
+
+#ifdef HAL_PERIPH_ENABLE_GPS
+/*
+  send MSP GPS packet
+ */
+void AP_Periph_FW::send_msp_GPS(void)
+{
+    MSP::msp_gps_data_message_t p;
+
+    if (gps.get_type(0) == AP_GPS::GPS_Type::GPS_TYPE_NONE) {
+        return;
+    }
+    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.instance = 0;
+    p.gps_week = gps.time_week(0);
+    p.ms_tow = gps.get_itow(0);
+    p.fix_type = uint8_t(gps.status(0));
+    p.satellites_in_view = 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.horizontal_vel_accuracy = sacc*100;
+    p.horizontal_pos_accuracy = hacc*100;
+    p.vertical_pos_accuracy = vacc*100;
+    p.hdop = gps.get_hdop(0);
+    p.longitude = loc.lng;
+    p.latitude = loc.lat;
+    p.msl_altitude = loc.alt;
+    p.ned_vel_north = vel.x*100;
+    p.ned_vel_east = vel.y*100;
+    p.ned_vel_down = vel.z*100;
+    p.ground_course = wrap_360_cd(gps.ground_course(0)*100);
+    float yaw_deg=0, acc;
+    if (gps.gps_yaw_deg(0, yaw_deg, acc)) {
+        p.true_yaw = wrap_360_cd(yaw_deg*100);
+    } else {
+        p.true_yaw = 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;
+
+    send_msp_packet(MSP2_SENSOR_GPS, &p, sizeof(p));
+}
+#endif // HAL_PERIPH_ENABLE_GPS
+
+
+#ifdef HAL_PERIPH_ENABLE_BARO
+/*
+  send MSP baro packet
+ */
+void AP_Periph_FW::send_msp_baro(void)
+{
+    MSP::msp_baro_data_message_t p;
+
+    if (msp.last_baro_ms == baro.get_last_update(0)) {
+        return;
+    }
+    msp.last_baro_ms = baro.get_last_update(0);
+
+    p.instance = 0;
+    p.time_ms = msp.last_baro_ms;
+    p.pressure_pa = baro.get_pressure();
+    p.temp = baro.get_temperature() * 100;
+
+    send_msp_packet(MSP2_SENSOR_BAROMETER, &p, sizeof(p));
+}
+#endif // HAL_PERIPH_ENABLE_BARO
+
+#ifdef HAL_PERIPH_ENABLE_MAG
+/*
+  send MSP compass packet
+ */
+void AP_Periph_FW::send_msp_compass(void)
+{
+    MSP::msp_compass_data_message_t p;
+
+    if (msp.last_mag_ms == compass.last_update_ms(0)) {
+        return;
+    }
+    msp.last_mag_ms = compass.last_update_ms(0);
+
+    const Vector3f &field = compass.get_field(0);
+    p.instance = 0;
+    p.time_ms = msp.last_mag_ms;
+    p.magX = field.x;
+    p.magY = field.y;
+    p.magZ = field.z;
+
+    send_msp_packet(MSP2_SENSOR_COMPASS, &p, sizeof(p));
+}
+#endif // HAL_PERIPH_ENABLE_MAG
+
+#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'

Tools/scripts/decode_devid.py

@@ -38,7 +38,8 @@ def num(s):
     1: "I2C",
     2: "SPI",
     3: "UAVCAN",
-    4: "SITL"
+    4: "SITL",
+    5: "MSP"
 }
 
 compass_types = {

libraries/AP_Baro/AP_Baro.cpp

@@ -46,6 +46,7 @@
 #if HAL_ENABLE_LIBUAVCAN_DRIVERS
 #include "AP_Baro_UAVCAN.h"
 #endif
+#include "AP_Baro_MSP.h"
 
 #include <AP_Airspeed/AP_Airspeed.h>
 #include <AP_AHRS/AP_AHRS.h>
@@ -162,12 +163,12 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = {
     // @Increment: 1
     AP_GROUPINFO("FLTR_RNG", 13, AP_Baro, _filter_range, HAL_BARO_FILTER_DEFAULT),
 
-#ifdef HAL_PROBE_EXTERNAL_I2C_BAROS
+#if defined(HAL_PROBE_EXTERNAL_I2C_BAROS) || defined(HAL_MSP_BARO_ENABLED)
     // @Param: PROBE_EXT
     // @DisplayName: External barometers to probe
     // @Description: This sets which types of external i2c barometer to look for. It is a bitmask of barometer types. The I2C buses to probe is based on GND_EXT_BUS. If GND_EXT_BUS is -1 then it will probe all external buses, otherwise it will probe just the bus number given in GND_EXT_BUS.
-    // @Bitmask: 0:BMP085,1:BMP280,2:MS5611,3:MS5607,4:MS5637,5:FBM320,6:DPS280,7:LPS25H,8:Keller,9:MS5837,10:BMP388,11:SPL06
-    // @Values: 1:BMP085,2:BMP280,4:MS5611,8:MS5607,16:MS5637,32:FBM320,64:DPS280,128:LPS25H,256:Keller,512:MS5837,1024:BMP388,2048:SPL06
+    // @Bitmask: 0:BMP085,1:BMP280,2:MS5611,3:MS5607,4:MS5637,5:FBM320,6:DPS280,7:LPS25H,8:Keller,9:MS5837,10:BMP388,11:SPL06,12:MSP
+    // @Values: 1:BMP085,2:BMP280,4:MS5611,8:MS5607,16:MS5637,32:FBM320,64:DPS280,128:LPS25H,256:Keller,512:MS5837,1024:BMP388,2048:SPL06,4096:MSP
     // @User: Advanced
     AP_GROUPINFO("PROBE_EXT", 14, AP_Baro, _baro_probe_ext, HAL_BARO_PROBE_EXT_DEFAULT),
 #endif
@@ -493,6 +494,8 @@ bool AP_Baro::_add_backend(AP_Baro_Backend *backend)
  */
 void AP_Baro::init(void)
 {
+    init_done = true;
+
     // ensure that there isn't a previous ground temperature saved
     if (!is_zero(_user_ground_temperature)) {
         _user_ground_temperature.set_and_save(0.0f);
@@ -641,6 +644,18 @@ void AP_Baro::init(void)
     _probe_i2c_barometers();
 #endif
 
+#if HAL_MSP_BARO_ENABLED
+    if ((_baro_probe_ext.get() & PROBE_MSP) && msp_instance_mask == 0) {
+        // allow for late addition of MSP sensor
+        msp_instance_mask |= 1;
+    }
+    for (uint8_t i=0; i<8; i++) {
+        if (msp_instance_mask & (1U<<i)) {
+            ADD_BACKEND(new AP_Baro_MSP(*this, i));
+        }
+    }
+#endif
+
 #if !defined(HAL_BARO_ALLOW_INIT_NO_BARO) // most boards requires external baro
 
     if (_num_drivers == 0 || _num_sensors == 0 || drivers[0] == nullptr) {
@@ -901,6 +916,26 @@ void AP_Baro::set_pressure_correction(uint8_t instance, float p_correction)
     }
 }
 
+#if HAL_MSP_BARO_ENABLED
+/*
+  handle MSP barometer data
+ */
+void AP_Baro::handle_msp(const MSP::msp_baro_data_message_t &pkt)
+{
+    if (pkt.instance > 7) {
+        return;
+    }
+    if (!init_done) {
+        msp_instance_mask |= 1U<<pkt.instance;
+    } else if (msp_instance_mask != 0) {
+        for (uint8_t i=0; i<_num_drivers; i++) {
+            drivers[i]->handle_msp(pkt);
+        }
+    }
+}
+#endif 
+
+
 namespace AP {
 
 AP_Baro &baro()

libraries/AP_Baro/AP_Baro.h

@@ -4,6 +4,11 @@
 #include <AP_Param/AP_Param.h>
 #include <Filter/Filter.h>
 #include <Filter/DerivativeFilter.h>
+#include <AP_MSP/msp.h>
+
+#ifndef HAL_MSP_BARO_ENABLED
+#define HAL_MSP_BARO_ENABLED HAL_MSP_ENABLED && !HAL_MINIMIZE_FEATURES
+#endif
 
 // maximum number of sensor instances
 #define BARO_MAX_INSTANCES 3
@@ -188,7 +193,11 @@ class AP_Baro
     HAL_Semaphore &get_semaphore(void) {
         return _rsem;
     }
-
+
+#if HAL_MSP_BARO_ENABLED
+    void handle_msp(const MSP::msp_baro_data_message_t &pkt);
+#endif
+
 private:
     // singleton
     static AP_Baro *_singleton;
@@ -205,6 +214,12 @@ class AP_Baro
 
     uint32_t _log_baro_bit = -1;
 
+    bool init_done;
+
+#if HAL_MSP_BARO_ENABLED
+    uint8_t msp_instance_mask;
+#endif
+
     // bitmask values for GND_PROBE_EXT
     enum {
         PROBE_BMP085=(1<<0),
@@ -218,7 +233,8 @@ class AP_Baro
         PROBE_KELLER=(1<<8),
         PROBE_MS5837=(1<<9),
         PROBE_BMP388=(1<<10),
-        PROBE_SPL06=(1<<11),
+        PROBE_SPL06 =(1<<11),
+        PROBE_MSP   =(1<<12),
     };
 
     struct sensor {

libraries/AP_Baro/AP_Baro_Backend.h

@@ -24,6 +24,10 @@ class AP_Baro_Backend
     bool pressure_ok(float press);
     uint32_t get_error_count() const { return _error_count; }
 
+#if HAL_MSP_BARO_ENABLED
+    virtual void handle_msp(const MSP::msp_baro_data_message_t &pkt) {}
+#endif 
+
     /*
       device driver IDs. These are used to fill in the devtype field
       of the device ID, which shows up as GND_BARO_ID* parameters to
@@ -43,6 +47,7 @@ class AP_Baro_Backend
         DEVTYPE_BARO_MS5611   = 0x0B,
         DEVTYPE_BARO_SPL06    = 0x0C,
         DEVTYPE_BARO_UAVCAN   = 0x0D,
+        DEVTYPE_BARO_MSP      = 0x0E,
     };
 
 protected: