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

AP_Compass: AK8963: remove state machine

Browse Source 
Don't use a state machine in AK8963: the start_measurements() method
should be called only once.  Even if there's a magnetic sensor overflow
the only thing we should do is to discard the new data.

This also moves the _collect_samples() method to be inside _update()
since it's the only place it should be called from, the one running on
the timer thread.
This commit is contained in:
Lucas De Marchi 2015-07-22 16:59:24 -03:00 committed by Andrew Tridgell
parent e232543fca
commit 27d95b67a8
2 changed files with 27 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

77
libraries/AP_Compass/AP_Compass_AK8963.cpp
View File

@ -97,7 +97,6 @@ extern const AP_HAL::HAL& hal;
AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_SerialBus *bus) : AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_SerialBus *bus) :
AP_Compass_Backend(compass), AP_Compass_Backend(compass),
_state(STATE_UNKNOWN),
_initialized(false), _initialized(false),
_last_update_timestamp(0), _last_update_timestamp(0),
_last_accum_time(0), _last_accum_time(0),
@ -183,18 +182,14 @@ bool AP_Compass_AK8963::init()
goto fail; goto fail;
} }
_state = STATE_SAMPLE;
_initialized = true; _initialized = true;
/* register the compass instance in the frontend */ /* register the compass instance in the frontend */
_compass_instance = register_compass(); _compass_instance = register_compass();
set_dev_id(_compass_instance, _bus->get_dev_id()); set_dev_id(_compass_instance, _bus->get_dev_id());
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void)); hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void));
_bus_sem->give(); _bus_sem->give();
hal.scheduler->resume_timer_procs(); hal.scheduler->resume_timer_procs();
return true; return true;
@ -244,19 +239,33 @@ void AP_Compass_AK8963::_update()
return; return;
} }
switch (_state) { struct AP_AK8963_SerialBus::raw_value rv;
case STATE_SAMPLE:
if (!_collect_samples()) { _bus->read_raw(&rv);
_state = STATE_ERROR;
/* Check for overflow. See AK8963's datasheet, section
* 6.4.3.6 - Magnetic Sensor Overflow. */
if ((rv.st2 & 0x08)) {
return;
} }
break;
case STATE_ERROR: float mag_x = (float) rv.val[0];
if (_bus->start_measurements()) { float mag_y = (float) rv.val[1];
_state = STATE_SAMPLE; float mag_z = (float) rv.val[2];
if (is_zero(mag_x) && is_zero(mag_y) && is_zero(mag_z)) {
return;
} }
break;
default: _mag_x_accum += mag_x;
break; _mag_y_accum += mag_y;
_mag_z_accum += mag_z;
_accum_count++;
if (_accum_count == 10) {
_mag_x_accum /= 2;
_mag_y_accum /= 2;
_mag_z_accum /= 2;
_accum_count = 5;
} }
_last_update_timestamp = hal.scheduler->micros(); _last_update_timestamp = hal.scheduler->micros();
@ -306,41 +315,6 @@ bool AP_Compass_AK8963::_calibrate()
return true; return true;
} }
bool AP_Compass_AK8963::_collect_samples()
{
struct AP_AK8963_SerialBus::raw_value rv;
if (!_initialized) {
return false;
}
_bus->read_raw(&rv);
if ((rv.st2 & 0x08)) {
return false;
}
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;
_mag_y_accum /= 2;
_mag_z_accum /= 2;
_accum_count = 5;
}
return true;
}
bool AP_Compass_AK8963::_sem_take_blocking() bool AP_Compass_AK8963::_sem_take_blocking()
{ {
return _bus_sem->take(10); return _bus_sem->take(10);
@ -471,7 +445,6 @@ bool AP_AK8963_SerialBus_MPU9250::start_measurements()
{ {
const uint8_t count = sizeof(struct raw_value); 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_ADDR, AK8963_I2C_ADDR | READ_FLAG); /* Set the I2C slave addres of AK8963 and set for read. */
_write(MPUREG_I2C_SLV0_REG, AK8963_INFO); /* 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 */ _write(MPUREG_I2C_SLV0_CTRL, I2C_SLV0_EN | count); /* Enable I2C and set @count byte */

11
libraries/AP_Compass/AP_Compass_AK8963.h
View File

@ -46,28 +46,17 @@ public:
void accumulate(void); void accumulate(void);
private: private:
typedef enum
{
STATE_UNKNOWN,
STATE_CONVERSION,
STATE_SAMPLE,
STATE_ERROR
} state_t;
bool _reset(); bool _reset();
bool _setup_mode(); bool _setup_mode();
bool _check_id(); bool _check_id();
bool _calibrate(); bool _calibrate();
void _update(); void _update();
bool _collect_samples();
void _dump_registers(); void _dump_registers();
bool _sem_take_blocking(); bool _sem_take_blocking();
bool _sem_take_nonblocking(); bool _sem_take_nonblocking();
bool _sem_give(); bool _sem_give();
state_t _state;
float _magnetometer_ASA[3] {0, 0, 0}; float _magnetometer_ASA[3] {0, 0, 0};
uint8_t _compass_instance; uint8_t _compass_instance;