Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: added register checking for AK09916 mode

Browse Source 
this allows AK09916 on I2C to recover from a power outage in flight
This commit is contained in:
Andrew Tridgell 2021-12-19 12:30:57 +11:00 committed by Peter Barker
parent 336b666f83
commit 45bfc75e4b
2 changed files with 38 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_Compass/AP_Compass_AK09916.cpp
View File

@ -243,6 +243,9 @@ bool AP_Compass_AK09916::init()
goto fail;
}
// one checked register for mode
_bus->setup_checked_registers(1);
if (!_setup_mode()) {
hal.console->printf("AK09916: Could not setup mode\n");
goto fail;
@ -301,22 +304,22 @@ void AP_Compass_AK09916::_update()
Vector3f raw_field;
if (!_bus->block_read(REG_ST1, (uint8_t *) &regs, sizeof(regs))) {
return;
goto check_registers;
}
if (!(regs.st1 & 0x01)) {
return;
goto check_registers;
}
/* Check for overflow. See AK09916's datasheet*/
if ((regs.st2 & 0x08)) {
return;
goto check_registers;
}
raw_field = Vector3f(regs.val[0], regs.val[1], regs.val[2]);
if (is_zero(raw_field.x) && is_zero(raw_field.y) && is_zero(raw_field.z)) {
return;
goto check_registers;
}
_make_adc_sensitivity_adjustment(raw_field);
@ -336,6 +339,9 @@ void AP_Compass_AK09916::_update()
#endif
accumulate_sample(raw_field, _compass_instance, 10);
check_registers:
_bus->check_next_register();
}
bool AP_Compass_AK09916::_check_id()
@ -363,7 +369,7 @@ bool AP_Compass_AK09916::_check_id()
}
bool AP_Compass_AK09916::_setup_mode() {
return _bus->register_write(REG_CNTL2, 0x08); //Continuous Mode 2
return _bus->register_write(REG_CNTL2, 0x08, true); //Continuous Mode 2
}
bool AP_Compass_AK09916::_reset()
@ -387,9 +393,9 @@ bool AP_AK09916_BusDriver_HALDevice::register_read(uint8_t reg, uint8_t *val)
return _dev->read_registers(reg, val, 1);
}
bool AP_AK09916_BusDriver_HALDevice::register_write(uint8_t reg, uint8_t val)
bool AP_AK09916_BusDriver_HALDevice::register_write(uint8_t reg, uint8_t val, bool checked)
{
return _dev->write_register(reg, val);
return _dev->write_register(reg, val, checked);
}
AP_HAL::Semaphore *AP_AK09916_BusDriver_HALDevice::get_semaphore()
@ -453,8 +459,9 @@ bool AP_AK09916_BusDriver_Auxiliary::register_read(uint8_t reg, uint8_t *val)
return _slave->passthrough_read(reg, val, 1) == 1;
}
bool AP_AK09916_BusDriver_Auxiliary::register_write(uint8_t reg, uint8_t val)
bool AP_AK09916_BusDriver_Auxiliary::register_write(uint8_t reg, uint8_t val, bool checked)
{
(void)checked;
return _slave->passthrough_write(reg, val) == 1;
}

27
libraries/AP_Compass/AP_Compass_AK09916.h
View File

@ -86,7 +86,6 @@ private:
AP_AK09916_BusDriver *_bus;
float _magnetometer_ASA[3] {0, 0, 0};
bool _force_external;
uint8_t _compass_instance;
bool _initialized;
@ -102,7 +101,7 @@ public:
virtual bool block_read(uint8_t reg, uint8_t *buf, uint32_t size) = 0;
virtual bool register_read(uint8_t reg, uint8_t *val) = 0;
virtual bool register_write(uint8_t reg, uint8_t val) = 0;
virtual bool register_write(uint8_t reg, uint8_t val, bool checked=false) = 0;
virtual AP_HAL::Semaphore *get_semaphore() = 0;
@ -115,6 +114,14 @@ public:
// return 24 bit bus identifier
virtual uint32_t get_bus_id(void) const = 0;
/**
setup for register value checking. Frequency is how often to
check registers. If set to 10 then every 10th call to
check_next_register will check a register
*/
virtual void setup_checked_registers(uint8_t num_regs) {}
virtual void check_next_register(void) {}
};
class AP_AK09916_BusDriver_HALDevice: public AP_AK09916_BusDriver
@ -124,7 +131,7 @@ public:
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;
virtual bool register_write(uint8_t reg, uint8_t val) override;
virtual bool register_write(uint8_t reg, uint8_t val, bool checked) override;
virtual AP_HAL::Semaphore *get_semaphore() override;
AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;
@ -138,6 +145,18 @@ public:
uint32_t get_bus_id(void) const override {
return _dev->get_bus_id();
}
/**
setup for register value checking. Frequency is how often to
check registers. If set to 10 then every 10th call to
check_next_register will check a register
*/
void setup_checked_registers(uint8_t num_regs) override {
_dev->setup_checked_registers(num_regs);
}
void check_next_register(void) override {
_dev->check_next_register();
}
private:
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
@ -152,7 +171,7 @@ public:
bool block_read(uint8_t reg, uint8_t *buf, uint32_t size) override;
bool register_read(uint8_t reg, uint8_t *val) override;
bool register_write(uint8_t reg, uint8_t val) override;
bool register_write(uint8_t reg, uint8_t val, bool checked) override;
AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override;