aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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

81
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_Backend(compass),
_state(STATE_UNKNOWN),
_initialized(false),
_last_update_timestamp(0),
_last_accum_time(0),
@ -183,18 +182,14 @@ bool AP_Compass_AK8963::init()
goto fail;
}
_state = STATE_SAMPLE;
_initialized = true;
/* register the compass instance in the frontend */
_compass_instance = register_compass();
set_dev_id(_compass_instance, _bus->get_dev_id());
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void));
_bus_sem->give();
hal.scheduler->resume_timer_procs();
return true;
@ -244,19 +239,33 @@ void AP_Compass_AK8963::_update()
return;
}
switch (_state) {
case STATE_SAMPLE:
if (!_collect_samples()) {
_state = STATE_ERROR;
}
break;
case STATE_ERROR:
if (_bus->start_measurements()) {
_state = STATE_SAMPLE;
}
break;
default:
break;
struct AP_AK8963_SerialBus::raw_value rv;
_bus->read_raw(&rv);
/* Check for overflow. See AK8963's datasheet, section
* 6.4.3.6 - Magnetic Sensor Overflow. */
if ((rv.st2 & 0x08)) {
return;
}
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;
}
_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;
}
_last_update_timestamp = hal.scheduler->micros();
@ -306,41 +315,6 @@ bool AP_Compass_AK8963::_calibrate()
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()
{
return _bus_sem->take(10);
@ -471,7 +445,6 @@ bool AP_AK8963_SerialBus_MPU9250::start_measurements()
{
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, 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 */

11
libraries/AP_Compass/AP_Compass_AK8963.h
View File

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