From 86b33121122bb2220b51621b5a301275162856d8 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Tue, 21 Jul 2015 03:49:42 -0300 Subject: [PATCH] AP_Compass: AK8963: factor out common code of read_raw() 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. --- libraries/AP_Compass/AP_Compass_AK8963.cpp | 76 ++++++---------------- libraries/AP_Compass/AP_Compass_AK8963.h | 16 +++-- 2 files changed, 31 insertions(+), 61 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index a31dfb95cb..637cffaedc 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -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() diff --git a/libraries/AP_Compass/AP_Compass_AK8963.h b/libraries/AP_Compass/AP_Compass_AK8963.h index 5f27facc0a..389f204bab 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.h +++ b/libraries/AP_Compass/AP_Compass_AK8963.h @@ -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);