AP_Compass: support for MSP compass

libraries/AP_Compass/AP_Compass.cpp

@@ -26,6 +26,9 @@
 #include "AP_Compass_MMC3416.h"
 #include "AP_Compass_MAG3110.h"
 #include "AP_Compass_RM3100.h"
+#if HAL_MSP_COMPASS_ENABLED
+#include "AP_Compass_MSP.h"
+#endif
 #include "AP_Compass.h"
 #include "Compass_learn.h"
 #include <stdio.h>
@@ -774,6 +777,8 @@ void Compass::init()
 #ifndef HAL_BUILD_AP_PERIPH
     AP::ahrs().set_compass(this);
 #endif
+
+    init_done = true;
 }
 
 #if COMPASS_MAX_INSTANCES > 1 || COMPASS_MAX_UNREG_DEV
@@ -1172,6 +1177,14 @@ void Compass::_detect_backends(void)
     CHECK_UNREG_LIMIT_RETURN;
 #endif
 
+#if HAL_MSP_COMPASS_ENABLED
+    for (uint8_t i=0; i<8; i++) {
+        if (msp_instance_mask & (1U<<i)) {
+            ADD_BACKEND(DRIVER_MSP, new AP_Compass_MSP(i));
+        }
+    }
+#endif
+
 #if defined(HAL_MAG_PROBE_LIST)
     // driver probes defined by COMPASS lines in hwdef.dat
     HAL_MAG_PROBE_LIST;
@@ -1941,6 +1954,23 @@ bool Compass::have_scale_factor(uint8_t i) const
     return true;
 }
 
+#if HAL_MSP_COMPASS_ENABLED
+void Compass::handle_msp(const MSP::msp_compass_data_message_t &pkt)
+{
+    if (!_driver_enabled(DRIVER_MSP)) {
+        return;
+    }
+    if (!init_done) {
+        if (pkt.instance < 8) {
+            msp_instance_mask |= 1U<<pkt.instance;
+        }
+    } else {
+        for (uint8_t i=0; i<_backend_count; i++) {
+            _backends[i]->handle_msp(pkt);
+        }
+    }
+}
+#endif // HAL_MSP_COMPASS_ENABLED
 
 // singleton instance
 Compass *Compass::_singleton;

libraries/AP_Compass/AP_Compass.h

@@ -8,6 +8,7 @@
 #include <AP_Math/AP_Math.h>
 #include <AP_Param/AP_Param.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
+#include <AP_MSP/msp.h>
 
 #include "AP_Compass_Backend.h"
 #include "Compass_PerMotor.h"
@@ -345,6 +346,10 @@ public:
     MAV_RESULT mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
                                  float lat_deg, float lon_deg);
 
+#if HAL_MSP_COMPASS_ENABLED
+    void handle_msp(const MSP::msp_compass_data_message_t &pkt);
+#endif
+
 private:
     static Compass *_singleton;
 
@@ -416,8 +421,9 @@ private:
         DRIVER_QMC5883L =12,
         DRIVER_SITL     =13,
         DRIVER_MAG3110  =14,
-        DRIVER_IST8308  = 15,
+        DRIVER_IST8308  =15,
 		DRIVER_RM3100   =16,
+        DRIVER_MSP      =17,
     };
 
     bool _driver_enabled(enum DriverType driver_type);
@@ -597,6 +603,11 @@ private:
     bool _initial_location_set;
 
     bool _cal_thread_started;
+
+#if HAL_MSP_COMPASS_ENABLED
+    uint8_t msp_instance_mask;
+#endif
+    bool init_done;
 };
 
 namespace AP {

libraries/AP_Compass/AP_Compass_Backend.h

@@ -21,6 +21,10 @@
 
 #include "AP_Compass.h"
 
+#ifndef HAL_MSP_COMPASS_ENABLED
+#define HAL_MSP_COMPASS_ENABLED HAL_MSP_ENABLED && !HAL_MINIMIZE_FEATURES
+#endif
+
 class Compass;  // forward declaration
 class AP_Compass_Backend
 {
@@ -61,6 +65,9 @@ public:
         DEVTYPE_RM3100 = 0x11,
     };
 
+#if HAL_MSP_COMPASS_ENABLED
+    virtual void handle_msp(const MSP::msp_compass_data_message_t &pkt) {}
+#endif
 
 protected:
 

libraries/AP_Compass/AP_Compass_MSP.cpp

@@ -0,0 +1,47 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#include "AP_Compass_MSP.h"
+
+#if HAL_MSP_COMPASS_ENABLED
+
+AP_Compass_MSP::AP_Compass_MSP(uint8_t _msp_instance)
+{
+    msp_instance = _msp_instance;
+
+    auto devid = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_MSP, 0, _msp_instance, 0);
+    register_compass(devid, instance);
+
+    set_dev_id(instance, devid);
+    set_external(instance, true);
+}
+
+void AP_Compass_MSP::handle_msp(const MSP::msp_compass_data_message_t &pkt)
+{
+    if (pkt.instance != msp_instance) {
+        return;
+    }
+    Vector3f field(pkt.magX, pkt.magY, pkt.magZ);
+    accumulate_sample(field, instance);
+}
+
+void AP_Compass_MSP::read(void)
+{
+    drain_accumulated_samples(instance);
+}
+
+#endif // HAL_MSP_COMPASS_ENABLED
+

libraries/AP_Compass/AP_Compass_MSP.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include "AP_Compass.h"
+#include "AP_Compass_Backend.h"
+#include <AP_MSP/msp.h>
+
+#if HAL_MSP_COMPASS_ENABLED
+
+class AP_Compass_MSP : public AP_Compass_Backend
+{
+public:
+    AP_Compass_MSP(uint8_t msp_instance);
+
+    void read(void) override;
+
+private:
+    void handle_msp(const MSP::msp_compass_data_message_t &pkt) override;
+    uint8_t msp_instance;
+    uint8_t instance;
+};
+
+#endif // HAL_MSP_COMPASS_ENABLED