From 04430457d5cad64cb39bb294256ca71104d67c28 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 20 Dec 2016 09:12:37 +1100 Subject: [PATCH] AP_Compass: attempts to diagnose the twitches in MMC3416 --- libraries/AP_Compass/AP_Compass_MMC3416.cpp | 62 ++++++++++++++++----- libraries/AP_Compass/AP_Compass_MMC3416.h | 7 ++- 2 files changed, 52 insertions(+), 17 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_MMC3416.cpp b/libraries/AP_Compass/AP_Compass_MMC3416.cpp index 5e473e6cff..86076bc9a7 100644 --- a/libraries/AP_Compass/AP_Compass_MMC3416.cpp +++ b/libraries/AP_Compass/AP_Compass_MMC3416.cpp @@ -21,6 +21,7 @@ #include #include #include +#include extern const AP_HAL::HAL &hal; @@ -34,8 +35,12 @@ extern const AP_HAL::HAL &hal; #define REG_CONTROL0_REFILL 0x80 #define REG_CONTROL0_RESET 0x40 #define REG_CONTROL0_SET 0x20 +#define REG_CONTROL0_NB 0x10 #define REG_CONTROL0_TM 0x01 +// datasheet says 50ms min for refill +#define MIN_DELAY_SET_RESET 50 + AP_Compass_Backend *AP_Compass_MMC3416::probe(Compass &compass, AP_HAL::OwnPtr dev, bool force_external, @@ -76,7 +81,8 @@ bool AP_Compass_MMC3416::init() if (!dev->read_registers(REG_PRODUCT_ID, &whoami, 1) || whoami != 0x06) { // not a MMC3416 - goto fail; + dev->get_semaphore()->give(); + return false; } // reset sensor @@ -112,19 +118,21 @@ bool AP_Compass_MMC3416::init() hal.scheduler->delay(250); return true; - -fail: - dev->get_semaphore()->give(); - return false; } bool AP_Compass_MMC3416::timer() { - uint32_t now = AP_HAL::millis(); - const uint8_t measure_count_limit = 50; + const uint16_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; + + uint32_t now = AP_HAL::millis(); + if (now - last_sample_ms > 500) { + // seems to be stuck or on first sample, reset state machine + state = STATE_REFILL1; + last_sample_ms = now; + } /* we use the SET/RESET method to remove bridge offset every @@ -136,12 +144,15 @@ bool AP_Compass_MMC3416::timer() case STATE_REFILL1: if (dev->write_register(REG_CONTROL0, REG_CONTROL0_REFILL)) { state = STATE_REFILL1_WAIT; - last_state_ms = AP_HAL::millis(); + refill_start_ms = AP_HAL::millis(); } break; - case STATE_REFILL1_WAIT: - if (now - last_state_ms >= 50) { + case STATE_REFILL1_WAIT: { + uint8_t status; + if (AP_HAL::millis() - refill_start_ms > MIN_DELAY_SET_RESET && + dev->read_registers(REG_STATUS, &status, 1) && + (status & 0x02) == 0) { if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_SET) || !dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement state = STATE_REFILL1; @@ -150,6 +161,7 @@ bool AP_Compass_MMC3416::timer() } } break; + } case STATE_MEASURE_WAIT1: { uint8_t status; @@ -162,14 +174,17 @@ bool AP_Compass_MMC3416::timer() state = STATE_REFILL1; } else { state = STATE_REFILL2_WAIT; - last_state_ms = AP_HAL::millis(); + refill_start_ms = AP_HAL::millis(); } } break; } - case STATE_REFILL2_WAIT: - if (now - last_state_ms >= 50) { + case STATE_REFILL2_WAIT: { + uint8_t status; + if (AP_HAL::millis() - refill_start_ms > MIN_DELAY_SET_RESET && + dev->read_registers(REG_STATUS, &status, 1) && + (status & 0x02) == 0) { if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_RESET) || !dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement state = STATE_REFILL1; @@ -178,6 +193,7 @@ bool AP_Compass_MMC3416::timer() } } break; + } case STATE_MEASURE_WAIT2: { uint8_t status; @@ -204,12 +220,21 @@ bool AP_Compass_MMC3416::timer() Vector3f new_offset = (f1 + f2) * (counts_to_milliGauss / 2); if (!have_initial_offset) { offset = new_offset; + have_initial_offset = true; } else { // low pass changes to the offset offset = offset * 0.95 + new_offset * 0.05; } #if 0 + DataFlash_Class::instance()->Log_Write("MMO", "TimeUS,Nx,Ny,Nz,Ox,Oy,Oz", "Qffffff", + AP_HAL::micros64(), + (double)new_offset.x, + (double)new_offset.y, + (double)new_offset.z, + (double)offset.x, + (double)offset.y, + (double)offset.z); printf("F(%.1f %.1f %.1f) O(%.1f %.1f %.1f)\n", field.x, field.y, field.z, offset.x, offset.y, offset.z); @@ -217,7 +242,7 @@ bool AP_Compass_MMC3416::timer() accumulate_field(field); - if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement + if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { state = STATE_REFILL1; } else { state = STATE_MEASURE_WAIT3; @@ -264,6 +289,13 @@ bool AP_Compass_MMC3416::timer() */ void AP_Compass_MMC3416::accumulate_field(Vector3f &field) { +#if 0 + DataFlash_Class::instance()->Log_Write("MMC", "TimeUS,X,Y,Z", "Qfff", + AP_HAL::micros64(), + (double)field.x, + (double)field.y, + (double)field.z); +#endif /* rotate raw_field from sensor frame to body frame */ rotate_field(field, compass_instance); @@ -278,6 +310,8 @@ void AP_Compass_MMC3416::accumulate_field(Vector3f &field) accum_count++; _sem->give(); } + + last_sample_ms = AP_HAL::millis(); } void AP_Compass_MMC3416::read() diff --git a/libraries/AP_Compass/AP_Compass_MMC3416.h b/libraries/AP_Compass/AP_Compass_MMC3416.h index 21ce911157..8e281916ce 100644 --- a/libraries/AP_Compass/AP_Compass_MMC3416.h +++ b/libraries/AP_Compass/AP_Compass_MMC3416.h @@ -66,11 +66,12 @@ private: uint16_t accum_count; bool force_external; Vector3f offset; - uint8_t measure_count; + uint16_t measure_count; bool have_initial_offset; - + uint32_t refill_start_ms; + uint32_t last_sample_ms; + uint16_t data0[3]; - uint32_t last_state_ms; enum Rotation rotation; };