Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: implement full state machine for MMC3416 

this uses 100Hz readings most of the time, but does slow offset
readings every 50 samples
This commit is contained in:
Andrew Tridgell 2016-12-19 21:11:00 +11:00
parent 167988b5c4
commit 36932a2959
2 changed files with 111 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

136
libraries/AP_Compass/AP_Compass_MMC3416.cpp
View File

@ -30,6 +30,12 @@ extern const AP_HAL::HAL &hal;
#define REG_CONTROL0 0x07
#define REG_CONTROL1 0x08
// bits in REG_CONTROL0
#define REG_CONTROL0_REFILL 0x80
#define REG_CONTROL0_RESET 0x40
#define REG_CONTROL0_SET 0x20
#define REG_CONTROL0_TM 0x01
AP_Compass_Backend *AP_Compass_MMC3416::probe(Compass &compass,
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
bool force_external,
@ -102,6 +108,9 @@ bool AP_Compass_MMC3416::init()
dev->register_periodic_callback(10000,
FUNCTOR_BIND_MEMBER(&AP_Compass_MMC3416::timer, bool));
// wait 250ms for the compass to make it's initial readings
hal.scheduler->delay(250);
return true;
fail:
@ -112,18 +121,29 @@ fail:
bool AP_Compass_MMC3416::timer()
{
uint32_t now = AP_HAL::millis();
const uint8_t measure_count_limit = 50;
const uint16_t zero_offset = 32768; // 16 bit mode
const uint16_t sensitivity = 2048; // counts per Gauss, 16 bit mode
const float counts_to_milliGauss = 1.0e3f / sensitivity;
/*
we use the SET/RESET method to remove bridge offset every
measure_count_limit measurements. This involves a fairly complex
state machine, but means we are much less sensitive to
temperature changes
*/
switch (state) {
case STATE_REFILL1:
if (dev->write_register(REG_CONTROL0, 0x80)) { // REFILL
if (dev->write_register(REG_CONTROL0, REG_CONTROL0_REFILL)) {
state = STATE_REFILL1_WAIT;
last_state_ms = AP_HAL::millis();
}
break;
case STATE_REFILL1_WAIT:
if (now - last_state_ms >= 50) {
if (!dev->write_register(REG_CONTROL0, 0x20) || // SET
!dev->write_register(REG_CONTROL0, 0x01)) { // Take Measurement
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_SET) ||
!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement
state = STATE_REFILL1;
} else {
state = STATE_MEASURE_WAIT1;
@ -138,10 +158,11 @@ bool AP_Compass_MMC3416::timer()
state = STATE_REFILL1;
break;
}
if (!dev->write_register(REG_CONTROL0, 0x80)) { // REFILL
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_REFILL)) {
state = STATE_REFILL1;
} else {
state = STATE_REFILL2_WAIT;
last_state_ms = AP_HAL::millis();
}
}
break;
@ -149,8 +170,8 @@ bool AP_Compass_MMC3416::timer()
case STATE_REFILL2_WAIT:
if (now - last_state_ms >= 50) {
if (!dev->write_register(REG_CONTROL0, 0x40) || // RESET
!dev->write_register(REG_CONTROL0, 0x01)) { // Take Measurement
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_RESET) ||
!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement
state = STATE_REFILL1;
} else {
state = STATE_MEASURE_WAIT2;
@ -168,45 +189,100 @@ bool AP_Compass_MMC3416::timer()
state = STATE_REFILL1;
break;
}
const uint16_t zero_offset = 32768; // 16 bit mode
const uint16_t sensitivity = 2048; // counts per Gauss, 16 bit mode
const float counts_to_milliGauss = 1.0e3f / sensitivity;
Vector3f field;
/*
calculate field and offset
*/
Vector3f f1(float(data0[0]) - zero_offset,
float(data0[1]) - zero_offset,
float(data0[2]) - zero_offset);
Vector3f f2(float(data1[0]) - zero_offset,
float(data1[1]) - zero_offset,
float(data1[2]) - zero_offset);
field = (f1 - f2) / 2;
field * counts_to_milliGauss;
/* rotate raw_field from sensor frame to body frame */
rotate_field(field, compass_instance);
/* publish raw_field (uncorrected point sample) for calibration use */
publish_raw_field(field, AP_HAL::micros(), compass_instance);
/* correct raw_field for known errors */
correct_field(field, compass_instance);
if (_sem->take(0)) {
accum += field;
accum_count++;
_sem->give();
field = (f1 - f2) * (counts_to_milliGauss / 2);
Vector3f new_offset = (f1 + f2) * (counts_to_milliGauss / 2);
if (!have_initial_offset) {
offset = new_offset;
} else {
// low pass changes to the offset
offset = offset * 0.95 + new_offset * 0.05;
}
#if 0
printf("F(%.1f %.1f %.1f) O(%.1f %.1f %.1f)\n",
field.x, field.y, field.z,
offset.x, offset.y, offset.z);
#endif
accumulate_field(field);
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement
state = STATE_REFILL1;
} else {
state = STATE_MEASURE_WAIT3;
}
break;
}
case STATE_MEASURE_WAIT3: {
uint8_t status;
if (!dev->read_registers(REG_STATUS, &status, 1) || !(status & 1)) {
break;
}
uint16_t data1[3];
if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data1[0], 6)) {
state = STATE_REFILL1;
break;
}
Vector3f field(float(data1[0]) - zero_offset,
float(data1[1]) - zero_offset,
float(data1[2]) - zero_offset);
field *= -counts_to_milliGauss;
field += offset;
accumulate_field(field);
// we stay in STATE_MEASURE_WAIT3 for measure_count_limit cycles
if (measure_count++ >= measure_count_limit) {
measure_count = 0;
state = STATE_REFILL1;
} else {
if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement
state = STATE_REFILL1;
}
}
state = STATE_REFILL1;
break;
}
}
return true;
}
/*
accumulate a field
*/
void AP_Compass_MMC3416::accumulate_field(Vector3f &field)
{
/* rotate raw_field from sensor frame to body frame */
rotate_field(field, compass_instance);
/* publish raw_field (uncorrected point sample) for calibration use */
publish_raw_field(field, AP_HAL::micros(), compass_instance);
/* correct raw_field for known errors */
correct_field(field, compass_instance);
if (_sem->take(0)) {
accum += field;
accum_count++;
_sem->give();
}
}
void AP_Compass_MMC3416::read()
{
if (!_sem->take_nonblocking()) {
if (!_sem->take(0)) {
return;
}
if (accum_count == 0) {
@ -217,7 +293,7 @@ void AP_Compass_MMC3416::read()
accum /= accum_count;
publish_filtered_field(accum, compass_instance);
accum.zero();
accum_count = 0;

5
libraries/AP_Compass/AP_Compass_MMC3416.h
View File

@ -51,6 +51,7 @@ private:
STATE_MEASURE_WAIT1,
STATE_REFILL2_WAIT,
STATE_MEASURE_WAIT2,
STATE_MEASURE_WAIT3,
} state;
/**
@ -58,11 +59,15 @@ private:
*/
bool init();
bool timer();
void accumulate_field(Vector3f &field);
uint8_t compass_instance;
Vector3f accum;
uint16_t accum_count;
bool force_external;
Vector3f offset;
uint8_t measure_count;
bool have_initial_offset;
uint16_t data0[3];
uint32_t last_state_ms;