From 360b771ca888836d70cc9c4d65eafd36f4eefcfd Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 27 Nov 2025 14:58:44 +1100 Subject: [PATCH 1/2] AP_Compass: all backends take a generic Device rather than an I2CDevice --- libraries/AP_Compass/AP_Compass.cpp | 1 + libraries/AP_Compass/AP_Compass_AK09916.cpp | 10 +++++----- libraries/AP_Compass/AP_Compass_AK09916.h | 12 ++++++------ libraries/AP_Compass/AP_Compass_AK8963.cpp | 8 ++++---- libraries/AP_Compass/AP_Compass_AK8963.h | 11 +++++------ libraries/AP_Compass/AP_Compass_BMM150.cpp | 2 +- libraries/AP_Compass/AP_Compass_BMM150.h | 4 ++-- libraries/AP_Compass/AP_Compass_BMM350.h | 2 +- libraries/AP_Compass/AP_Compass_IIS2MDC.cpp | 2 +- libraries/AP_Compass/AP_Compass_IIS2MDC.h | 4 ++-- libraries/AP_Compass/AP_Compass_IST8308.cpp | 2 +- libraries/AP_Compass/AP_Compass_IST8308.h | 4 ++-- libraries/AP_Compass/AP_Compass_IST8310.cpp | 2 +- libraries/AP_Compass/AP_Compass_IST8310.h | 4 ++-- libraries/AP_Compass/AP_Compass_MMC3416.cpp | 2 +- libraries/AP_Compass/AP_Compass_MMC3416.h | 4 ++-- libraries/AP_Compass/AP_Compass_MMC5xx3.h | 2 +- libraries/AP_Compass/AP_Compass_QMC5883L.cpp | 2 +- libraries/AP_Compass/AP_Compass_QMC5883L.h | 4 ++-- libraries/AP_Compass/AP_Compass_QMC5883P.cpp | 2 +- libraries/AP_Compass/AP_Compass_QMC5883P.h | 4 ++-- 21 files changed, 44 insertions(+), 44 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 30f240120029c0..ae7ce50e9e4b03 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -3,6 +3,7 @@ #if AP_COMPASS_ENABLED #include +#include #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX #include #endif diff --git a/libraries/AP_Compass/AP_Compass_AK09916.cpp b/libraries/AP_Compass/AP_Compass_AK09916.cpp index ec5d2ab037bf43..a4d347dd76e86e 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.cpp +++ b/libraries/AP_Compass/AP_Compass_AK09916.cpp @@ -72,7 +72,7 @@ AP_Compass_AK09916::~AP_Compass_AK09916() delete _bus; } -AP_Compass_Backend *AP_Compass_AK09916::probe(AP_HAL::OwnPtr dev, +AP_Compass_Backend *AP_Compass_AK09916::probe(AP_HAL::OwnPtr dev, bool force_external, enum Rotation rotation) { @@ -94,8 +94,8 @@ AP_Compass_Backend *AP_Compass_AK09916::probe(AP_HAL::OwnPtr } #if AP_COMPASS_ICM20948_ENABLED -AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(AP_HAL::OwnPtr dev, - AP_HAL::OwnPtr dev_icm, +AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(AP_HAL::OwnPtr dev, + AP_HAL::OwnPtr dev_icm, bool force_external, enum Rotation rotation) { @@ -362,8 +362,8 @@ bool AP_Compass_AK09916::_reset() return _bus->register_write(REG_CNTL3, 0x01); //Soft Reset } -/* AP_HAL::I2CDevice implementation of the AK09916 */ -AP_AK09916_BusDriver_HALDevice::AP_AK09916_BusDriver_HALDevice(AP_HAL::OwnPtr dev) +/* AP_HAL::Device implementation of the AK09916 */ +AP_AK09916_BusDriver_HALDevice::AP_AK09916_BusDriver_HALDevice(AP_HAL::OwnPtr dev) : _dev(std::move(dev)) { } diff --git a/libraries/AP_Compass/AP_Compass_AK09916.h b/libraries/AP_Compass/AP_Compass_AK09916.h index 817c45297c51ea..c34232d829a91a 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.h +++ b/libraries/AP_Compass/AP_Compass_AK09916.h @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include "AP_Compass.h" @@ -48,14 +48,14 @@ class AP_Compass_AK09916 : public AP_Compass_Backend { public: /* Probe for AK09916 standalone on I2C bus */ - static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, + static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, bool force_external, enum Rotation rotation); #if AP_COMPASS_ICM20948_ENABLED /* Probe for AK09916 on auxiliary bus of ICM20948, connected through I2C */ - static AP_Compass_Backend *probe_ICM20948(AP_HAL::OwnPtr dev, - AP_HAL::OwnPtr dev_icm, + static AP_Compass_Backend *probe_ICM20948(AP_HAL::OwnPtr dev, + AP_HAL::OwnPtr dev_icm, bool force_external, enum Rotation rotation); @@ -141,7 +141,7 @@ class AP_AK09916_BusDriver class AP_AK09916_BusDriver_HALDevice: public AP_AK09916_BusDriver { public: - AP_AK09916_BusDriver_HALDevice(AP_HAL::OwnPtr dev); + AP_AK09916_BusDriver_HALDevice(AP_HAL::OwnPtr dev); virtual bool block_read(uint8_t reg, uint8_t *buf, uint32_t size) override; virtual bool register_read(uint8_t reg, uint8_t *val) override; @@ -173,7 +173,7 @@ class AP_AK09916_BusDriver_HALDevice: public AP_AK09916_BusDriver } private: - AP_HAL::OwnPtr _dev; + AP_HAL::OwnPtr _dev; }; class AP_AK09916_BusDriver_Auxiliary : public AP_AK09916_BusDriver diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index a17b2fbdca88fa..e450913a91dec4 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -63,7 +63,7 @@ AP_Compass_AK8963::~AP_Compass_AK8963() delete _bus; } -AP_Compass_Backend *AP_Compass_AK8963::probe(AP_HAL::OwnPtr dev, +AP_Compass_Backend *AP_Compass_AK8963::probe(AP_HAL::OwnPtr dev, enum Rotation rotation) { if (!dev) { @@ -83,7 +83,7 @@ AP_Compass_Backend *AP_Compass_AK8963::probe(AP_HAL::OwnPtr d return sensor; } -AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(AP_HAL::OwnPtr dev, +AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(AP_HAL::OwnPtr dev, enum Rotation rotation) { if (!dev) { @@ -271,8 +271,8 @@ bool AP_Compass_AK8963::_calibrate() return true; } -/* AP_HAL::I2CDevice implementation of the AK8963 */ -AP_AK8963_BusDriver_HALDevice::AP_AK8963_BusDriver_HALDevice(AP_HAL::OwnPtr dev) +/* AP_HAL::Device implementation of the AK8963 */ +AP_AK8963_BusDriver_HALDevice::AP_AK8963_BusDriver_HALDevice(AP_HAL::OwnPtr dev) : _dev(std::move(dev)) { } diff --git a/libraries/AP_Compass/AP_Compass_AK8963.h b/libraries/AP_Compass/AP_Compass_AK8963.h index b5d092d53b2dbe..17f23190ce2e2b 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.h +++ b/libraries/AP_Compass/AP_Compass_AK8963.h @@ -6,8 +6,7 @@ #include #include -#include -#include +#include #include #include "AP_Compass.h" @@ -22,11 +21,11 @@ class AP_Compass_AK8963 : public AP_Compass_Backend { public: /* Probe for AK8963 standalone on I2C bus */ - static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, + static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, enum Rotation rotation); /* Probe for AK8963 on auxiliary bus of MPU9250, connected through I2C */ - static AP_Compass_Backend *probe_mpu9250(AP_HAL::OwnPtr dev, + static AP_Compass_Backend *probe_mpu9250(AP_HAL::OwnPtr dev, enum Rotation rotation); /* Probe for AK8963 on auxiliary bus of MPU9250, connected through SPI */ @@ -93,7 +92,7 @@ class AP_AK8963_BusDriver class AP_AK8963_BusDriver_HALDevice: public AP_AK8963_BusDriver { public: - AP_AK8963_BusDriver_HALDevice(AP_HAL::OwnPtr dev); + AP_AK8963_BusDriver_HALDevice(AP_HAL::OwnPtr dev); virtual bool block_read(uint8_t reg, uint8_t *buf, uint32_t size) override; virtual bool register_read(uint8_t reg, uint8_t *val) override; @@ -113,7 +112,7 @@ class AP_AK8963_BusDriver_HALDevice: public AP_AK8963_BusDriver } private: - AP_HAL::OwnPtr _dev; + AP_HAL::OwnPtr _dev; }; class AP_AK8963_BusDriver_Auxiliary : public AP_AK8963_BusDriver diff --git a/libraries/AP_Compass/AP_Compass_BMM150.cpp b/libraries/AP_Compass/AP_Compass_BMM150.cpp index 11ae92b70a3b84..e9af8ae73e7e9c 100644 --- a/libraries/AP_Compass/AP_Compass_BMM150.cpp +++ b/libraries/AP_Compass/AP_Compass_BMM150.cpp @@ -65,7 +65,7 @@ extern const AP_HAL::HAL &hal; -AP_Compass_Backend *AP_Compass_BMM150::probe(AP_HAL::OwnPtr dev, bool force_external, enum Rotation rotation) +AP_Compass_Backend *AP_Compass_BMM150::probe(AP_HAL::OwnPtr dev, bool force_external, enum Rotation rotation) { if (!dev) { return nullptr; diff --git a/libraries/AP_Compass/AP_Compass_BMM150.h b/libraries/AP_Compass/AP_Compass_BMM150.h index 56eb25827d65f3..3e3498b3398518 100644 --- a/libraries/AP_Compass/AP_Compass_BMM150.h +++ b/libraries/AP_Compass/AP_Compass_BMM150.h @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include "AP_Compass.h" @@ -34,7 +34,7 @@ class AP_Compass_BMM150 : public AP_Compass_Backend { public: - static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, bool force_external, enum Rotation rotation); + static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, bool force_external, enum Rotation rotation); void read() override; diff --git a/libraries/AP_Compass/AP_Compass_BMM350.h b/libraries/AP_Compass/AP_Compass_BMM350.h index fd6878f8de09a5..b7f436de5dc54a 100644 --- a/libraries/AP_Compass/AP_Compass_BMM350.h +++ b/libraries/AP_Compass/AP_Compass_BMM350.h @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include "AP_Compass.h" diff --git a/libraries/AP_Compass/AP_Compass_IIS2MDC.cpp b/libraries/AP_Compass/AP_Compass_IIS2MDC.cpp index 8252c819cb26b0..d27ab1b64b7bef 100644 --- a/libraries/AP_Compass/AP_Compass_IIS2MDC.cpp +++ b/libraries/AP_Compass/AP_Compass_IIS2MDC.cpp @@ -43,7 +43,7 @@ extern const AP_HAL::HAL &hal; -AP_Compass_Backend *AP_Compass_IIS2MDC::probe(AP_HAL::OwnPtr dev, +AP_Compass_Backend *AP_Compass_IIS2MDC::probe(AP_HAL::OwnPtr dev, bool force_external, enum Rotation rotation) { diff --git a/libraries/AP_Compass/AP_Compass_IIS2MDC.h b/libraries/AP_Compass/AP_Compass_IIS2MDC.h index 27db5acf627dbd..54b2f3a756aa3d 100644 --- a/libraries/AP_Compass/AP_Compass_IIS2MDC.h +++ b/libraries/AP_Compass/AP_Compass_IIS2MDC.h @@ -20,7 +20,7 @@ #if AP_COMPASS_IIS2MDC_ENABLED #include -#include +#include #include "AP_Compass.h" #include "AP_Compass_Backend.h" @@ -40,7 +40,7 @@ class AP_Compass_IIS2MDC : public AP_Compass_Backend { public: - static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, + static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, bool force_external, enum Rotation rotation); diff --git a/libraries/AP_Compass/AP_Compass_IST8308.cpp b/libraries/AP_Compass/AP_Compass_IST8308.cpp index 57b41d0615f37a..53eb60ac814a47 100644 --- a/libraries/AP_Compass/AP_Compass_IST8308.cpp +++ b/libraries/AP_Compass/AP_Compass_IST8308.cpp @@ -81,7 +81,7 @@ extern const AP_HAL::HAL &hal; -AP_Compass_Backend *AP_Compass_IST8308::probe(AP_HAL::OwnPtr dev, +AP_Compass_Backend *AP_Compass_IST8308::probe(AP_HAL::OwnPtr dev, bool force_external, enum Rotation rotation) { diff --git a/libraries/AP_Compass/AP_Compass_IST8308.h b/libraries/AP_Compass/AP_Compass_IST8308.h index a55295295d43e0..730384d81a0ac1 100644 --- a/libraries/AP_Compass/AP_Compass_IST8308.h +++ b/libraries/AP_Compass/AP_Compass_IST8308.h @@ -22,7 +22,7 @@ #include #include -#include +#include #include "AP_Compass_Backend.h" @@ -33,7 +33,7 @@ class AP_Compass_IST8308 : public AP_Compass_Backend { public: - static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, + static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, bool force_external, enum Rotation rotation); diff --git a/libraries/AP_Compass/AP_Compass_IST8310.cpp b/libraries/AP_Compass/AP_Compass_IST8310.cpp index 27a411f0fd1f8f..67b15bf78574c7 100644 --- a/libraries/AP_Compass/AP_Compass_IST8310.cpp +++ b/libraries/AP_Compass/AP_Compass_IST8310.cpp @@ -78,7 +78,7 @@ static const int16_t IST8310_MIN_VAL_Z = -IST8310_MAX_VAL_Z; extern const AP_HAL::HAL &hal; -AP_Compass_Backend *AP_Compass_IST8310::probe(AP_HAL::OwnPtr dev, +AP_Compass_Backend *AP_Compass_IST8310::probe(AP_HAL::OwnPtr dev, bool force_external, enum Rotation rotation) { diff --git a/libraries/AP_Compass/AP_Compass_IST8310.h b/libraries/AP_Compass/AP_Compass_IST8310.h index 2ef19de48e5e18..6ae7d825f05c2f 100644 --- a/libraries/AP_Compass/AP_Compass_IST8310.h +++ b/libraries/AP_Compass/AP_Compass_IST8310.h @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include "AP_Compass_Backend.h" @@ -38,7 +38,7 @@ class AP_Compass_IST8310 : public AP_Compass_Backend { public: - static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, + static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, bool force_external, enum Rotation rotation); diff --git a/libraries/AP_Compass/AP_Compass_MMC3416.cpp b/libraries/AP_Compass/AP_Compass_MMC3416.cpp index f3aef33b01b711..d894a8e512d287 100644 --- a/libraries/AP_Compass/AP_Compass_MMC3416.cpp +++ b/libraries/AP_Compass/AP_Compass_MMC3416.cpp @@ -43,7 +43,7 @@ extern const AP_HAL::HAL &hal; // datasheet says 50ms min for refill #define MIN_DELAY_SET_RESET 50 -AP_Compass_Backend *AP_Compass_MMC3416::probe(AP_HAL::OwnPtr dev, +AP_Compass_Backend *AP_Compass_MMC3416::probe(AP_HAL::OwnPtr dev, bool force_external, enum Rotation rotation) { diff --git a/libraries/AP_Compass/AP_Compass_MMC3416.h b/libraries/AP_Compass/AP_Compass_MMC3416.h index c858a99116c89d..3c92a2668c8a26 100644 --- a/libraries/AP_Compass/AP_Compass_MMC3416.h +++ b/libraries/AP_Compass/AP_Compass_MMC3416.h @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include "AP_Compass.h" @@ -33,7 +33,7 @@ class AP_Compass_MMC3416 : public AP_Compass_Backend { public: - static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, + static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, bool force_external, enum Rotation rotation); diff --git a/libraries/AP_Compass/AP_Compass_MMC5xx3.h b/libraries/AP_Compass/AP_Compass_MMC5xx3.h index 7c3ba8228b951a..785efd0588b9ad 100644 --- a/libraries/AP_Compass/AP_Compass_MMC5xx3.h +++ b/libraries/AP_Compass/AP_Compass_MMC5xx3.h @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include "AP_Compass.h" diff --git a/libraries/AP_Compass/AP_Compass_QMC5883L.cpp b/libraries/AP_Compass/AP_Compass_QMC5883L.cpp index 804731d7613817..2d43b4fb8b20a1 100644 --- a/libraries/AP_Compass/AP_Compass_QMC5883L.cpp +++ b/libraries/AP_Compass/AP_Compass_QMC5883L.cpp @@ -57,7 +57,7 @@ #define QMC5883L_REG_ID 0x0D #define QMC5883_ID_VAL 0xFF -AP_Compass_Backend *AP_Compass_QMC5883L::probe(AP_HAL::OwnPtr dev, +AP_Compass_Backend *AP_Compass_QMC5883L::probe(AP_HAL::OwnPtr dev, bool force_external, enum Rotation rotation) { diff --git a/libraries/AP_Compass/AP_Compass_QMC5883L.h b/libraries/AP_Compass/AP_Compass_QMC5883L.h index 1fc1d7b4255157..ac4f9f912be40f 100644 --- a/libraries/AP_Compass/AP_Compass_QMC5883L.h +++ b/libraries/AP_Compass/AP_Compass_QMC5883L.h @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include "AP_Compass.h" @@ -46,7 +46,7 @@ class AP_Compass_QMC5883L : public AP_Compass_Backend { public: - static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, + static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, bool force_external, enum Rotation rotation); diff --git a/libraries/AP_Compass/AP_Compass_QMC5883P.cpp b/libraries/AP_Compass/AP_Compass_QMC5883P.cpp index f30c9f7dfb5792..8fa1dbd3fad7b2 100644 --- a/libraries/AP_Compass/AP_Compass_QMC5883P.cpp +++ b/libraries/AP_Compass/AP_Compass_QMC5883P.cpp @@ -73,7 +73,7 @@ #define DEBUG 0 #endif -AP_Compass_Backend *AP_Compass_QMC5883P::probe(AP_HAL::OwnPtr dev, +AP_Compass_Backend *AP_Compass_QMC5883P::probe(AP_HAL::OwnPtr dev, bool force_external, enum Rotation rotation) { diff --git a/libraries/AP_Compass/AP_Compass_QMC5883P.h b/libraries/AP_Compass/AP_Compass_QMC5883P.h index 9e68353080ca71..32df741446c896 100644 --- a/libraries/AP_Compass/AP_Compass_QMC5883P.h +++ b/libraries/AP_Compass/AP_Compass_QMC5883P.h @@ -17,7 +17,7 @@ #pragma once #include -#include +#include #include #include "AP_Compass_config.h" @@ -45,7 +45,7 @@ class AP_Compass_QMC5883P : public AP_Compass_Backend { public: - static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, + static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, bool force_external, enum Rotation rotation); From 08b993026662800e0296e190a3d31d5ab48a90a7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 26 Nov 2025 22:38:13 +1100 Subject: [PATCH 2/2] AP_Compass: add some helper functions for compass probing ... and stop leaking memory if the device isn't enabled --- libraries/AP_Compass/AP_Compass.cpp | 196 +++++++++++++++++----------- libraries/AP_Compass/AP_Compass.h | 10 ++ 2 files changed, 127 insertions(+), 79 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index ae7ce50e9e4b03..f9cfb6e9edbc4f 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -1127,8 +1127,7 @@ void Compass::_probe_external_i2c_compasses(void) #if AP_COMPASS_HMC5843_ENABLED // external i2c bus FOREACH_I2C_EXTERNAL(i) { - add_backend(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(i, HAL_COMPASS_HMC5843_I2C_ADDR), - true, ROTATION_ROLL_180)); + probe_i2c_dev(DRIVER_HMC5843, AP_Compass_HMC5843::probe, i, HAL_COMPASS_HMC5843_I2C_ADDR, true, ROTATION_ROLL_180); RETURN_IF_NO_SPACE; } @@ -1137,8 +1136,7 @@ void Compass::_probe_external_i2c_compasses(void) AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_AEROFC) { // internal i2c bus FOREACH_I2C_INTERNAL(i) { - add_backend(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(i, HAL_COMPASS_HMC5843_I2C_ADDR), - all_external, all_external?ROTATION_ROLL_180:ROTATION_YAW_270)); + probe_i2c_dev(DRIVER_HMC5843, AP_Compass_HMC5843::probe, i, HAL_COMPASS_HMC5843_I2C_ADDR, all_external, all_external?ROTATION_ROLL_180:ROTATION_YAW_270); RETURN_IF_NO_SPACE; } } @@ -1148,8 +1146,7 @@ void Compass::_probe_external_i2c_compasses(void) #if AP_COMPASS_QMC5883L_ENABLED //external i2c bus FOREACH_I2C_EXTERNAL(i) { - add_backend(DRIVER_QMC5883L, AP_Compass_QMC5883L::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883L_I2C_ADDR), - true, HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL)); + probe_i2c_dev(DRIVER_QMC5883L, AP_Compass_QMC5883L::probe, i, HAL_COMPASS_QMC5883L_I2C_ADDR, true, HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL); RETURN_IF_NO_SPACE; } @@ -1158,9 +1155,7 @@ void Compass::_probe_external_i2c_compasses(void) if (all_external) { // only probe QMC5883L on internal if we are treating internals as externals FOREACH_I2C_INTERNAL(i) { - add_backend(DRIVER_QMC5883L, AP_Compass_QMC5883L::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883L_I2C_ADDR), - all_external, - all_external?HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL:HAL_COMPASS_QMC5883L_ORIENTATION_INTERNAL)); + probe_i2c_dev(DRIVER_QMC5883L, AP_Compass_QMC5883L::probe, i, HAL_COMPASS_QMC5883L_I2C_ADDR, all_external, all_external?HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL:HAL_COMPASS_QMC5883L_ORIENTATION_INTERNAL); RETURN_IF_NO_SPACE; } } @@ -1170,8 +1165,7 @@ void Compass::_probe_external_i2c_compasses(void) #if AP_COMPASS_QMC5883P_ENABLED //external i2c bus FOREACH_I2C_EXTERNAL(i) { - add_backend(DRIVER_QMC5883P, AP_Compass_QMC5883P::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883P_I2C_ADDR), - true, HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL)); + probe_i2c_dev(DRIVER_QMC5883P, AP_Compass_QMC5883P::probe, i, HAL_COMPASS_QMC5883P_I2C_ADDR, true, HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL); RETURN_IF_NO_SPACE; } @@ -1180,9 +1174,7 @@ void Compass::_probe_external_i2c_compasses(void) if (all_external) { // only probe QMC5883P on internal if we are treating internals as externals FOREACH_I2C_INTERNAL(i) { - add_backend(DRIVER_QMC5883P, AP_Compass_QMC5883P::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883P_I2C_ADDR), - all_external, - all_external?HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL:HAL_COMPASS_QMC5883P_ORIENTATION_INTERNAL)); + probe_i2c_dev(DRIVER_QMC5883P, AP_Compass_QMC5883P::probe, i, HAL_COMPASS_QMC5883P_I2C_ADDR, all_external, all_external?HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL:HAL_COMPASS_QMC5883P_ORIENTATION_INTERNAL); RETURN_IF_NO_SPACE; } } @@ -1192,8 +1184,7 @@ void Compass::_probe_external_i2c_compasses(void) #if AP_COMPASS_IIS2MDC_ENABLED //external i2c bus FOREACH_I2C_EXTERNAL(i) { - add_backend(DRIVER_IIS2MDC, AP_Compass_IIS2MDC::probe(GET_I2C_DEVICE(i, HAL_COMPASS_IIS2MDC_I2C_ADDR), - true, HAL_COMPASS_IIS2MDC_ORIENTATION_EXTERNAL)); + probe_i2c_dev(DRIVER_IIS2MDC, AP_Compass_IIS2MDC::probe, i, HAL_COMPASS_IIS2MDC_I2C_ADDR, true, HAL_COMPASS_IIS2MDC_ORIENTATION_EXTERNAL); RETURN_IF_NO_SPACE; } @@ -1202,9 +1193,7 @@ void Compass::_probe_external_i2c_compasses(void) if (all_external) { // only probe IIS2MDC on internal if we are treating internals as externals FOREACH_I2C_INTERNAL(i) { - add_backend(DRIVER_IIS2MDC, AP_Compass_IIS2MDC::probe(GET_I2C_DEVICE(i, HAL_COMPASS_IIS2MDC_I2C_ADDR), - all_external, - all_external?HAL_COMPASS_IIS2MDC_ORIENTATION_EXTERNAL:HAL_COMPASS_IIS2MDC_ORIENTATION_INTERNAL)); + probe_i2c_dev(DRIVER_IIS2MDC, AP_Compass_IIS2MDC::probe, i, HAL_COMPASS_IIS2MDC_I2C_ADDR, all_external, all_external?HAL_COMPASS_IIS2MDC_ORIENTATION_EXTERNAL:HAL_COMPASS_IIS2MDC_ORIENTATION_INTERNAL); RETURN_IF_NO_SPACE; } } @@ -1214,25 +1203,17 @@ void Compass::_probe_external_i2c_compasses(void) // AK09916 on ICM20948 #if AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED FOREACH_I2C_EXTERNAL(i) { - add_backend(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR), - GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR), - true, ROTATION_PITCH_180_YAW_90)); + probe_ak09916_via_icm20948(i, HAL_COMPASS_AK09916_I2C_ADDR, HAL_COMPASS_ICM20948_I2C_ADDR, true, ROTATION_PITCH_180_YAW_90); RETURN_IF_NO_SPACE; - add_backend(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR), - GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR2), - true, ROTATION_PITCH_180_YAW_90)); + probe_ak09916_via_icm20948(i, HAL_COMPASS_AK09916_I2C_ADDR, HAL_COMPASS_ICM20948_I2C_ADDR2, true, ROTATION_PITCH_180_YAW_90); RETURN_IF_NO_SPACE; } #if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED FOREACH_I2C_INTERNAL(i) { - add_backend(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR), - GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR), - all_external, ROTATION_PITCH_180_YAW_90)); + probe_ak09916_via_icm20948(i, HAL_COMPASS_AK09916_I2C_ADDR, HAL_COMPASS_ICM20948_I2C_ADDR, all_external, ROTATION_PITCH_180_YAW_90); RETURN_IF_NO_SPACE; - add_backend(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR), - GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR2), - all_external, ROTATION_PITCH_180_YAW_90)); + probe_ak09916_via_icm20948(i, HAL_COMPASS_AK09916_I2C_ADDR, HAL_COMPASS_ICM20948_I2C_ADDR2, all_external, ROTATION_PITCH_180_YAW_90); RETURN_IF_NO_SPACE; } #endif // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED @@ -1242,29 +1223,25 @@ void Compass::_probe_external_i2c_compasses(void) // lis3mdl on bus 0 with default address #if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED FOREACH_I2C_INTERNAL(i) { - add_backend(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR), - all_external, all_external?ROTATION_YAW_90:ROTATION_NONE)); + probe_i2c_dev(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe, i, HAL_COMPASS_LIS3MDL_I2C_ADDR, all_external, all_external?ROTATION_YAW_90:ROTATION_NONE); RETURN_IF_NO_SPACE; } // lis3mdl on bus 0 with alternate address FOREACH_I2C_INTERNAL(i) { - add_backend(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR2), - all_external, all_external?ROTATION_YAW_90:ROTATION_NONE)); + probe_i2c_dev(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe, i, HAL_COMPASS_LIS3MDL_I2C_ADDR2, all_external, all_external?ROTATION_YAW_90:ROTATION_NONE); RETURN_IF_NO_SPACE; } #endif // external lis3mdl on bus 1 with default address FOREACH_I2C_EXTERNAL(i) { - add_backend(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR), - true, ROTATION_YAW_90)); + probe_i2c_dev(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe, i, HAL_COMPASS_LIS3MDL_I2C_ADDR, true, ROTATION_YAW_90); RETURN_IF_NO_SPACE; } // external lis3mdl on bus 1 with alternate address FOREACH_I2C_EXTERNAL(i) { - add_backend(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR2), - true, ROTATION_YAW_90)); + probe_i2c_dev(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe, i, HAL_COMPASS_LIS3MDL_I2C_ADDR2, true, ROTATION_YAW_90); RETURN_IF_NO_SPACE; } #endif // AP_COMPASS_LIS3MDL_ENABLED @@ -1273,16 +1250,14 @@ void Compass::_probe_external_i2c_compasses(void) // external lis2mdl #if AP_COMPASS_LIS2MDL_EXTERNAL_BUS_PROBING_ENABLED FOREACH_I2C_EXTERNAL(i) { - add_backend(DRIVER_LIS2MDL, AP_Compass_LIS2MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS2MDL_I2C_ADDR), - true, ROTATION_NONE)); + probe_i2c_dev(DRIVER_LIS2MDL, AP_Compass_LIS2MDL::probe, i, HAL_COMPASS_LIS2MDL_I2C_ADDR, true, ROTATION_NONE); RETURN_IF_NO_SPACE; } #endif // internal lis2mdl #if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED FOREACH_I2C_INTERNAL(i) { - add_backend(DRIVER_LIS2MDL, AP_Compass_LIS2MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS2MDL_I2C_ADDR), - all_external, ROTATION_NONE)); + probe_i2c_dev(DRIVER_LIS2MDL, AP_Compass_LIS2MDL::probe, i, HAL_COMPASS_LIS2MDL_I2C_ADDR, all_external, ROTATION_NONE); RETURN_IF_NO_SPACE; } #endif @@ -1291,14 +1266,12 @@ void Compass::_probe_external_i2c_compasses(void) #if AP_COMPASS_AK09916_ENABLED // AK09916. This can be found twice, due to the ICM20948 i2c bus pass-thru, so we need to be careful to avoid that FOREACH_I2C_EXTERNAL(i) { - add_backend(DRIVER_AK09916, AP_Compass_AK09916::probe(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR), - true, ROTATION_YAW_270)); + probe_i2c_dev(DRIVER_AK09916, AP_Compass_AK09916::probe, i, HAL_COMPASS_AK09916_I2C_ADDR, true, ROTATION_YAW_270); RETURN_IF_NO_SPACE; } #if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED FOREACH_I2C_INTERNAL(i) { - add_backend(DRIVER_AK09916, AP_Compass_AK09916::probe(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR), - all_external, all_external?ROTATION_YAW_270:ROTATION_NONE)); + probe_i2c_dev(DRIVER_AK09916, AP_Compass_AK09916::probe, i, HAL_COMPASS_AK09916_I2C_ADDR, all_external, all_external?ROTATION_YAW_270:ROTATION_NONE); RETURN_IF_NO_SPACE; } #endif // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED @@ -1318,14 +1291,12 @@ void Compass::_probe_external_i2c_compasses(void) for (uint8_t a=0; aget_device(HAL_COMPASS_HMC5843_NAME), - false, ROTATION_PITCH_180)); + probe_spi_dev(DRIVER_HMC5843, AP_Compass_HMC5843::probe, HAL_COMPASS_HMC5843_NAME, false, ROTATION_PITCH_180); RETURN_IF_NO_SPACE; #endif #if AP_COMPASS_LSM303D_ENABLED - add_backend(DRIVER_LSM303D, AP_Compass_LSM303D::probe(hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), ROTATION_NONE)); + probe_spi_dev(DRIVER_LSM303D, AP_Compass_LSM303D::probe, HAL_INS_LSM9DS0_A_NAME, ROTATION_NONE); RETURN_IF_NO_SPACE; #endif break; case AP_BoardConfig::PX4_BOARD_PIXHAWK2: #if AP_COMPASS_LSM303D_ENABLED - add_backend(DRIVER_LSM303D, AP_Compass_LSM303D::probe(hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME), ROTATION_YAW_270)); + probe_spi_dev(DRIVER_LSM303D, AP_Compass_LSM303D::probe, HAL_INS_LSM9DS0_EXT_A_NAME, ROTATION_YAW_270); RETURN_IF_NO_SPACE; #endif #if AP_COMPASS_AK8963_ENABLED // we run the AK8963 only on the 2nd MPU9250, which leaves the // first MPU9250 to run without disturbance at high rate - add_backend(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(1, ROTATION_YAW_270)); + probe_ak8963_via_mpu9250(1, ROTATION_YAW_270); RETURN_IF_NO_SPACE; #endif #if AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED - add_backend(DRIVER_AK09916, AP_Compass_AK09916::probe_ICM20948(0, ROTATION_ROLL_180_YAW_90)); + probe_ak09916_via_icm20948(0, ROTATION_ROLL_180_YAW_90); RETURN_IF_NO_SPACE; #endif break; @@ -1550,13 +1514,11 @@ void Compass::probe_i2c_spi_compasses(void) case AP_BoardConfig::PX4_BOARD_FMUV6: #if AP_COMPASS_IST8310_ENABLED FOREACH_I2C_EXTERNAL(i) { - add_backend(DRIVER_IST8310, AP_Compass_IST8310::probe(GET_I2C_DEVICE(i, HAL_COMPASS_IST8310_I2C_ADDR), - true, ROTATION_ROLL_180_YAW_90)); + probe_i2c_dev(DRIVER_IST8310, AP_Compass_IST8310::probe, i, HAL_COMPASS_IST8310_I2C_ADDR, true, ROTATION_ROLL_180_YAW_90); RETURN_IF_NO_SPACE; } FOREACH_I2C_INTERNAL(i) { - add_backend(DRIVER_IST8310, AP_Compass_IST8310::probe(GET_I2C_DEVICE(i, HAL_COMPASS_IST8310_I2C_ADDR), - false, ROTATION_ROLL_180_YAW_90)); + probe_i2c_dev(DRIVER_IST8310, AP_Compass_IST8310::probe, i, HAL_COMPASS_IST8310_I2C_ADDR, false, ROTATION_ROLL_180_YAW_90); RETURN_IF_NO_SPACE; } #endif // AP_COMPASS_IST8310_ENABLED @@ -1564,21 +1526,21 @@ void Compass::probe_i2c_spi_compasses(void) case AP_BoardConfig::PX4_BOARD_PHMINI: #if AP_COMPASS_AK8963_ENABLED - add_backend(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(0, ROTATION_ROLL_180)); + probe_ak8963_via_mpu9250(0, ROTATION_ROLL_180); RETURN_IF_NO_SPACE; #endif break; case AP_BoardConfig::PX4_BOARD_AUAV21: #if AP_COMPASS_AK8963_ENABLED - add_backend(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(0, ROTATION_ROLL_180_YAW_90)); + probe_ak8963_via_mpu9250(0, ROTATION_ROLL_180_YAW_90); RETURN_IF_NO_SPACE; #endif break; case AP_BoardConfig::PX4_BOARD_PH2SLIM: #if AP_COMPASS_AK8963_ENABLED - add_backend(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(0, ROTATION_YAW_270)); + probe_ak8963_via_mpu9250(0, ROTATION_YAW_270); RETURN_IF_NO_SPACE; #endif break; @@ -1589,6 +1551,82 @@ void Compass::probe_i2c_spi_compasses(void) #endif } +#if AP_COMPASS_AK8963_ENABLED +void Compass::probe_ak8963_via_mpu9250(uint8_t imu_instance, Rotation rotation) +{ + if (!_driver_enabled(DRIVER_AK8963)) { + return; + } + auto *backend = AP_Compass_AK8963::probe_mpu9250(imu_instance, rotation); + add_backend(DRIVER_AK8963, backend); // add_backend does nullptr check +} +#endif // AP_COMPASS_AK8963_ENABLED + +#if AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED +void Compass::probe_ak09916_via_icm20948(uint8_t i2c_bus, uint8_t ak09916_addr, uint8_t icm20948_addr, bool external, Rotation rotation) +{ + if (!_driver_enabled(DRIVER_ICM20948)) { + return; + } + auto *backend = AP_Compass_AK09916::probe_ICM20948( + GET_I2C_DEVICE(i2c_bus, HAL_COMPASS_AK09916_I2C_ADDR), + GET_I2C_DEVICE(i2c_bus, HAL_COMPASS_ICM20948_I2C_ADDR), + external, + rotation + ); + + add_backend(DRIVER_ICM20948, backend); // add_backend does nullptr check +} + +void Compass::probe_ak09916_via_icm20948(uint8_t ins_instance, Rotation rotation) +{ + if (!_driver_enabled(DRIVER_AK09916)) { + return; + } + + auto *backend = AP_Compass_AK09916::probe_ICM20948(ins_instance, rotation); + + add_backend(DRIVER_AK09916, backend); +} + +#endif // #if AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED + +void Compass::probe_i2c_dev(DriverType driver_type, AP_Compass_Backend* (*probefn)(AP_HAL::OwnPtr, bool, Rotation), uint8_t i2c_bus, uint8_t i2c_addr, bool external, Rotation rotation) +{ + if (!_driver_enabled(driver_type)) { + return; + } + auto *backend = probefn( + GET_I2C_DEVICE(i2c_bus, i2c_addr), + external, + rotation + ); + + add_backend(driver_type, backend); // add_backend does nullptr check +} + +void Compass::probe_spi_dev(DriverType driver_type, AP_Compass_Backend* (*probefn)(AP_HAL::OwnPtr, bool, Rotation), const char *name, bool external, Rotation rotation) +{ + if (!_driver_enabled(driver_type)) { + return; + } + auto *backend = probefn(hal.spi->get_device(name), external, rotation); + + add_backend(driver_type, backend); // add_backend does nullptr check +} + +// short-lived method which expectes a probe function that doesn't +// offer the ability to specify an external rotation: +void Compass::probe_spi_dev(DriverType driver_type, AP_Compass_Backend* (*probefn)(AP_HAL::OwnPtr, Rotation), const char *name, Rotation rotation) +{ + if (!_driver_enabled(driver_type)) { + return; + } + auto *backend = probefn(hal.spi->get_device(name), rotation); + + add_backend(driver_type, backend); // add_backend does nullptr check +} + #if AP_COMPASS_DRONECAN_ENABLED /* look for DroneCAN compasses diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 64f42ca0d781d3..0b3898428451bc 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -503,6 +503,16 @@ friend class AP_Compass_DroneCAN; bool _driver_enabled(enum DriverType driver_type); void add_backend(DriverType driver_type, AP_Compass_Backend *backend); + // helper probe functions + void probe_ak09916_via_icm20948(uint8_t i2c_bus, uint8_t ak09916_addr, uint8_t icm20948_addr, bool external, Rotation rotation); + void probe_ak09916_via_icm20948(uint8_t ins_instance, Rotation rotation); + void probe_ak8963_via_mpu9250(uint8_t imu_instance, Rotation rotation); + void probe_i2c_dev(DriverType driver_type, AP_Compass_Backend* (*probefn)(AP_HAL::OwnPtr, bool, Rotation), uint8_t i2c_bus, uint8_t i2c_addr, bool external, Rotation rotation); + void probe_spi_dev(DriverType driver_type, AP_Compass_Backend* (*probefn)(AP_HAL::OwnPtr, bool, Rotation), const char *name, bool external, Rotation rotation); + // short-lived method which expectes a probe function that doesn't + // offer the ability to specify an external rotation + void probe_spi_dev(DriverType driver_type, AP_Compass_Backend* (*probefn)(AP_HAL::OwnPtr, Rotation), const char *name, Rotation rotation); + // backend objects AP_Compass_Backend *_backends[COMPASS_MAX_BACKEND]; uint8_t _backend_count;