diff --git a/libraries/AP_Compass/AP_Compass_AK09916.cpp b/libraries/AP_Compass/AP_Compass_AK09916.cpp index e4bb9eece6..fcbd93fe16 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.cpp +++ b/libraries/AP_Compass/AP_Compass_AK09916.cpp @@ -168,8 +168,15 @@ fail: return nullptr; } +// un-named, assume SPI for compat AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(uint8_t inv2_instance, enum Rotation rotation) +{ + return probe_ICM20948_SPI(inv2_instance,rotation); +} + +AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948_SPI(uint8_t inv2_instance, + enum Rotation rotation) { #if HAL_INS_ENABLED AP_InertialSensor &ins = AP::ins(); @@ -192,6 +199,26 @@ AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(uint8_t inv2_instance, #endif } +AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948_I2C(uint8_t inv2_instance, + enum Rotation rotation) +{ + AP_InertialSensor &ins = AP::ins(); + + AP_AK09916_BusDriver *bus = + new AP_AK09916_BusDriver_Auxiliary(ins, HAL_INS_INV2_I2C, inv2_instance, HAL_COMPASS_AK09916_I2C_ADDR); + if (!bus) { + return nullptr; + } + + AP_Compass_AK09916 *sensor = new AP_Compass_AK09916(bus, false, rotation); + if (!sensor || !sensor->init()) { + delete sensor; + return nullptr; + } + + return sensor; +} + bool AP_Compass_AK09916::init() { AP_HAL::Semaphore *bus_sem = _bus->get_semaphore(); diff --git a/libraries/AP_Compass/AP_Compass_AK09916.h b/libraries/AP_Compass/AP_Compass_AK09916.h index a15e5ff220..bfc40a002b 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.h +++ b/libraries/AP_Compass/AP_Compass_AK09916.h @@ -54,8 +54,13 @@ public: bool force_external, enum Rotation rotation); - /* Probe for AK09916 on auxiliary bus of ICM20948, connected through SPI */ - static AP_Compass_Backend *probe_ICM20948(uint8_t mpu9250_instance, + /* Probe for AK09916 on auxiliary bus of ICM20948, connected through SPI by default */ + static AP_Compass_Backend *probe_ICM20948(uint8_t mpu9250_instance, enum Rotation rotation); + static AP_Compass_Backend *probe_ICM20948_SPI(uint8_t mpu9250_instance, + enum Rotation rotation); + + /* Probe for AK09916 on auxiliary bus of ICM20948, connected through I2C */ + static AP_Compass_Backend *probe_ICM20948_I2C(uint8_t mpu9250_instance, enum Rotation rotation); static constexpr const char *name = "AK09916";