AP_Compass: add milligauss counterparts to get_field() and get_offsets()

From now on there's a pair get_field_milligauss() and
get_offsets_milligauss() that can make the transition to the common
units across all compasses easier.

libraries/AP_Compass/AP_Compass_AK8963.cpp

@@ -214,6 +214,12 @@ 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);

libraries/AP_Compass/AP_Compass_AK8963.h

@@ -46,6 +46,7 @@ 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);

libraries/AP_Compass/AP_Compass_Backend.cpp

@@ -41,6 +41,11 @@ 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;

libraries/AP_Compass/AP_Compass_Backend.h

@@ -43,6 +43,8 @@ public:
     // backends
     virtual void accumulate(void) {};
 
+    virtual float get_conversion_ratio(void) = 0;
+
 protected:
 
     /*

libraries/AP_Compass/AP_Compass_HIL.cpp

@@ -69,3 +69,8 @@ void AP_Compass_HIL::read()
         }
     }
 }
+
+float AP_Compass_HIL::get_conversion_ratio(void)
+{
+    return 1.0f;
+}

libraries/AP_Compass/AP_Compass_HIL.h

@@ -16,6 +16,7 @@ public:
     AP_Compass_HIL(Compass &compass);
     void read(void);
     bool init(void);
+    float get_conversion_ratio(void) override;
 
     // detect the sensor
     static AP_Compass_Backend *detect(Compass &compass);

libraries/AP_Compass/AP_Compass_HMC5843.cpp

@@ -269,7 +269,7 @@ AP_Compass_HMC5843::init()
     uint8_t calibration_gain = 0x20;
     uint16_t expected_x = 715;
     uint16_t expected_yz = 715;
-    float gain_multiple = 1.0;
+    _gain_multiple = 1.0;
 
     _bus_sem = _bus->get_semaphore();
     hal.scheduler->suspend_timer_procs();
@@ -298,10 +298,10 @@ AP_Compass_HMC5843::init()
          */
         expected_x = 766;
         expected_yz  = 713;
-        gain_multiple = 660.0f / 1090;  // adjustment for runtime vs calibration gain
+        _gain_multiple = 660.0f / 1090;  // adjustment for runtime vs calibration gain
     }
 
-    if (!_calibrate(calibration_gain, expected_x, expected_yz, gain_multiple)) {
+    if (!_calibrate(calibration_gain, expected_x, expected_yz)) {
         hal.console->printf_P(PSTR("HMC5843: Could not calibrate sensor\n"));
         goto errout;
     }
@@ -342,8 +342,7 @@ fail_sem:
 
 bool AP_Compass_HMC5843::_calibrate(uint8_t calibration_gain,
         uint16_t expected_x,
-        uint16_t expected_yz,
-        float gain_multiple)
+        uint16_t expected_yz)
 {
     int numAttempts = 0, good_count = 0;
     bool success = false;
@@ -413,7 +412,7 @@ bool AP_Compass_HMC5843::_calibrate(uint8_t calibration_gain,
 
     if (good_count >= 5) {
         /*
-          The use of gain_multiple below is incorrect, as the gain
+          The use of _gain_multiple below is incorrect, as the gain
           difference between 2.5Ga mode and 1Ga mode is already taken
           into account by the expected_x and expected_yz values.  We
           are not going to fix it however as it would mean all
@@ -423,9 +422,9 @@ bool AP_Compass_HMC5843::_calibrate(uint8_t calibration_gain,
           doesn't have any impact other than the learned compass
           offsets
          */
-        _scaling[0] = _scaling[0] * gain_multiple / good_count;
-        _scaling[1] = _scaling[1] * gain_multiple / good_count;
-        _scaling[2] = _scaling[2] * gain_multiple / good_count;
+        _scaling[0] = _scaling[0] * _gain_multiple / good_count;
+        _scaling[1] = _scaling[1] * _gain_multiple / good_count;
+        _scaling[2] = _scaling[2] * _gain_multiple / good_count;
         success = true;
     } else {
         /* best guess */
@@ -482,6 +481,14 @@ void AP_Compass_HMC5843::read()
     _retry_time = 0;
 }
 
+float AP_Compass_HMC5843::get_conversion_ratio(void)
+{
+    /* This value converts from hmc-units to milligauss. It looks strange for a reason.
+     * It's meant to cancel the unneccassary division in the read() method.
+     */
+    return 1.0f / _gain_multiple;
+}
+
 /* I2C implementation of the HMC5843 */
 AP_HMC5843_SerialBus_I2C::AP_HMC5843_SerialBus_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr)
     : _i2c(i2c)

libraries/AP_Compass/AP_Compass_HMC5843.h

@@ -31,7 +31,7 @@ private:
     bool                read_register(uint8_t address, uint8_t *value);
     bool                write_register(uint8_t address, uint8_t value);
 
-    bool                _calibrate(uint8_t calibration_gain, uint16_t expected_x, uint16_t expected_yz, float gain_multiple);
+    bool                _calibrate(uint8_t calibration_gain, uint16_t expected_x, uint16_t expected_yz);
     bool                _detect_version();
 
     uint32_t            _retry_time; // when unhealthy the millis() value to retry at
@@ -48,6 +48,7 @@ private:
     uint8_t             _compass_instance;
     uint8_t             _product_id;
 
+    float               _gain_multiple;
 public:
     // detect the sensor
     static AP_Compass_Backend *detect_i2c(Compass &compass,
@@ -60,6 +61,7 @@ public:
     bool        init(void);
     void        read(void);
     void        accumulate(void);
+    float       get_conversion_ratio(void) override;
 };
 
 class AP_HMC5843_SerialBus

libraries/AP_Compass/AP_Compass_PX4.cpp

@@ -159,4 +159,9 @@ void AP_Compass_PX4::accumulate(void)
     }
 }
 
+float AP_Compass_PX4::get_conversion_ratio(void)
+{
+    return 1.0f;
+}
+
 #endif // CONFIG_HAL_BOARD

libraries/AP_Compass/AP_Compass_PX4.h

@@ -12,6 +12,7 @@ public:
     bool        init(void);
     void        read(void);
     void        accumulate(void);
+    float       get_conversion_ratio(void) override;
 
     AP_Compass_PX4(Compass &compass);
     // detect the sensor

libraries/AP_Compass/Compass.h

@@ -113,6 +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()); }
 
     // raw/unfiltered measurement interface
     uint32_t raw_meas_time_us(uint8_t i) const { return _state[i].raw_meas_time_us; }
@@ -171,6 +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()); }
 
     /// Sets the initial location used to get declination
     ///
@@ -360,6 +364,7 @@ private:
         AP_Vector3f offset;
         AP_Vector3f diagonals;
         AP_Vector3f offdiagonals;
+        Vector3f    offset_milligauss;
 
 #if COMPASS_MAX_INSTANCES > 1
         // device id detected at init.  
@@ -380,6 +385,7 @@ private:
 
         // corrected magnetic field strength
         Vector3f    field;
+        Vector3f    field_milligauss;
 
         // when we last got data
         uint32_t    last_update_ms;