Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             :    This program is distributed in the hope that it will be useful,
       7             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       8             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
       9             :    GNU General Public License for more details.
      10             :    You should have received a copy of the GNU General Public License
      11             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      12             : */
      13             : #pragma once
      14             : 
      15             : #include <AP_Math/AP_Math.h>
      16             : #include <AP_Math/vectorN.h>
      17             : 
      18             : #define ACCEL_CAL_MAX_NUM_PARAMS 9
      19             : #define ACCEL_CAL_TOLERANCE 0.1
      20             : #define MAX_ITERATIONS  50
      21             : enum accel_cal_status_t {
      22             :     ACCEL_CAL_NOT_STARTED=0,
      23             :     ACCEL_CAL_WAITING_FOR_ORIENTATION=1,
      24             :     ACCEL_CAL_COLLECTING_SAMPLE=2,
      25             :     ACCEL_CAL_SUCCESS=3,
      26             :     ACCEL_CAL_FAILED=4
      27             : };
      28             : 
      29             : enum accel_cal_fit_type_t {
      30             :     ACCEL_CAL_AXIS_ALIGNED_ELLIPSOID=0,
      31             :     ACCEL_CAL_ELLIPSOID=1
      32             : };
      33             : 
      34             : class AccelCalibrator {
      35             : public:
      36             :     AccelCalibrator();
      37             : 
      38             :     //Select options, initialise variables and initiate accel calibration
      39             :     void start(enum accel_cal_fit_type_t fit_type = ACCEL_CAL_AXIS_ALIGNED_ELLIPSOID, uint8_t num_samples = 6, float sample_time = 0.5f);
      40             :     void start(enum accel_cal_fit_type_t fit_type, uint8_t num_samples, float sample_time, Vector3f offset, Vector3f diag, Vector3f offdiag);
      41             :     
      42             :     // set Accel calibrator status to make itself ready for future accel cals
      43             :     void clear();
      44             : 
      45             :     // returns true if accel calibrator is running
      46             :     bool running();
      47             : 
      48             :     // set Accel calibrator to start collecting samples in the next cycle
      49             :     void collect_sample();
      50             : 
      51             :     // check if client's calibrator is active
      52             :     void check_for_timeout();
      53             : 
      54             :     // get diag,offset or offdiag parameters as per the selected fitting surface or request
      55             :     void get_calibration(Vector3f& offset) const;
      56             :     void get_calibration(Vector3f& offset, Vector3f& diag) const;
      57             :     void get_calibration(Vector3f& offset, Vector3f& diag, Vector3f& offdiag) const;
      58             : 
      59             : 
      60             :     // collect and avg sample to be passed onto LSQ estimator after all requisite orientations are done
      61             :     void new_sample(const Vector3f& delta_velocity, float dt);
      62             : 
      63             :     // interface for LSq estimator to read sample buffer sent after conversion from delta velocity
      64             :     // to averaged acc over time
      65             :     bool get_sample(uint8_t i, Vector3f& s) const;
      66             : 
      67             :     // returns true and sample corrected with diag offdiag parameters as calculated by LSq estimation procedure
      68             :     // returns false if no correct parameter exists to be applied along with existing sample without corrections
      69             :     bool get_sample_corrected(uint8_t i, Vector3f& s) const;
      70             : 
      71             :     // set tolerance bar for parameter fitness value to cross so as to be deemed as correct values
      72             :     void set_tolerance(float tolerance) { _conf_tolerance = tolerance; }
      73             : 
      74             :     // returns current state of accel calibrators
      75       27532 :     enum accel_cal_status_t get_status() const { return _status; }
      76             : 
      77             :     // returns number of samples collected
      78         862 :     uint8_t get_num_samples_collected() const { return _samples_collected; }
      79             : 
      80             :     // returns mean squared fitness of sample points to the selected surface
      81             :     float get_fitness() const { return _fitness; }
      82             : 
      83             :     struct param_t {
      84             :         Vector3f offset;
      85             :         Vector3f diag;
      86             :         Vector3f offdiag;
      87             :     };
      88             : 
      89             : private:
      90             :     struct AccelSample {
      91             :         Vector3f delta_velocity;
      92             :         float delta_time;
      93             :     };
      94             :     typedef    VectorN<float, ACCEL_CAL_MAX_NUM_PARAMS> VectorP;
      95             : 
      96             :     union param_u {
      97             :         struct param_t s;
      98             :         VectorN<float, ACCEL_CAL_MAX_NUM_PARAMS> a;
      99             : 
     100           7 :         param_u() : a{}
     101             :         {
     102             :             static_assert(sizeof(*this) == sizeof(struct param_t),
     103             :                           "Invalid union members: sizes do not match");
     104           7 :         }
     105             :     };
     106             : 
     107             :     //configuration
     108             :     uint8_t _conf_num_samples;
     109             :     float _conf_sample_time;
     110             :     enum accel_cal_fit_type_t _conf_fit_type;
     111             :     float _conf_tolerance;
     112             : 
     113             :     // state
     114             :     accel_cal_status_t _status;
     115             :     struct AccelSample* _sample_buffer;
     116             :     uint8_t _samples_collected;
     117             :     union param_u _param;
     118             :     float _fitness;
     119             :     uint32_t _last_samp_frag_collected_ms;
     120             :     float _min_sample_dist;
     121             : 
     122             :     // private methods
     123             :     // check sanity of including the sample and add it to buffer if test is passed
     124             :     bool accept_sample(const Vector3f& sample);
     125             : 
     126             :     // reset to calibrator state before the start of calibration
     127             :     void reset_state();
     128             : 
     129             :     // sets status of calibrator and takes appropriate actions
     130             :     void set_status(enum accel_cal_status_t);
     131             : 
     132             :     // determines if the result is acceptable
     133             :     bool accept_result() const;
     134             : 
     135             :     // returns number of parameters are required for selected Fit type
     136             :     uint8_t get_num_params() const;
     137             : 
     138             :     // Function related to Gauss Newton Least square regression process
     139             :     float calc_residual(const Vector3f& sample, const struct param_t& params) const;
     140             :     float calc_mean_squared_residuals(const struct param_t& params) const;
     141             :     void calc_jacob(const Vector3f& sample, const struct param_t& params, VectorP& ret) const;
     142             :     void run_fit(uint8_t max_iterations, float& fitness);
     143             : };