diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index f9598b80ea..3c1edd31c6 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -180,8 +180,6 @@ 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, 1.0f); - _bus_sem->give(); hal.scheduler->resume_timer_procs(); diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index 1bfc1642bd..4aa694ab26 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -141,9 +141,3 @@ 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 1d6ae29a3e..6d41721857 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.h +++ b/libraries/AP_Compass/AP_Compass_Backend.h @@ -73,9 +73,6 @@ 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 afc2b07dc7..671fcf2b56 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/libraries/AP_Compass/AP_Compass_HIL.cpp @@ -53,7 +53,6 @@ AP_Compass_HIL::init(void) // register two compass instances for (uint8_t i=0; iregister_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_LSM303D::_update, void)); - set_milligauss_ratio(_compass_instance, 1.0f); - _spi_sem->give(); hal.scheduler->resume_timer_procs(); _initialised = true; diff --git a/libraries/AP_Compass/AP_Compass_PX4.cpp b/libraries/AP_Compass/AP_Compass_PX4.cpp index d1f1048e85..8f5a41cbd4 100644 --- a/libraries/AP_Compass/AP_Compass_PX4.cpp +++ b/libraries/AP_Compass/AP_Compass_PX4.cpp @@ -96,7 +96,6 @@ bool AP_Compass_PX4::init(void) _count[i] = 0; _sum[i].zero(); - set_milligauss_ratio(_instance[i], 1.0f); } // give the driver a chance to run, and gather one sample diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 370d98eb59..596fee568d 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -119,7 +119,7 @@ public: uint8_t get_count(void) const { return _compass_count; } /// Return the current field as a Vector3f - const Vector3f &get_field(uint8_t i) const { return _state[i].field * _state[i].milligauss_ratio; } + const Vector3f &get_field(uint8_t i) const { return _state[i].field; } const Vector3f &get_field(void) const { return get_field(get_primary()); } // raw/unfiltered measurement interface @@ -180,7 +180,7 @@ public: /// /// @returns The current compass offsets. /// - const Vector3f &get_offsets(uint8_t i) const { return _state[i].offset * _state[i].milligauss_ratio; } + const Vector3f &get_offsets(uint8_t i) const { return _state[i].offset; } const Vector3f &get_offsets(void) const { return get_offsets(get_primary()); } /// Sets the initial location used to get declination @@ -391,7 +391,6 @@ private: // corrected magnetic field strength Vector3f field; - float milligauss_ratio; // when we last got data uint32_t last_update_ms;