AP_Compass: added ExternalAHRS driver

libraries/AP_Compass/AP_Compass.cpp

@@ -6,6 +6,7 @@
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Logger/AP_Logger.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
+#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
 
 #include "AP_Compass_SITL.h"
 #include "AP_Compass_AK8963.h"
@@ -29,6 +30,9 @@
 #if HAL_MSP_COMPASS_ENABLED
 #include "AP_Compass_MSP.h"
 #endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+#include "AP_Compass_ExternalAHRS.h"
+#endif
 #include "AP_Compass.h"
 #include "Compass_learn.h"
 #include <stdio.h>
@@ -488,7 +492,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
     // @Param: TYPEMASK
     // @DisplayName: Compass disable driver type mask
     // @Description: This is a bitmask of driver types to disable. If a driver type is set in this mask then that driver will not try to find a sensor at startup
-    // @Bitmask: 0:HMC5883,1:LSM303D,2:AK8963,3:BMM150,4:LSM9DS1,5:LIS3MDL,6:AK09916,7:IST8310,8:ICM20948,9:MMC3416,11:UAVCAN,12:QMC5883,14:MAG3110,15:IST8308,16:RM3100,17:MSP
+    // @Bitmask: 0:HMC5883,1:LSM303D,2:AK8963,3:BMM150,4:LSM9DS1,5:LIS3MDL,6:AK09916,7:IST8310,8:ICM20948,9:MMC3416,11:UAVCAN,12:QMC5883,14:MAG3110,15:IST8308,16:RM3100,17:MSP,18:ExternalAHRS
     // @User: Advanced
     AP_GROUPINFO("TYPEMASK", 33, Compass, _driver_type_mask, 0),
 
@@ -1185,6 +1189,12 @@ void Compass::_detect_backends(void)
     }
 #endif
 
+#if HAL_EXTERNAL_AHRS_ENABLED
+    if (int8_t serial_port = AP::externalAHRS().get_port() >= 0) {
+        ADD_BACKEND(DRIVER_SERIAL, new AP_Compass_ExternalAHRS(serial_port));
+    }
+#endif
+    
 #if AP_FEATURE_BOARD_DETECT
     if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2) {
         // default to disabling LIS3MDL on pixhawk2 due to hardware issue
@@ -1194,7 +1204,6 @@ void Compass::_detect_backends(void)
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     ADD_BACKEND(DRIVER_SITL, new AP_Compass_SITL());
-    return;
 #endif
 
 #ifdef HAL_PROBE_EXTERNAL_I2C_COMPASSES
@@ -1998,6 +2007,18 @@ void Compass::handle_msp(const MSP::msp_compass_data_message_t &pkt)
 }
 #endif // HAL_MSP_COMPASS_ENABLED
 
+#if HAL_EXTERNAL_AHRS_ENABLED
+void Compass::handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt)
+{
+    if (!_driver_enabled(DRIVER_SERIAL)) {
+        return;
+    }
+    for (uint8_t i=0; i<_backend_count; i++) {
+        _backends[i]->handle_external(pkt);
+    }
+}
+#endif // HAL_EXTERNAL_AHRS_ENABLED
+
 // singleton instance
 Compass *Compass::_singleton;
 

libraries/AP_Compass/AP_Compass.h

@@ -9,6 +9,7 @@
 #include <AP_Param/AP_Param.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_MSP/msp.h>
+#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
 
 #include "AP_Compass_Backend.h"
 #include "Compass_PerMotor.h"
@@ -350,6 +351,10 @@ public:
     void handle_msp(const MSP::msp_compass_data_message_t &pkt);
 #endif
 
+#if HAL_EXTERNAL_AHRS_ENABLED
+    void handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt);
+#endif
+    
 private:
     static Compass *_singleton;
 
@@ -424,6 +429,7 @@ private:
         DRIVER_IST8308  =15,
 		DRIVER_RM3100   =16,
         DRIVER_MSP      =17,
+        DRIVER_SERIAL   =18,
     };
 
     bool _driver_enabled(enum DriverType driver_type);

libraries/AP_Compass/AP_Compass_Backend.h

@@ -69,6 +69,10 @@ public:
     virtual void handle_msp(const MSP::msp_compass_data_message_t &pkt) {}
 #endif
 
+#if HAL_EXTERNAL_AHRS_ENABLED
+    virtual void handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt) {}
+#endif
+    
 protected:
 
     /*

libraries/AP_Compass/AP_Compass_ExternalAHRS.cpp

@@ -0,0 +1,43 @@
+/*
+   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_ExternalAHRS.h"
+
+#if HAL_EXTERNAL_AHRS_ENABLED
+
+AP_Compass_ExternalAHRS::AP_Compass_ExternalAHRS(uint8_t port)
+{
+    auto devid = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL,port,0,0);
+    register_compass(devid, instance);
+
+    set_dev_id(instance, devid);
+    set_external(instance, true);
+}
+
+void AP_Compass_ExternalAHRS::handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt)
+{
+    Vector3f field = pkt.field;
+    accumulate_sample(field, instance);
+}
+
+void AP_Compass_ExternalAHRS::read(void)
+{
+    drain_accumulated_samples(instance);
+}
+
+#endif // HAL_EXTERNAL_AHRS_ENABLED
+
+

libraries/AP_Compass/AP_Compass_ExternalAHRS.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include "AP_Compass.h"
+#include "AP_Compass_Backend.h"
+#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
+
+#if HAL_EXTERNAL_AHRS_ENABLED
+
+class AP_Compass_ExternalAHRS : public AP_Compass_Backend
+{
+public:
+    AP_Compass_ExternalAHRS(uint8_t instance);
+
+    void read(void) override;
+
+private:
+    void handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt) override;
+    uint8_t instance;
+};
+
+#endif // HAL_EXTERNAL_AHRS_ENABLED
+