Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include "AP_Compass_config.h"
       4             : 
       5             : #if COMPASS_CAL_ENABLED
       6             : 
       7             : #include <AP_Math/AP_Math.h>
       8             : 
       9             : #define COMPASS_CAL_NUM_SPHERE_PARAMS       4
      10             : #define COMPASS_CAL_NUM_ELLIPSOID_PARAMS    9
      11             : #define COMPASS_CAL_NUM_SAMPLES             300     // number of samples required before fitting begins
      12             : 
      13             : class CompassCalibrator {
      14             : public:
      15             :     CompassCalibrator();
      16             : 
      17             :     // start or stop the calibration
      18             :     void start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx, float tolerance);
      19             :     void stop();
      20             : 
      21             :     // Update point sample
      22             :     void new_sample(const Vector3f& sample);
      23             : 
      24             :     // set compass's initial orientation and whether it should be automatically fixed (if required)
      25             :     void set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation, bool always_45_deg);
      26             : 
      27             :     // running is true if actively calculating offsets, diagonals or offdiagonals
      28             :     bool running();
      29             : 
      30             :     // failed is true if any terminal failure state is hit
      31             :     bool failed();
      32             : 
      33             : 
      34             :     // update the state machine and calculate offsets, diagonals and offdiagonals
      35             :     void update();
      36             : 
      37             :     // compass calibration states - these correspond to the mavlink
      38             :     // MAG_CAL_STATUS enumeration
      39             :     enum class Status {
      40             :         NOT_STARTED = 0,
      41             :         WAITING_TO_START = 1,
      42             :         RUNNING_STEP_ONE = 2,
      43             :         RUNNING_STEP_TWO = 3,
      44             :         SUCCESS = 4,
      45             :         FAILED = 5,
      46             :         FAILED_ORIENTATION = 6,
      47             :         FAILED_RADIUS = 7,
      48             :         FAILED_OFFSETS = 8,
      49             :         FAILED_DIAG_SCALING = 9,
      50             :         FAILED_RESIDUALS_HIGH = 10,
      51             :     };
      52             : 
      53             :     // get completion mask for mavlink reporting (a bitmask of faces/directions for which we have compass samples)
      54             :     typedef uint8_t completion_mask_t[10];
      55             : 
      56             :     // Structure accessed for cal status update via mavlink 
      57             :     struct State {
      58             :         Status status;
      59             :         uint8_t attempt;
      60             :         float completion_pct;
      61             :         completion_mask_t completion_mask;
      62             :     } cal_state;
      63             : 
      64             :     // Structure accessed after calibration is finished/failed
      65             :     struct Report {
      66             :         Status status;
      67             :         float fitness;
      68             :         Vector3f ofs;
      69             :         Vector3f diag;
      70             :         Vector3f offdiag;
      71             :         float orientation_confidence;
      72             :         Rotation original_orientation;
      73             :         Rotation orientation;
      74             :         float scale_factor;
      75             :         bool check_orientation;
      76             :     } cal_report;
      77             : 
      78             :     // Structure setup to set calibration run settings
      79             :     struct Settings {
      80             :         float tolerance;
      81             :         bool check_orientation;
      82             :         enum Rotation orientation;
      83             :         enum Rotation orig_orientation;
      84             :         bool is_external;
      85             :         bool fix_orientation;
      86             :         uint16_t offset_max;
      87             :         uint8_t attempt;
      88             :         bool retry;
      89             :         float delay_start_sec;
      90             :         uint32_t start_time_ms;
      91             :         uint8_t compass_idx;
      92             :         bool always_45_deg;
      93             :     } cal_settings;
      94             : 
      95             :     // Get calibration result
      96             :     const Report get_report();
      97             :     
      98             :     // Get current Calibration state
      99             :     const State get_state();
     100             : 
     101             : protected:
     102             :     // convert index to rotation, this allows to skip some rotations
     103             :     // protected so CompassCalibrator_index_test can see it
     104             :     Rotation auto_rotation_index(uint8_t n) const;
     105             : 
     106             :     // return true if this is a right angle rotation
     107             :     bool right_angle_rotation(Rotation r) const;
     108             : 
     109             : private:
     110             : 
     111             :     // results
     112             :     class param_t {
     113             :     public:
     114       14720 :         float* get_sphere_params() {
     115       14720 :             return &radius;
     116             :         }
     117             : 
     118       68040 :         float* get_ellipsoid_params() {
     119       68040 :             return &offset.x;
     120             :         }
     121             : 
     122             :         float radius;       // magnetic field strength calculated from samples
     123             :         Vector3f offset;    // offsets
     124             :         Vector3f diag;      // diagonal scaling
     125             :         Vector3f offdiag;   // off diagonal scaling
     126             :         float scale_factor; // scaling factor to compensate for radius error
     127             :     };
     128             : 
     129             :     // compact class for approximate attitude, to save memory
     130             :     class AttitudeSample {
     131             :     public:
     132             :         Matrix3f get_rotmat() const;
     133             :         void set_from_ahrs();
     134             :     private:
     135             :         int8_t roll;
     136             :         int8_t pitch;
     137             :         int8_t yaw;
     138             :     };
     139             : 
     140             :     // compact class to hold compass samples, to save memory
     141             :     class CompassSample {
     142             :     public:
     143             :         Vector3f get() const;
     144             :         void set(const Vector3f &in);
     145             :         AttitudeSample att;
     146             :     private:
     147             :         int16_t x;
     148             :         int16_t y;
     149             :         int16_t z;
     150             :     };
     151             : 
     152             :     // set status including any required initialisation
     153             :     bool set_status(Status status);
     154             : 
     155             :     // consume point raw sample from intermediate structure
     156             :     void pull_sample();
     157             : 
     158             :     // returns true if sample should be added to buffer
     159             :     bool accept_sample(const Vector3f &sample, uint16_t skip_index = UINT16_MAX);
     160             :     bool accept_sample(const CompassSample &sample, uint16_t skip_index = UINT16_MAX);
     161             : 
     162             :     // returns specific failure Status if fit is unacceptable, SUCCESS if acceptable
     163             :     Status fit_acceptable_status() const;
     164             : 
     165             :     // clear sample buffer and reset offsets and scaling to their defaults
     166             :     void reset_state();
     167             : 
     168             :     // initialize fitness before starting a fit
     169             :     void initialize_fit();
     170             : 
     171             :     // true if enough samples have been collected and fitting has begun (aka runniong())
     172             :     bool _fitting() const;
     173             : 
     174             :     // thins out samples between step one and step two
     175             :     void thin_samples();
     176             : 
     177             :     // calc the fitness of a single sample vs a set of parameters (offsets, diagonals, off diagonals)
     178             :     float calc_residual(const Vector3f& sample, const param_t& params) const;
     179             : 
     180             :     // calc the fitness of the parameters (offsets, diagonals, off diagonals) vs all the samples collected
     181             :     // returns 1.0e30f if the sample buffer is empty
     182             :     float calc_mean_squared_residuals(const param_t& params) const;
     183             : 
     184             :     // calculate initial offsets by simply taking the average values of the samples
     185             :     void calc_initial_offset();
     186             : 
     187             :     // run sphere fit to calculate diagonals and offdiagonals
     188             :     void calc_sphere_jacob(const Vector3f& sample, const param_t& params, float* ret) const;
     189             :     void run_sphere_fit();
     190             : 
     191             :     // run ellipsoid fit to calculate diagonals and offdiagonals
     192             :     void calc_ellipsoid_jacob(const Vector3f& sample, const param_t& params, float* ret) const;
     193             :     void run_ellipsoid_fit();
     194             : 
     195             :     // update the completion mask based on a single sample
     196             :     void update_completion_mask(const Vector3f& sample);
     197             : 
     198             :     // reset and updated the completion mask using all samples in the sample buffer
     199             :     void update_completion_mask();
     200             : 
     201             :     // calculate compass orientation
     202             :     Vector3f calculate_earth_field(CompassSample &sample, enum Rotation r);
     203             :     bool calculate_orientation();
     204             : 
     205             :     // fix radius to compensate for sensor scaling errors
     206             :     bool fix_radius();
     207             : 
     208             :     // update methods to read write intermediate structures, called inside thread
     209             :     inline void update_cal_status();
     210             :     inline void update_cal_report();
     211             :     inline void update_cal_settings();
     212             : 
     213             :     // running method for use in thread
     214             :     bool _running() const;
     215             : 
     216             :     uint8_t _compass_idx;                   // index of the compass providing data
     217             :     Status _status;                         // current state of calibrator
     218             : 
     219             :     // values provided by caller
     220             :     float _delay_start_sec;                 // seconds to delay start of calibration (provided by caller)
     221             :     bool _retry;                            // true if calibration should be restarted on failured (provided by caller)
     222             :     float _tolerance = 5.0;                 // worst acceptable RMS tolerance (aka fitness).  see set_tolerance()
     223             :     uint16_t _offset_max;                   // maximum acceptable offsets (provided by caller)
     224             : 
     225             :     // behavioral state
     226             :     uint32_t _start_time_ms;                // system time start() function was last called
     227             :     uint8_t _attempt;                       // number of attempts have been made to calibrate
     228             :     completion_mask_t _completion_mask;     // bitmask of directions in which we have samples
     229             :     CompassSample *_sample_buffer;          // buffer of sensor values
     230             :     uint16_t _samples_collected;            // number of samples in buffer
     231             :     uint16_t _samples_thinned;              // number of samples removed by the thin_samples() call (called before step 2 begins)
     232             : 
     233             :     // fit state
     234             :     class param_t _params;                  // latest calibration outputs
     235             :     uint16_t _fit_step;                     // step during RUNNING_STEP_ONE/TWO which performs sphere fit and ellipsoid fit
     236             :     float _fitness;                         // fitness (mean squared residuals) of current parameters
     237             :     float _initial_fitness;                 // fitness before latest "fit" was attempted (used to determine if fit was an improvement)
     238             :     float _sphere_lambda;                   // sphere fit's lambda
     239             :     float _ellipsoid_lambda;                // ellipsoid fit's lambda
     240             : 
     241             :     // variables for orientation checking
     242             :     enum Rotation _orientation;             // latest detected orientation
     243             :     enum Rotation _orig_orientation;        // original orientation provided by caller
     244             :     enum Rotation _orientation_solution;    // latest solution
     245             :     bool _is_external;                      // true if compass is external (provided by caller)
     246             :     bool _check_orientation;                // true if orientation should be automatically checked
     247             :     bool _fix_orientation;                  // true if orientation should be fixed if necessary
     248             :     bool _always_45_deg;                    // true if orientation should consider 45deg with equal tolerance
     249             :     float _orientation_confidence;          // measure of confidence in automatic orientation detection
     250             :     CompassSample _last_sample;
     251             : 
     252             :     Status _requested_status;
     253             :     bool   _status_set_requested;
     254             :     bool   _retry_pending;
     255             :     uint32_t _retry_hold_start_ms;      // millis() when the FAILED_* status was published (retry hold timer)
     256             : 
     257             :     bool _new_sample;
     258             : 
     259             :     // Semaphore for state related intermediate structures
     260             :     HAL_Semaphore state_sem;
     261             : 
     262             :     // Semaphore for intermediate structure for point sample collection
     263             :     HAL_Semaphore sample_sem;
     264             : };
     265             : 
     266             : #endif  // COMPASS_CAL_ENABLED