diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 28ec2353a7..b0fcb39382 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -620,6 +620,13 @@ void Compass::_detect_backends(void) ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_AK09916_I2C_ADDR), both_i2c_external, both_i2c_external?ROTATION_YAW_270:ROTATION_NONE), AP_Compass_AK09916::name, both_i2c_external); + + // IST8310 on external and internal bus + ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_IST8310_I2C_ADDR), + true, ROTATION_PITCH_180), AP_Compass_IST8310::name, true); + + ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_IST8310_I2C_ADDR), + both_i2c_external, ROTATION_PITCH_180), AP_Compass_IST8310::name, both_i2c_external); #endif // HAL_MINIMIZE_FEATURES } break; @@ -627,7 +634,7 @@ void Compass::_detect_backends(void) case AP_BoardConfig::PX4_BOARD_AEROFC: #ifdef HAL_COMPASS_IST8310_I2C_BUS ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_IST8310_I2C_BUS, HAL_COMPASS_IST8310_I2C_ADDR), - ROTATION_PITCH_180_YAW_90), AP_Compass_IST8310::name, true); + true, ROTATION_PITCH_180_YAW_90), AP_Compass_IST8310::name, true); #endif break; default: diff --git a/libraries/AP_Compass/AP_Compass_IST8310.cpp b/libraries/AP_Compass/AP_Compass_IST8310.cpp index de9001ea53..8abb826600 100644 --- a/libraries/AP_Compass/AP_Compass_IST8310.cpp +++ b/libraries/AP_Compass/AP_Compass_IST8310.cpp @@ -77,13 +77,14 @@ extern const AP_HAL::HAL &hal; AP_Compass_Backend *AP_Compass_IST8310::probe(Compass &compass, AP_HAL::OwnPtr dev, + bool force_external, enum Rotation rotation) { if (!dev) { return nullptr; } - AP_Compass_IST8310 *sensor = new AP_Compass_IST8310(compass, std::move(dev), rotation); + AP_Compass_IST8310 *sensor = new AP_Compass_IST8310(compass, std::move(dev), force_external, rotation); if (!sensor || !sensor->init()) { delete sensor; return nullptr; @@ -94,10 +95,12 @@ AP_Compass_Backend *AP_Compass_IST8310::probe(Compass &compass, AP_Compass_IST8310::AP_Compass_IST8310(Compass &compass, AP_HAL::OwnPtr dev, + bool force_external, enum Rotation rotation) : AP_Compass_Backend(compass) , _dev(std::move(dev)) , _rotation(rotation) + , _force_external(force_external) { } @@ -163,6 +166,10 @@ bool AP_Compass_IST8310::init() _dev->set_device_type(DEVTYPE_IST8310); set_dev_id(_instance, _dev->get_bus_id()); + if (_force_external) { + set_external(_instance, true); + } + _periodic_handle = _dev->register_periodic_callback(SAMPLING_PERIOD_USEC, FUNCTOR_BIND_MEMBER(&AP_Compass_IST8310::timer, void)); diff --git a/libraries/AP_Compass/AP_Compass_IST8310.h b/libraries/AP_Compass/AP_Compass_IST8310.h index c6f1c88377..a466a1be5c 100644 --- a/libraries/AP_Compass/AP_Compass_IST8310.h +++ b/libraries/AP_Compass/AP_Compass_IST8310.h @@ -24,11 +24,16 @@ #include "AP_Compass.h" #include "AP_Compass_Backend.h" +#ifndef HAL_COMPASS_IST8310_I2C_ADDR +#define HAL_COMPASS_IST8310_I2C_ADDR 0x0E +#endif + class AP_Compass_IST8310 : public AP_Compass_Backend { public: static AP_Compass_Backend *probe(Compass &compass, AP_HAL::OwnPtr dev, + bool force_external = false, enum Rotation rotation = ROTATION_NONE); void read() override; @@ -38,6 +43,7 @@ public: private: AP_Compass_IST8310(Compass &compass, AP_HAL::OwnPtr dev, + bool force_external, enum Rotation rotation); void timer(); @@ -55,4 +61,5 @@ private: enum Rotation _rotation; uint8_t _instance; bool _ignore_next_sample; + bool _force_external; };