AP_Compass: implement automatic compass orientation

this automatically determines the compass orientation when doing a 3D
compass calibration, if COMPASS_ROT_AUTO is enabled.

libraries/AP_Compass/AP_Compass.cpp

@@ -443,6 +443,12 @@ const AP_Param::GroupInfo Compass::var_info[] = {
     // @Range: 0 100
     // @Increment: 1
     AP_GROUPINFO("FLTR_RNG", 34, Compass, _filter_range, HAL_COMPASS_FILTER_DEFAULT),
+
+    // @Param: ROT_AUTO
+    // @DisplayName: Automatically set orientation
+    // @Description: When enabled this will automatically set the orientation of external compasses on successful completion of compass calibration
+    // @Values: 0:Disabled,1:Enabled
+    AP_GROUPINFO("ROT_AUTO", 35, Compass, _rotate_auto, 1),
     
     AP_GROUPEND
 };

libraries/AP_Compass/AP_Compass.h

@@ -417,6 +417,9 @@ private:
     // 0 = disabled, 1 = enabled for throttle, 2 = enabled for current
     AP_Int8     _motor_comp_type;
 
+    // automatic compass orientation on calibration
+    AP_Int8     _rotate_auto;
+    
     // throttle expressed as a percentage from 0 ~ 1.0, used for motor compensation
     float       _thr;
 

libraries/AP_Compass/AP_Compass_Calibration.cpp

@@ -61,6 +61,9 @@ Compass::_start_calibration(uint8_t i, bool retry, float delay)
         // lot noisier
         _calibrator[i].set_tolerance(_calibration_threshold*2);
     }
+    if (_state[i].external && _rotate_auto) {
+        _calibrator[i].set_orientation((enum Rotation)_state[i].orientation.get(), _state[i].external);
+    }
     _cal_saved[i] = false;
     _calibrator[i].start(retry, delay, get_offsets_max());
 
@@ -147,6 +150,10 @@ Compass::_accept_calibration(uint8_t i)
         set_and_save_diagonals(i,diag);
         set_and_save_offdiagonals(i,offdiag);
 
+        if (_state[i].external && _rotate_auto) {
+            _state[i].orientation.set_and_save_ifchanged(cal.get_orientation());
+        }
+
         if (!is_calibrating()) {
             AP_Notify::events.compass_cal_saved = 1;
         }

libraries/AP_Compass/CompassCalibrator.cpp

@@ -60,6 +60,7 @@
 #include "CompassCalibrator.h"
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_GeodesicGrid.h>
+#include <AP_AHRS/AP_AHRS.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -167,6 +168,7 @@ void CompassCalibrator::new_sample(const Vector3f& sample) {
     if(running() && _samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample(sample)) {
         update_completion_mask(sample);
         _sample_buffer[_samples_collected].set(sample);
+        _sample_buffer[_samples_collected].att.set_from_ahrs();
         _samples_collected++;
     }
 }
@@ -195,6 +197,7 @@ void CompassCalibrator::update(bool &failure) {
     } else if(_status == COMPASS_CAL_RUNNING_STEP_TWO) {
         if (_fit_step >= 35) {
             if(fit_acceptable()) {
+                calculate_orientation();
                 set_status(COMPASS_CAL_SUCCESS);
             } else {
                 set_status(COMPASS_CAL_FAILED);
@@ -696,3 +699,130 @@ void CompassCalibrator::CompassSample::set(const Vector3f &in) {
     y = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.y);
     z = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.z);
 }
+
+
+void CompassCalibrator::AttitudeSample::set_from_ahrs(void) {
+    const Matrix3f &dcm = AP::ahrs().get_DCM_rotation_body_to_ned();
+    float roll_rad, pitch_rad, yaw_rad;
+    dcm.to_euler(&roll_rad, &pitch_rad, &yaw_rad);
+    roll = constrain_int16(127 * (roll_rad / M_PI), -127, 127);
+    pitch = constrain_int16(127 * (pitch_rad / M_PI_2), -127, 127);
+    yaw = constrain_int16(127 * (yaw_rad / M_PI), -127, 127);
+}
+
+Matrix3f CompassCalibrator::AttitudeSample::get_rotmat(void) {
+    float roll_rad, pitch_rad, yaw_rad;
+    roll_rad = roll * (M_PI / 127);
+    pitch_rad = pitch * (M_PI_2 / 127);
+    yaw_rad = yaw * (M_PI / 127);
+    Matrix3f dcm;
+    dcm.from_euler(roll_rad, pitch_rad, yaw_rad);
+    return dcm;
+}
+
+/*
+  calculate the implied earth field for a compass sample and compass rotation
+ */
+Vector3f CompassCalibrator::calculate_earth_field(CompassSample &sample, enum Rotation r)
+{
+    Vector3f v = sample.get();
+
+    // convert the sample back to sensor frame
+    v.rotate_inverse(_orientation);
+
+    // rotate to body frame for this rotation
+    v.rotate(r);
+
+    Vector3f rot_offsets = _params.offset;
+    rot_offsets.rotate_inverse(_orientation);
+
+    rot_offsets.rotate(r);
+    
+    v += rot_offsets;
+
+    Matrix3f rot = sample.att.get_rotmat();
+
+    Vector3f efield = rot * v;
+    return efield;
+}
+
+/*
+  calculate compass orientation using the attitude estimate associated with each sample
+ */
+void CompassCalibrator::calculate_orientation(void)
+{
+    if (!_auto_orientation) {
+        // only calculate orientation for external compasses
+        return;
+    }
+
+    float sum_error[ROTATION_MAX] {};
+
+    for (enum Rotation r = ROTATION_NONE; r<ROTATION_MAX; r = (enum Rotation)(r+1)) {
+        // calculate the average implied earth field across all samples
+        Vector3f total_ef {};
+        for (uint32_t i=0; i<_samples_collected; i++) {
+            Vector3f efield = calculate_earth_field(_sample_buffer[i], r);
+            total_ef += efield;
+        }
+        Vector3f avg_efield = total_ef / _samples_collected;
+
+        // now calculate the square error for this rotation against the average earth field
+        for (uint32_t i=0; i<_samples_collected; i++) {
+            Vector3f efield = calculate_earth_field(_sample_buffer[i], r);
+            float err = (efield - avg_efield).length_squared();
+            sum_error[r] += err;
+        }
+    }
+
+    // find the rotation with the lowest square error
+    enum Rotation besti = ROTATION_NONE;
+    float bestv = sum_error[0];
+    for (enum Rotation r = ROTATION_NONE; r<ROTATION_MAX; r = (enum Rotation)(r+1)) {
+        if (sum_error[r] < bestv) {
+            bestv = sum_error[r];
+            besti = r;
+        }
+    }
+
+    // consider this a pass if the best orientation is 4x better
+    // square error than 2nd best
+    float second_best = besti==ROTATION_NONE?sum_error[1]:sum_error[0];
+    for (enum Rotation r = ROTATION_NONE; r<ROTATION_MAX; r = (enum Rotation)(r+1)) {
+        if (r != besti) {
+            if (sum_error[r] < second_best) {
+                second_best = sum_error[r];
+            }
+        }
+    }
+    bool pass = (second_best / bestv) > 4;
+    if (!pass) {
+        hal.console->printf("Bad orientation estimation: %u %f\n", besti, second_best/bestv);
+    } else if (besti == _orientation) {
+        // no orientation change
+        hal.console->printf("Good orientation: %u %f\n", besti, second_best/bestv);
+    } else {
+        hal.console->printf("New orientation: %u was %u %f\n", besti, _orientation, second_best/bestv);
+    }
+
+    if (!pass) {
+        return;
+    }
+    
+    if (_orientation != besti) {
+        // correct the offsets for the new orientation
+        Vector3f rot_offsets = _params.offset;
+        rot_offsets.rotate_inverse(_orientation);
+        rot_offsets.rotate(besti);
+        _params.offset = rot_offsets;
+
+        Vector3f &diagonals = _params.diag;
+        Vector3f &offdiagonals = _params.offdiag;
+        // we should calculated the corrected diagonals and off-diagonals here, but
+        // for now just avoid eliptical correction if we're changing the orientation
+        diagonals = Vector3f(1,1,1);
+        offdiagonals.zero();
+    
+        _orientation = besti;
+    }
+}

libraries/AP_Compass/CompassCalibrator.h

@@ -1,3 +1,5 @@
+#pragma once
+
 #include <AP_Math/AP_Math.h>
 
 #define COMPASS_CAL_NUM_SPHERE_PARAMS 4
@@ -32,9 +34,16 @@ public:
 
     bool running() const;
 
+    void set_orientation(enum Rotation orientation, bool is_external) {
+        _auto_orientation = true;
+        _orientation = orientation;
+        _is_external = is_external;
+    }
+    
     void set_tolerance(float tolerance) { _tolerance = tolerance; }
 
     void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals);
+    enum Rotation get_orientation(void) { return _orientation; }
 
     float get_completion_percent() const;
     completion_mask_t& get_completion_mask();
@@ -59,17 +68,31 @@ private:
         Vector3f offdiag;
     };
 
+    // compact class for approximate attitude, to save memory
+    class AttitudeSample {
+    public:
+        Matrix3f get_rotmat();
+        void set_from_ahrs();
+    private:
+        int8_t roll;
+        int8_t pitch;
+        int8_t yaw;
+    };
+
     class CompassSample {
     public:
         Vector3f get() const;
         void set(const Vector3f &in);
+        AttitudeSample att;
     private:
         int16_t x;
         int16_t y;
         int16_t z;
     };
 
-
+    enum Rotation _orientation;
+    bool _is_external;
+    bool _auto_orientation;
 
     enum compass_cal_status_t _status;
 
@@ -136,4 +159,7 @@ private:
      * Reset and update #_completion_mask with the current samples.
      */
     void update_completion_mask();
+
+    Vector3f calculate_earth_field(CompassSample &sample, enum Rotation r);
+    void calculate_orientation();
 };