diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 1882c04f4c..19c6dc701a 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/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 @@ -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<handle_msp(pkt); + } + } +} +#endif // HAL_MSP_COMPASS_ENABLED // singleton instance Compass *Compass::_singleton; diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 2b96141eca..cab230aba1 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -8,6 +8,7 @@ #include #include #include +#include #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 { diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h index a3e4545b74..863cc331ab 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.h +++ b/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: diff --git a/libraries/AP_Compass/AP_Compass_MSP.cpp b/libraries/AP_Compass/AP_Compass_MSP.cpp new file mode 100644 index 0000000000..cc85bb9676 --- /dev/null +++ b/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 . + */ + +#include +#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 + diff --git a/libraries/AP_Compass/AP_Compass_MSP.h b/libraries/AP_Compass/AP_Compass_MSP.h new file mode 100644 index 0000000000..704b68ab32 --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_MSP.h @@ -0,0 +1,22 @@ +#pragma once + +#include "AP_Compass.h" +#include "AP_Compass_Backend.h" +#include + +#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