AP_Compass: add calibration interface

libraries/AP_Compass/AP_Compass_Calibration.cpp

@@ -0,0 +1,232 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#include <AP_HAL.h>
+#include "Compass.h"
+#include <AP_Notify.h>
+
+void
+Compass::compass_cal_update()
+{
+    AP_Notify::flags.compass_cal_running = 0;
+
+    for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+        _calibrator[i].run_fit_chunk();
+
+        if(_calibrator[i].check_for_timeout()) {
+            cancel_calibration_all();
+        }
+
+        if(_calibrator[i].running()) {
+            AP_Notify::flags.compass_cal_running = 1;
+        }
+    }
+}
+
+bool
+Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay)
+{
+    if(healthy(i)) {
+        if(!is_calibrating() && delay > 0.5f) {
+            AP_Notify::events.initiated_compass_cal = 1;
+        }
+        if(i == get_primary()) {
+            _calibrator[i].set_tolerance(5);
+        } else {
+            _calibrator[i].set_tolerance(10);
+        }
+        _calibrator[i].start(retry, autosave, delay);
+        return true;
+    } else {
+        return false;
+    }
+}
+
+bool
+Compass::start_calibration_mask(uint8_t mask, bool retry, bool autosave, float delay)
+{
+    for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+        if((1<<i) & mask) {
+            if(!start_calibration(i,retry,autosave,delay)) {
+                cancel_calibration_mask(mask);
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
+bool
+Compass::start_calibration_all(bool retry, bool autosave, float delay)
+{
+    for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+        if(healthy(i) && use_for_yaw(i)) {
+            if(!start_calibration(i,retry,autosave,delay)) {
+                cancel_calibration_all();
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
+void
+Compass::cancel_calibration(uint8_t i)
+{
+    _calibrator[i].clear();
+    AP_Notify::events.compass_cal_canceled = 1;
+    AP_Notify::events.initiated_compass_cal = 0;
+}
+
+void
+Compass::cancel_calibration_mask(uint8_t mask)
+{
+    for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+        if((1<<i) & mask) {
+            cancel_calibration(i);
+        }
+    }
+}
+
+void
+Compass::cancel_calibration_all()
+{
+    cancel_calibration_mask(0xFF);
+}
+
+bool
+Compass::accept_calibration(uint8_t i)
+{
+    CompassCalibrator& cal = _calibrator[i];
+    uint8_t cal_status = cal.get_status();
+
+    if(cal_status == COMPASS_CAL_SUCCESS) {
+        Vector3f ofs, diag, offdiag;
+        cal.get_calibration(ofs, diag, offdiag);
+        cal.clear();
+
+        set_and_save_offsets(i, ofs);
+        set_and_save_diagonals(i,diag);
+        set_and_save_offdiagonals(i,offdiag);
+
+        if(!is_calibrating()) {
+            AP_Notify::events.compass_cal_saved = 1;
+        }
+        return true;
+    } else {
+        return false;
+    }
+}
+
+bool
+Compass::accept_calibration_mask(uint8_t mask)
+{
+    for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+        if((1<<i) & mask) {
+            CompassCalibrator& cal = _calibrator[i];
+            uint8_t cal_status = cal.get_status();
+            if(cal_status != COMPASS_CAL_SUCCESS && cal_status != COMPASS_CAL_NOT_STARTED) {
+                // a compass failed or is still in progress
+                return false;
+            }
+        }
+    }
+
+    bool success = true;
+    for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+        if((1<<i) & mask) {
+            if(!accept_calibration(i)) {
+                success = false;
+            }
+        }
+    }
+
+    return success;
+}
+
+bool
+Compass::accept_calibration_all()
+{
+    return accept_calibration_mask(0xFF);
+}
+
+void
+Compass::send_mag_cal_progress(mavlink_channel_t chan)
+{
+    uint8_t cal_mask = get_cal_mask();
+
+    for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+        CompassCalibrator& cal = _calibrator[i];
+
+        uint8_t& compass_id = i;
+        uint8_t cal_status = cal.get_status();
+
+        if(cal_status == COMPASS_CAL_WAITING_TO_START  ||
+           cal_status == COMPASS_CAL_RUNNING_STEP_ONE ||
+           cal_status == COMPASS_CAL_RUNNING_STEP_TWO) {
+            uint8_t completion_pct = cal.get_completion_percent();
+            uint8_t completion_mask[10];
+            Vector3f direction(0.0f,0.0f,0.0f);
+            uint8_t attempt = cal.get_attempt();
+
+            memset(completion_mask, 0, sizeof(completion_mask));
+
+            mavlink_msg_mag_cal_progress_send(
+                chan,
+                compass_id, cal_mask,
+                cal_status, attempt, completion_pct, completion_mask,
+                direction.x, direction.y, direction.z
+            );
+        }
+    }
+}
+
+void Compass::send_mag_cal_report(mavlink_channel_t chan)
+{
+    uint8_t cal_mask = get_cal_mask();
+
+    for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+        CompassCalibrator& cal = _calibrator[i];
+
+        uint8_t& compass_id = i;
+        uint8_t cal_status = cal.get_status();
+
+        if(cal_status == COMPASS_CAL_SUCCESS ||
+           cal_status == COMPASS_CAL_FAILED) {
+            float fitness = cal.get_fitness();
+            Vector3f ofs, diag, offdiag;
+            cal.get_calibration(ofs, diag, offdiag);
+            uint8_t autosaved = cal.get_autosave();
+
+            mavlink_msg_mag_cal_report_send(
+                chan,
+                compass_id, cal_mask,
+                cal_status, autosaved,
+                fitness,
+                ofs.x, ofs.y, ofs.z,
+                diag.x, diag.y, diag.z,
+                offdiag.x, offdiag.y, offdiag.z
+            );
+        }
+
+        if(cal_status == COMPASS_CAL_SUCCESS && cal.get_autosave()) {
+            accept_calibration(i);
+        }
+    }
+}
+
+bool
+Compass::is_calibrating() const
+{
+    return get_cal_mask();
+}
+
+uint8_t
+Compass::get_cal_mask() const
+{
+    uint8_t cal_mask = 0;
+    for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+        if(_calibrator[i].get_status() != COMPASS_CAL_NOT_STARTED) {
+            cal_mask |= 1 << i;
+        }
+    }
+    return cal_mask;
+}

libraries/AP_Compass/Compass.cpp

@@ -486,6 +486,18 @@ Compass::read(void)
     return healthy();
 }
 
+uint8_t
+Compass::get_healthy_mask() const
+{
+    uint8_t healthy_mask = 0;
+    for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+        if(healthy(i)) {
+            healthy_mask |= 1 << i;
+        }
+    }
+    return healthy_mask;
+}
+
 void
 Compass::set_offsets(uint8_t i, const Vector3f &offsets)
 {

libraries/AP_Compass/Compass.h

@@ -3,11 +3,13 @@
 #define Compass_h
 
 #include <inttypes.h>
-#include <AP_Common/AP_Common.h>
-#include <AP_Param/AP_Param.h>
-#include <AP_Math/AP_Math.h>
-#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
-#include <AP_HAL/AP_HAL.h>
+#include <AP_Common.h>
+#include <AP_Param.h>
+#include <AP_Math.h>
+#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
+#include <AP_HAL.h>
+#include <GCS_MAVLink.h>
+#include "CompassCalibrator.h"
 #include "AP_Compass_Backend.h"
 
 // compass product id
@@ -109,8 +111,6 @@ public:
     const Vector3f &get_field(void) const { return get_field(get_primary()); }
 
     // raw/unfiltered measurement interface
-
-
     uint32_t raw_meas_time_us(uint8_t i) const { return _state[i].raw_meas_time_us; }
     uint32_t raw_meas_time_us() const { return _state[get_primary()].raw_meas_time_us; }
     uint32_t unfiltered_meas_time_us(uint8_t i) const { return _state[i].raw_meas_time_us; }
@@ -128,9 +128,31 @@ public:
     const Vector3f &get_unfiltered_field(uint8_t i) const { return _state[i].unfiltered_field; }
     const Vector3f &get_unfiltered_field(void) const { return get_unfiltered_field(get_primary()); }
 
+    // compass calibrator interface
+    void compass_cal_update();
+
+    bool start_calibration(uint8_t i, bool retry=false, bool autosave=false, float delay_sec=0.0f);
+    bool start_calibration_all(bool retry=false, bool autosave=false, float delay_sec=0.0f);
+    bool start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f);
+
+    void cancel_calibration(uint8_t i);
+    void cancel_calibration_all();
+    void cancel_calibration_mask(uint8_t mask);
+
+    bool accept_calibration(uint8_t i);
+    bool accept_calibration_all();
+    bool accept_calibration_mask(uint8_t mask);
+
+    uint8_t get_cal_mask() const;
+    bool is_calibrating() const;
+
+    void send_mag_cal_progress(mavlink_channel_t chan);
+    void send_mag_cal_report(mavlink_channel_t chan);
+
     /// Return the health of a compass
     bool healthy(uint8_t i) const { return _state[i].healthy; }
     bool healthy(void) const { return healthy(get_primary()); }
+    uint8_t get_healthy_mask() const;
 
     /// Returns the current offset values
     ///
@@ -353,6 +375,8 @@ private:
         Vector3f    unfiltered_field;
     } _state[COMPASS_MAX_INSTANCES];
 
+    CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES];
+
     // if we want HIL only
     bool _hil_mode:1;
 };