From c59bdc12df5272ddbabf3ea3a146b893013a3c2f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 9 Sep 2015 14:52:14 +1000 Subject: [PATCH] AP_Compass: fix the milligauss handling the previous approach assumed a 1:1 mapping between compass backends and compass instances, which isn't true on PX4. It also only setup milligauss offsets on a set_and_save call, which is not the only way offsets change this adds a milligauss_ratio per instance, which is considerably simpler --- libraries/AP_Compass/AP_Compass_AK8963.cpp | 8 ++------ libraries/AP_Compass/AP_Compass_AK8963.h | 1 - libraries/AP_Compass/AP_Compass_Backend.cpp | 11 ++++++----- libraries/AP_Compass/AP_Compass_Backend.h | 5 +++-- libraries/AP_Compass/AP_Compass_HIL.cpp | 6 +----- libraries/AP_Compass/AP_Compass_HIL.h | 1 - libraries/AP_Compass/AP_Compass_HMC5843.cpp | 10 ++-------- libraries/AP_Compass/AP_Compass_HMC5843.h | 1 - libraries/AP_Compass/AP_Compass_PX4.cpp | 7 ++----- libraries/AP_Compass/AP_Compass_PX4.h | 1 - libraries/AP_Compass/Compass.cpp | 3 --- libraries/AP_Compass/Compass.h | 11 +++++------ 12 files changed, 21 insertions(+), 44 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index 42d236e0f7..19406a8f7c 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -178,6 +178,8 @@ bool AP_Compass_AK8963::init() set_dev_id(_compass_instance, _bus->get_dev_id()); hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void)); + set_milligauss_ratio(_compass_instance, 10.0f); + _bus_sem->give(); hal.scheduler->resume_timer_procs(); @@ -216,12 +218,6 @@ void AP_Compass_AK8963::read() publish_filtered_field(field, _compass_instance); } -float AP_Compass_AK8963::get_conversion_ratio(void) -{ - /* Convert from microTesla to milliGauss */ - return 10.0f; -} - Vector3f AP_Compass_AK8963::_get_filtered_field() const { Vector3f field(_mag_x_accum, _mag_y_accum, _mag_z_accum); diff --git a/libraries/AP_Compass/AP_Compass_AK8963.h b/libraries/AP_Compass/AP_Compass_AK8963.h index 0794177bc9..488f563c49 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.h +++ b/libraries/AP_Compass/AP_Compass_AK8963.h @@ -46,7 +46,6 @@ public: bool init(void); void read(void); void accumulate(void); - float get_conversion_ratio(void) override; private: static AP_Compass_Backend *_detect(Compass &compass, AP_AK8963_SerialBus *bus); diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index 7e4cead8bf..1bfc1642bd 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -41,11 +41,6 @@ void AP_Compass_Backend::publish_raw_field(const Vector3f &mag, uint32_t time_us { Compass::mag_state &state = _compass._state[instance]; - /* Update field in milligauss. Later this will be used throughout all codebase. - * We need this trick in order not to make users recalibrate their compasses. - * */ - state.field_milligauss = state.field * get_conversion_ratio(); - state.last_update_ms = hal.scheduler->millis(); state.last_update_usec = hal.scheduler->micros(); state.raw_field = mag; @@ -146,3 +141,9 @@ void AP_Compass_Backend::set_external(uint8_t instance, bool external) { _compass._state[instance].external.set(external); } + +// set ratio to convert to milligauss +void AP_Compass_Backend::set_milligauss_ratio(uint8_t instance, float ratio) +{ + _compass._state[instance].milligauss_ratio = ratio; +} diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h index 1358f79e80..1d6ae29a3e 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.h +++ b/libraries/AP_Compass/AP_Compass_Backend.h @@ -43,8 +43,6 @@ public: // backends virtual void accumulate(void) {}; - virtual float get_conversion_ratio(void) = 0; - protected: /* @@ -75,6 +73,9 @@ protected: // set external state for an instance void set_external(uint8_t instance, bool external); + // set ratio to convert to milligauss + void set_milligauss_ratio(uint8_t instance, float ratio); + // access to frontend Compass &_compass; diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index c1588bca79..afc2b07dc7 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/libraries/AP_Compass/AP_Compass_HIL.cpp @@ -53,6 +53,7 @@ AP_Compass_HIL::init(void) // register two compass instances for (uint8_t i=0; i 0); _count[i] = 0; _sum[i].zero(); + + set_milligauss_ratio(_instance[i], 1.0f); } // give the driver a chance to run, and gather one sample @@ -159,9 +161,4 @@ void AP_Compass_PX4::accumulate(void) } } -float AP_Compass_PX4::get_conversion_ratio(void) -{ - return 1.0f; -} - #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_Compass/AP_Compass_PX4.h b/libraries/AP_Compass/AP_Compass_PX4.h index 56567b8765..cac15e6ee1 100644 --- a/libraries/AP_Compass/AP_Compass_PX4.h +++ b/libraries/AP_Compass/AP_Compass_PX4.h @@ -12,7 +12,6 @@ public: bool init(void); void read(void); void accumulate(void); - float get_conversion_ratio(void) override; AP_Compass_PX4(Compass &compass); // detect the sensor diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index decaadd938..fa37eccbd9 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -517,9 +517,6 @@ Compass::set_and_save_offsets(uint8_t i, const Vector3f &offsets) if (i < COMPASS_MAX_INSTANCES) { _state[i].offset.set(offsets); save_offsets(i); - - /* Ugly hack to update offsets in milligauss that are going to be used across all the codebase in the future */ - _state[i].offset_milligauss = offsets * _backends[i]->get_conversion_ratio(); } } diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 47742ebd46..58ef5b35c1 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -113,8 +113,8 @@ public: /// Return the current field as a Vector3f const Vector3f &get_field(uint8_t i) const { return _state[i].field; } const Vector3f &get_field(void) const { return get_field(get_primary()); } - const Vector3f &get_field_milligauss(uint8_t i) const { return _state[i].field_milligauss; } - const Vector3f &get_field_milligauss(void) const { return get_field_milligauss(get_primary()); } + const Vector3f get_field_milligauss(uint8_t i) const { return get_field(i) * _state[i].milligauss_ratio; } + const Vector3f get_field_milligauss(void) const { return get_field_milligauss(get_primary()); } // raw/unfiltered measurement interface uint32_t raw_meas_time_us(uint8_t i) const { return _state[i].raw_meas_time_us; } @@ -173,8 +173,8 @@ public: /// const Vector3f &get_offsets(uint8_t i) const { return _state[i].offset; } const Vector3f &get_offsets(void) const { return get_offsets(get_primary()); } - const Vector3f &get_offsets_milligauss(uint8_t i) const { return _state[i].offset_milligauss; } - const Vector3f &get_offsets_milligauss(void) const { return get_offsets_milligauss(get_primary()); } + const Vector3f get_offsets_milligauss(uint8_t i) const { return get_offsets(i) * _state[i].milligauss_ratio; } + const Vector3f get_offsets_milligauss(void) const { return get_offsets_milligauss(get_primary()); } /// Sets the initial location used to get declination /// @@ -364,7 +364,6 @@ private: AP_Vector3f offset; AP_Vector3f diagonals; AP_Vector3f offdiagonals; - Vector3f offset_milligauss; #if COMPASS_MAX_INSTANCES > 1 // device id detected at init. @@ -385,7 +384,7 @@ private: // corrected magnetic field strength Vector3f field; - Vector3f field_milligauss; + float milligauss_ratio; // when we last got data uint32_t last_update_ms;