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

AP_Compass: AK8963: factor out common code of read_raw()

Browse Source 
Similar code was added in the read_raw() implementation for each bus.
Add a new POD struct read_raw to contain the registers from the AK8963
and use it instead as argument.
This commit is contained in:
Lucas De Marchi 2015-07-21 03:49:42 -03:00 committed by Andrew Tridgell
parent d941174978
commit 86b3312112
2 changed files with 31 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -119,7 +119,6 @@ AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_SerialBus *bus)
_bus(bus)
{
_mag_x_accum =_mag_y_accum = _mag_z_accum = 0;
_mag_x =_mag_y = _mag_z = 0;
_accum_count = 0;
_magnetometer_adc_resolution = AK8963_16BIT_ADC;
}
@ -329,17 +328,28 @@ bool AP_Compass_AK8963::_calibrate()
bool AP_Compass_AK8963::_collect_samples()
{
struct AP_AK8963_SerialBus::raw_value rv;
if (!_initialized) {
return false;
}
if (!_bus->read_raw(_mag_x, _mag_y, _mag_z)) {
_bus->read_raw(&rv);
if ((rv.st2 & 0x08)) {
return false;
}
_mag_x_accum += _mag_x;
_mag_y_accum += _mag_y;
_mag_z_accum += _mag_z;
float mag_x = (float) rv.val[0];
float mag_y = (float) rv.val[1];
float mag_z = (float) rv.val[2];
if (is_zero(mag_x) && is_zero(mag_y) && is_zero(mag_z)) {
return false;
}
_mag_x_accum += mag_x;
_mag_y_accum += mag_y;
_mag_z_accum += mag_z;
_accum_count++;
if (_accum_count == 10) {
_mag_x_accum /= 2;
@ -467,30 +477,9 @@ bool AP_AK8963_SerialBus_MPU9250::configure()
return true;
}
bool AP_AK8963_SerialBus_MPU9250::read_raw(float &mag_x, float &mag_y, float &mag_z)
void AP_AK8963_SerialBus_MPU9250::read_raw(struct raw_value *rv)
{
uint8_t rx[14] = {0};
const uint8_t count = 9;
_read(MPUREG_EXT_SENS_DATA_00, rx, count);
uint8_t st2 = rx[8]; /* End data read by reading ST2 register */
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx + 1] << 8) | v[2*idx]))
if (st2 & 0x08) {
return false;
}
mag_x = (float) int16_val(rx, 1);
mag_y = (float) int16_val(rx, 2);
mag_z = (float) int16_val(rx, 3);
if (is_zero(mag_x) && is_zero(mag_y) && is_zero(mag_z)) {
return false;
}
return true;
_read(MPUREG_EXT_SENS_DATA_00, (uint8_t *) rv, sizeof(*rv));
}
AP_HAL::Semaphore * AP_AK8963_SerialBus_MPU9250::get_semaphore()
@ -500,13 +489,11 @@ AP_HAL::Semaphore * AP_AK8963_SerialBus_MPU9250::get_semaphore()
bool AP_AK8963_SerialBus_MPU9250::start_conversion()
{
static const uint8_t address = AK8963_INFO;
/* Read registers from INFO through ST2 */
static const uint8_t count = 0x09;
const uint8_t count = sizeof(struct raw_value);
configure();
_write(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR | READ_FLAG); /* Set the I2C slave addres of AK8963 and set for read. */
_write(MPUREG_I2C_SLV0_REG, address); /* I2C slave 0 register address from where to begin data transfer */
_write(MPUREG_I2C_SLV0_REG, AK8963_INFO); /* I2C slave 0 register address from where to begin data transfer */
_write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | count); /* Enable I2C and set @count byte */
return true;
@ -534,30 +521,9 @@ void AP_AK8963_SerialBus_I2C::register_read(uint8_t address, uint8_t *value, uin
_i2c->readRegisters(_addr, address, count, value);
}
bool AP_AK8963_SerialBus_I2C::read_raw(float &mag_x, float &mag_y, float &mag_z)
void AP_AK8963_SerialBus_I2C::read_raw(struct raw_value *rv)
{
uint8_t rx[9] = {0};
const uint8_t count = 9;
_i2c->readRegisters(_addr, AK8963_INFO, count, rx);
uint8_t st2 = rx[8]; /* End data read by reading ST2 register */
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx + 1] << 8) | v[2*idx]))
if (st2 & 0x08) {
return false;
}
mag_x = (float) int16_val(rx, 1);
mag_y = (float) int16_val(rx, 2);
mag_z = (float) int16_val(rx, 3);
if (is_zero(mag_x) && is_zero(mag_y) && is_zero(mag_z)) {
return false;
}
return true;
_i2c->readRegisters(_addr, AK8963_INFO, sizeof(*rv), (uint8_t *) rv);
}
AP_HAL::Semaphore * AP_AK8963_SerialBus_I2C::get_semaphore()

16
libraries/AP_Compass/AP_Compass_AK8963.h
View File

@ -12,6 +12,13 @@
class AP_AK8963_SerialBus
{
public:
struct PACKED raw_value {
uint8_t info;
uint8_t st1;
int16_t val[3];
uint8_t st2;
};
virtual void register_read(uint8_t address, uint8_t *value, uint8_t count) = 0;
uint8_t register_read(uint8_t address) {
uint8_t reg;
@ -22,7 +29,7 @@ public:
virtual AP_HAL::Semaphore* get_semaphore() = 0;
virtual bool start_conversion() = 0;
virtual bool configure() = 0;
virtual bool read_raw(float &mag_x, float &mag_y, float &mag_z) = 0;
virtual void read_raw(struct raw_value *rv) = 0;
virtual uint32_t get_dev_id() = 0;
};
@ -62,9 +69,6 @@ private:
state_t _state;
float _magnetometer_ASA[3] {0, 0, 0};
float _mag_x;
float _mag_y;
float _mag_z;
uint8_t _compass_instance;
float _mag_x_accum;
@ -90,7 +94,7 @@ public:
AP_HAL::Semaphore* get_semaphore();
bool start_conversion();
bool configure();
bool read_raw(float &mag_x, float &mag_y, float &mag_z);
void read_raw(struct raw_value *rv);
uint32_t get_dev_id();
private:
void _read(uint8_t address, uint8_t *value, uint32_t count);
@ -111,7 +115,7 @@ public:
AP_HAL::Semaphore* get_semaphore();
bool start_conversion(){return true;}
bool configure(){return true;}
bool read_raw(float &mag_x, float &mag_y, float &mag_z);
void read_raw(struct raw_value *rv);
uint32_t get_dev_id();
private:
void _read(uint8_t address, uint8_t *value, uint32_t count);