diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 3db1f8b08f..fbaf23248b 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #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 @@ -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; diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index cab230aba1..4ef1883c1b 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -9,6 +9,7 @@ #include #include #include +#include #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); diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h index 22a6b24881..44a2b9db21 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.h +++ b/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: /* diff --git a/libraries/AP_Compass/AP_Compass_ExternalAHRS.cpp b/libraries/AP_Compass/AP_Compass_ExternalAHRS.cpp new file mode 100644 index 0000000000..f28e2df55d --- /dev/null +++ b/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 . + */ + +#include +#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 + + diff --git a/libraries/AP_Compass/AP_Compass_ExternalAHRS.h b/libraries/AP_Compass/AP_Compass_ExternalAHRS.h new file mode 100644 index 0000000000..816a3e2adf --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_ExternalAHRS.h @@ -0,0 +1,22 @@ +#pragma once + +#include "AP_Compass.h" +#include "AP_Compass_Backend.h" +#include + +#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 +