Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: switch to a general method of heater compensation

Browse Source 
this allows any board to easily setup heater compensation for an
internal compass. The offsets are in body frame (previously in sensor
frame) and are sensor specific using bus device IDs
This commit is contained in:
Andrew Tridgell 2022-08-01 18:07:10 +10:00 committed by Randy Mackay
parent f16d2cc2aa
commit 6167bae280
3 changed files with 25 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_Compass/AP_Compass_AK09916.cpp
View File

@ -324,19 +324,6 @@ void AP_Compass_AK09916::_update()
_make_adc_sensitivity_adjustment(raw_field); _make_adc_sensitivity_adjustment(raw_field);
raw_field *= AK09916_MILLIGAUSS_SCALE; 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); accumulate_sample(raw_field, _compass_instance, 10);
check_registers: check_registers:

25
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -5,6 +5,7 @@
#include <AP_BattMonitor/AP_BattMonitor.h> #include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_Vehicle/AP_Vehicle_Type.h> #include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
extern const AP_HAL::HAL& hal; 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); 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) { if (!state.external) {
// and add in AHRS_ORIENTATION setting if not an external compass // and add in AHRS_ORIENTATION setting if not an external compass
if (_compass._board_orientation == ROTATION_CUSTOM && _compass._custom_rotation) { if (_compass._board_orientation == ROTATION_CUSTOM && _compass._custom_rotation) {

13
libraries/AP_Compass/AP_Compass_IST8310.cpp
View File

@ -229,19 +229,6 @@ void AP_Compass_IST8310::timer()
/* Resolution: 0.3 µT/LSB - already convert to milligauss */ /* Resolution: 0.3 µT/LSB - already convert to milligauss */
Vector3f field = Vector3f{x * 3.0f, y * 3.0f, z * 3.0f}; 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); accumulate_sample(field, _instance);
} }