AP_Compass: add comments to calibrator

libraries/AP_Compass/CompassCalibrator.cpp

@@ -81,6 +81,15 @@ void CompassCalibrator::clear()
     set_status(COMPASS_CAL_NOT_STARTED);
 }
 
+void CompassCalibrator::set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation)
+{
+    _check_orientation = true;
+    _orientation = orientation;
+    _orig_orientation = orientation;
+    _is_external = is_external;
+    _fix_orientation = fix_orientation;
+}
+
 void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx)
 {
     if (running()) {
@@ -127,6 +136,8 @@ float CompassCalibrator::get_completion_percent() const
     };
 }
 
+// update completion mask based on latest sample
+// used to ensure we have collected samples in all directions
 void CompassCalibrator::update_completion_mask(const Vector3f& v)
 {
     Matrix3f softiron {
@@ -142,6 +153,7 @@ void CompassCalibrator::update_completion_mask(const Vector3f& v)
     _completion_mask[section / 8] |= 1 << (section % 8);
 }
 
+// reset and updated the completion mask using all samples in the sample buffer
 void CompassCalibrator::update_completion_mask()
 {
     memset(_completion_mask, 0, sizeof(_completion_mask));
@@ -181,6 +193,7 @@ void CompassCalibrator::update(bool &failure)
 {
     failure = false;
 
+    // collect the minimum number of samples
     if (!fitting()) {
         return;
     }
@@ -230,17 +243,17 @@ bool CompassCalibrator::fitting() const
     return running() && (_samples_collected == COMPASS_CAL_NUM_SAMPLES);
 }
 
+// initialize fitness before starting a fit
 void CompassCalibrator::initialize_fit()
 {
-    //initialize _fitness before starting a fit
     if (_samples_collected != 0) {
         _fitness = calc_mean_squared_residuals(_params);
     } else {
         _fitness = 1.0e30f;
     }
-    _ellipsoid_lambda = 1.0f;
-    _sphere_lambda = 1.0f;
     _initial_fitness = _fitness;
+    _sphere_lambda = 1.0f;
+    _ellipsoid_lambda = 1.0f;
     _fit_step = 0;
 }
 
@@ -284,6 +297,7 @@ bool CompassCalibrator::set_status(compass_cal_status_t status)
                 return false;
             }
 
+            // on first attempt delay start if requested by caller
             if (_attempt == 1 && (AP_HAL::millis()-_start_time_ms)*1.0e-3f < _delay_start_sec) {
                 return false;
             }
@@ -452,6 +466,7 @@ float CompassCalibrator::calc_mean_squared_residuals() const
     return calc_mean_squared_residuals(_params);
 }
 
+// calc the fitness given a set of parameters (offsets, diagonals, off diagonals)
 float CompassCalibrator::calc_mean_squared_residuals(const param_t& params) const
 {
     if (_sample_buffer == nullptr || _samples_collected == 0) {
@@ -467,6 +482,17 @@ float CompassCalibrator::calc_mean_squared_residuals(const param_t& params) cons
     return sum;
 }
 
+// calculate initial offsets by simply taking the average values of the samples
+void CompassCalibrator::calc_initial_offset()
+{
+    // Set initial offset to the average value of the samples
+    _params.offset.zero();
+    for (uint16_t k = 0; k < _samples_collected; k++) {
+        _params.offset -= _sample_buffer[k].get();
+    }
+    _params.offset /= _samples_collected;
+}
+
 void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const param_t& params, float* ret) const
 {
     const Vector3f &offset = params.offset;
@@ -491,16 +517,7 @@ void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const param_t&
     ret[3] = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z    * C))/length);
 }
 
-void CompassCalibrator::calc_initial_offset()
-{
-    // Set initial offset to the average value of the samples
-    _params.offset.zero();
-    for (uint16_t k = 0; k<_samples_collected; k++) {
-        _params.offset -= _sample_buffer[k].get();
-    }
-    _params.offset /= _samples_collected;
-}
-
+// run sphere fit to calculate diagonals and offdiagonals
 void CompassCalibrator::run_sphere_fit()
 {
     if (_sample_buffer == nullptr) {
@@ -509,6 +526,7 @@ void CompassCalibrator::run_sphere_fit()
 
     const float lma_damping = 10.0f;
 
+    // take backup of fitness and parameters so we can determine later if this fit has improved the calibration
     float fitness = _fitness;
     float fit1, fit2;
     param_t fit1_params, fit2_params;
@@ -538,7 +556,7 @@ void CompassCalibrator::run_sphere_fit()
     }
 
     //------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
-    //refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
+    // refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
     for (uint8_t i = 0; i < COMPASS_CAL_NUM_SPHERE_PARAMS; i++) {
         JTJ[i*COMPASS_CAL_NUM_SPHERE_PARAMS+i] += _sphere_lambda;
         JTJ2[i*COMPASS_CAL_NUM_SPHERE_PARAMS+i] += _sphere_lambda/lma_damping;
@@ -552,6 +570,7 @@ void CompassCalibrator::run_sphere_fit()
         return;
     }
 
+    // extract radius, offset, diagonals and offdiagonal parameters
     for (uint8_t row=0; row < COMPASS_CAL_NUM_SPHERE_PARAMS; row++) {
         for (uint8_t col=0; col < COMPASS_CAL_NUM_SPHERE_PARAMS; col++) {
             fit1_params.get_sphere_params()[row] -= JTFI[col] * JTJ[row*COMPASS_CAL_NUM_SPHERE_PARAMS+col];
@@ -559,12 +578,16 @@ void CompassCalibrator::run_sphere_fit()
         }
     }
 
+    // calculate fitness of two possible sets of parameters
     fit1 = calc_mean_squared_residuals(fit1_params);
     fit2 = calc_mean_squared_residuals(fit2_params);
 
+    // decide which of the two sets of parameters is best and store in fit1_params
     if (fit1 > _fitness && fit2 > _fitness) {
+        // if neither set of parameters provided better results, increase lambda
         _sphere_lambda *= lma_damping;
     } else if (fit2 < _fitness && fit2 < fit1) {
+        // if fit2 was better we will use it. decrease lambda
         _sphere_lambda /= lma_damping;
         fit1_params = fit2_params;
         fitness = fit2;
@@ -573,6 +596,7 @@ void CompassCalibrator::run_sphere_fit()
     }
     //--------------------Levenberg-Marquardt-part-ends-here--------------------------------//
 
+    // store new parameters and update fitness
     if (!isnan(fitness) && fitness < _fitness) {
         _fitness = fitness;
         _params = fit1_params;
@@ -618,6 +642,7 @@ void CompassCalibrator::run_ellipsoid_fit()
 
     const float lma_damping = 10.0f;
 
+    // take backup of fitness and parameters so we can determine later if this fit has improved the calibration
     float fitness = _fitness;
     float fit1, fit2;
     param_t fit1_params, fit2_params;
@@ -661,6 +686,7 @@ void CompassCalibrator::run_ellipsoid_fit()
         return;
     }
 
+    // extract radius, offset, diagonals and offdiagonal parameters
     for (uint8_t row=0; row < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; row++) {
         for (uint8_t col=0; col < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; col++) {
             fit1_params.get_ellipsoid_params()[row] -= JTFI[col] * JTJ[row*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+col];
@@ -668,12 +694,16 @@ void CompassCalibrator::run_ellipsoid_fit()
         }
     }
 
+    // calculate fitness of two possible sets of parameters
     fit1 = calc_mean_squared_residuals(fit1_params);
     fit2 = calc_mean_squared_residuals(fit2_params);
 
+    // decide which of the two sets of parameters is best and store in fit1_params
     if (fit1 > _fitness && fit2 > _fitness) {
+        // if neither set of parameters provided better results, increase lambda
         _ellipsoid_lambda *= lma_damping;
     } else if (fit2 < _fitness && fit2 < fit1) {
+        // if fit2 was better we will use it. decrease lambda
         _ellipsoid_lambda /= lma_damping;
         fit1_params = fit2_params;
         fitness = fit2;
@@ -682,6 +712,7 @@ void CompassCalibrator::run_ellipsoid_fit()
     }
     //--------------------Levenberg-part-ends-here--------------------------------//
 
+    // store new parameters and update fitness
     if (fitness < _fitness) {
         _fitness = fitness;
         _params = fit1_params;

libraries/AP_Compass/CompassCalibrator.h

@@ -2,13 +2,12 @@
 
 #include <AP_Math/AP_Math.h>
 
-#define COMPASS_CAL_NUM_SPHERE_PARAMS 4
-#define COMPASS_CAL_NUM_ELLIPSOID_PARAMS 9
-#define COMPASS_CAL_NUM_SAMPLES 300
-
-//RMS tolerance
-#define COMPASS_CAL_DEFAULT_TOLERANCE 5.0f
+#define COMPASS_CAL_NUM_SPHERE_PARAMS       4
+#define COMPASS_CAL_NUM_ELLIPSOID_PARAMS    9
+#define COMPASS_CAL_NUM_SAMPLES             300     // number of samples required before fitting begins
+#define COMPASS_CAL_DEFAULT_TOLERANCE       5.0f    // default RMS tolerance
 
+// compass calibration states
 enum compass_cal_status_t {
     COMPASS_CAL_NOT_STARTED = 0,
     COMPASS_CAL_WAITING_TO_START = 1,
@@ -21,42 +20,52 @@ enum compass_cal_status_t {
 
 class CompassCalibrator {
 public:
-    typedef uint8_t completion_mask_t[10];
-
     CompassCalibrator();
 
+    // set tolerance of calibration (aka fitness)
+    void set_tolerance(float tolerance) { _tolerance = tolerance; }
+
+    // set compass's initial orientation and whether it should be automatically fixed (if required)
+    void set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation);
+
+    // start or stop the calibration
     void start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx);
     void clear();
 
+    // update the state machine and calculate offsets, diagonals and offdiagonals
     void update(bool &failure);
     void new_sample(const Vector3f &sample);
 
     bool check_for_timeout();
 
+    // running is true if actively calculating offsets, diagonals or offdiagonals
     bool running() const;
 
-    void set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation) {
-        _check_orientation = true;
-        _orientation = orientation;
-        _orig_orientation = orientation;
-        _is_external = is_external;
-        _fix_orientation = fix_orientation;
-    }
-
-    void set_tolerance(float tolerance) { _tolerance = tolerance; }
+    // get status of calibrations progress
+    enum compass_cal_status_t get_status() const { return _status; }
 
+    // get calibration outputs (offsets, diagonals, offdiagonals) and fitness
     void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals);
+    float get_fitness() const { return sqrtf(_fitness); }
+
+    // get corrected (and original) orientation
     enum Rotation get_orientation() const { return _orientation; }
     enum Rotation get_original_orientation() const { return _orig_orientation; }
+    float get_orientation_confidence() const { return _orientation_confidence; }
 
+    // get completion percentage (0 to 100) for reporting to GCS
     float get_completion_percent() const;
-    const completion_mask_t& get_completion_mask() const { return _completion_mask; }
-    enum compass_cal_status_t get_status() const { return _status; }
-    float get_fitness() const { return sqrtf(_fitness); }
-    float get_orientation_confidence() const { return _orientation_confidence; }
+
+    // get how many attempts have been made to calibrate for reporting to GCS
     uint8_t get_attempt() const { return _attempt; }
 
+    // get completion mask for mavlink reporting (a bitmask of faces/directions for which we have compass samples)
+    typedef uint8_t completion_mask_t[10];
+    const completion_mask_t& get_completion_mask() const { return _completion_mask; }
+
 private:
+
+    // results
     class param_t {
     public:
         float* get_sphere_params() {
@@ -67,10 +76,10 @@ private:
             return &offset.x;
         }
 
-        float radius;
-        Vector3f offset;
-        Vector3f diag;
-        Vector3f offdiag;
+        float radius;       // magnetic field strength calculated from samples
+        Vector3f offset;    // offsets
+        Vector3f diag;      // diagonal scaling
+        Vector3f offdiag;   // off diagonal scaling
     };
 
     // compact class for approximate attitude, to save memory
@@ -84,6 +93,7 @@ private:
         int8_t yaw;
     };
 
+    // compact class to hold compass samples, to save memory
     class CompassSample {
     public:
         Vector3f get() const;
@@ -95,40 +105,7 @@ private:
         int16_t z;
     };
 
-    enum Rotation _orientation;
-    enum Rotation _orig_orientation;
-    bool _is_external;
-    bool _check_orientation;
-    bool _fix_orientation;
-    uint8_t _compass_idx;
-
-    enum compass_cal_status_t _status;
-
-    // timeout watchdog state
-    uint32_t _last_sample_ms;
-
-    // behavioral state
-    float _delay_start_sec;
-    uint32_t _start_time_ms;
-    bool _retry;
-    float _tolerance;
-    uint8_t _attempt;
-    uint16_t _offset_max;
-
-    completion_mask_t _completion_mask;
-
-    //fit state
-    class param_t _params;
-    uint16_t _fit_step;
-    CompassSample *_sample_buffer;
-    float _fitness; // mean squared residuals
-    float _initial_fitness;
-    float _sphere_lambda;
-    float _ellipsoid_lambda;
-    uint16_t _samples_collected;
-    uint16_t _samples_thinned;
-    float _orientation_confidence;
-
+    // set status including any required initialisation
     bool set_status(compass_cal_status_t status);
 
     // returns true if sample should be added to buffer
@@ -138,37 +115,78 @@ private:
     // returns true if fit is acceptable
     bool fit_acceptable();
 
+    // clear sample buffer and reset offsets and scaling to their defaults
     void reset_state();
+
+    // initialize fitness before starting a fit
     void initialize_fit();
 
+    // true if enough samples have been collected and fitting has begun (aka runniong())
     bool fitting() const;
 
     // thins out samples between step one and step two
     void thin_samples();
 
+    // calc the fitness of a single sample vs a set of parameters (offsets, diagonals, off diagonals)
     float calc_residual(const Vector3f& sample, const param_t& params) const;
+
+    // calc the fitness of the parameters (offsets, diagonals, off diagonals) vs all the samples collected
+    // returns 1.0e30f if the sample buffer is empty
     float calc_mean_squared_residuals(const param_t& params) const;
     float calc_mean_squared_residuals() const;
 
+    // calculate initial offsets by simply taking the average values of the samples
     void calc_initial_offset();
+
+    // run sphere fit to calculate diagonals and offdiagonals
     void calc_sphere_jacob(const Vector3f& sample, const param_t& params, float* ret) const;
     void run_sphere_fit();
 
+    // run ellipsoid fit to calculate diagonals and offdiagonals
     void calc_ellipsoid_jacob(const Vector3f& sample, const param_t& params, float* ret) const;
     void run_ellipsoid_fit();
 
-    /**
-     * Update #_completion_mask for the geodesic section of \p v. Corrections
-     * are applied to \p v with #_params.
-     *
-     * @param v[in] A vector representing one calibration sample.
-     */
-    void update_completion_mask(const Vector3f& v);
-    /**
-     * Reset and update #_completion_mask with the current samples.
-     */
+    // update the completion mask based on a single sample
+    void update_completion_mask(const Vector3f& sample);
+
+    // reset and updated the completion mask using all samples in the sample buffer
     void update_completion_mask();
 
+    // calculate compass orientation
     Vector3f calculate_earth_field(CompassSample &sample, enum Rotation r);
     bool calculate_orientation();
+
+    uint8_t _compass_idx;                   // index of the compass providing data
+    enum compass_cal_status_t _status;      // current state of calibrator
+    uint32_t _last_sample_ms;               // system time of last sample received for timeout
+
+    // values provided by caller
+    float _delay_start_sec;                 // seconds to delay start of calibration (provided by caller)
+    bool _retry;                            // true if calibration should be restarted on failured (provided by caller)
+    float _tolerance;                       // worst acceptable tolerance (aka fitness).  see set_tolerance()
+    uint16_t _offset_max;                   // maximum acceptable offsets (provided by caller)
+
+    // behavioral state
+    uint32_t _start_time_ms;                // system time start() function was last called
+    uint8_t _attempt;                       // number of attempts have been made to calibrate
+    completion_mask_t _completion_mask;     // bitmask of directions in which we have samples
+    CompassSample *_sample_buffer;          // buffer of sensor values
+    uint16_t _samples_collected;            // number of samples in buffer
+    uint16_t _samples_thinned;              // number of samples removed by the thin_samples() call (called before step 2 begins)
+
+    // fit state
+    class param_t _params;                  // latest calibration outputs
+    uint16_t _fit_step;                     // step during RUNNING_STEP_ONE/TWO which performs sphere fit and ellipsoid fit
+    float _fitness;                         // fitness (mean squared residuals) of current parameters
+    float _initial_fitness;                 // fitness before latest "fit" was attempted (used to determine if fit was an improvement)
+    float _sphere_lambda;                   // sphere fit's lambda
+    float _ellipsoid_lambda;                // ellipsoid fit's lambda
+
+    // variables for orientation checking
+    enum Rotation _orientation;             // latest detected orientation
+    enum Rotation _orig_orientation;        // original orientation provided by caller
+    bool _is_external;                      // true if compass is external (provided by caller)
+    bool _check_orientation;                // true if orientation should be automatically checked
+    bool _fix_orientation;                  // true if orientation should be fixed if necessary
+    float _orientation_confidence;          // measure of confidence in automatic orientation detection
 };