diff --git a/libraries/AP_Compass/AP_Compass_AK09916.cpp b/libraries/AP_Compass/AP_Compass_AK09916.cpp index 3be3053a58..fec2e90232 100644 --- a/libraries/AP_Compass/AP_Compass_AK09916.cpp +++ b/libraries/AP_Compass/AP_Compass_AK09916.cpp @@ -324,19 +324,6 @@ void AP_Compass_AK09916::_update() _make_adc_sensitivity_adjustment(raw_field); raw_field *= AK09916_MILLIGAUSS_SCALE; -#ifdef HAL_AK09916_HEATER_OFFSET - /* - the internal AK09916 can be impacted by the magnetic field from - a heater. We use the heater duty cycle to correct for the error - */ - if (AP_HAL::Device::devid_get_bus_type(_bus->get_bus_id()) == AP_HAL::Device::BUS_TYPE_SPI) { - auto *bc = AP::boardConfig(); - if (bc) { - raw_field += HAL_AK09916_HEATER_OFFSET * bc->get_heater_duty_cycle() * 0.01; - } - } -#endif - accumulate_sample(raw_field, _compass_instance, 10); check_registers: diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index ae41108221..9b9f5611cc 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -5,6 +5,7 @@ #include #include +#include extern const AP_HAL::HAL& hal; @@ -21,6 +22,30 @@ void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance) } mag.rotate(state.rotation); +#ifdef HAL_HEATER_MAG_OFFSET + /* + apply compass compensations for boards that have a heater which + interferes with an internal compass. This needs to be applied + before the board orientation so it is independent of + AHRS_ORIENTATION + */ + if (!is_external(instance)) { + const uint32_t dev_id = uint32_t(_compass._state[Compass::StateIndex(instance)].dev_id); + static const struct offset { + uint32_t dev_id; + Vector3f ofs; + } offsets[] = HAL_HEATER_MAG_OFFSET; + const auto *bc = AP::boardConfig(); + if (bc) { + for (const auto &o : offsets) { + if (o.dev_id == dev_id) { + mag += o.ofs * bc->get_heater_duty_cycle() * 0.01; + } + } + } + } +#endif + if (!state.external) { // and add in AHRS_ORIENTATION setting if not an external compass if (_compass._board_orientation == ROTATION_CUSTOM && _compass._custom_rotation) { diff --git a/libraries/AP_Compass/AP_Compass_IST8310.cpp b/libraries/AP_Compass/AP_Compass_IST8310.cpp index cd4fb05cb3..7a0769761d 100644 --- a/libraries/AP_Compass/AP_Compass_IST8310.cpp +++ b/libraries/AP_Compass/AP_Compass_IST8310.cpp @@ -229,19 +229,6 @@ void AP_Compass_IST8310::timer() /* Resolution: 0.3 µT/LSB - already convert to milligauss */ Vector3f field = Vector3f{x * 3.0f, y * 3.0f, z * 3.0f}; -#ifdef HAL_IST8310_I2C_HEATER_OFFSET - /* - the internal IST8310 can be impacted by the magnetic field from - a heater. We use the heater duty cycle to correct for the error - */ - if (!is_external(_instance) && AP_HAL::Device::devid_get_bus_type(_dev->get_bus_id()) == AP_HAL::Device::BUS_TYPE_I2C) { - const auto *bc = AP::boardConfig(); - if (bc) { - field += HAL_IST8310_I2C_HEATER_OFFSET * bc->get_heater_duty_cycle() * 0.01; - } - } -#endif - accumulate_sample(field, _instance); }