zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Compass: implement full state machine for MMC3416 

this uses 100Hz readings most of the time, but does slow offset
readings every 50 samples
mission-4.1.18
Andrew Tridgell 8 years ago
parent
167988b5c4
commit
36932a2959
2 changed files with 111 additions and 30 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 136
      libraries/AP_Compass/AP_Compass_MMC3416.cpp
  2. 5
      libraries/AP_Compass/AP_Compass_MMC3416.h

136
libraries/AP_Compass/AP_Compass_MMC3416.cpp
Unescape View File

@ -30,6 +30,12 @@ extern const AP_HAL::HAL &hal; @@ -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() @@ -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: @@ -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() @@ -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() @@ -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() @@ -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;
}
state = STATE_REFILL1;
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;
}
}
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() @@ -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
Unescape View File

@ -51,6 +51,7 @@ private: @@ -51,6 +51,7 @@ private:
STATE_MEASURE_WAIT1,
STATE_REFILL2_WAIT,
STATE_MEASURE_WAIT2,
STATE_MEASURE_WAIT3,
} state;
/**
@ -58,11 +59,15 @@ private: @@ -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;

Write Preview
Loading…
Cancel
Save