Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_Param/AP_Param.h>
       4             : #include <AP_Math/AP_Math.h>
       5             : #include <AP_Math/polyfit.h>
       6             : #include <Filter/LowPassFilter2p.h>
       7             : 
       8             : // AP_InertialSensor_TCal class is public for use by SITL
       9             : class AP_InertialSensor_TCal {
      10             : public:
      11             :     static const struct AP_Param::GroupInfo var_info[];
      12             :     void correct_accel(float temperature, float cal_temp, Vector3f &accel) const;
      13             :     void correct_gyro(float temperature, float cal_temp, Vector3f &accel) const;
      14             :     void sitl_apply_accel(float temperature, Vector3f &accel) const;
      15             :     void sitl_apply_gyro(float temperature, Vector3f &accel) const;
      16             : 
      17             :     void update_accel_learning(const Vector3f &accel);
      18             :     void update_gyro_learning(const Vector3f &accel);
      19             : 
      20             :     enum class Enable : uint8_t {
      21             :         Disabled = 0,
      22             :         Enabled = 1,
      23             :         LearnCalibration = 2,
      24             :     };
      25             : 
      26             :     // add samples for learning
      27             :     void update_accel_learning(const Vector3f &gyro, float temperature);
      28             :     void update_gyro_learning(const Vector3f &accel, float temperature);
      29             :     
      30             :     // class for online learning of calibration
      31             :     class Learn {
      32             :     public:
      33             :         Learn(AP_InertialSensor_TCal &_tcal, float _start_temp);
      34             : 
      35             :         // state for accel/gyro (accel first)
      36             :         struct LearnState {
      37             :             float last_temp;
      38             :             uint32_t last_sample_ms;
      39             :             Vector3f sum;
      40             :             uint32_t sum_count;
      41             :             LowPassFilter2p<float> temp_filter;
      42             :             // double precision is needed for good results when we
      43             :             // span a wide range of temperatures
      44             :             PolyFit<4, double, Vector3f> pfit;
      45             :         } state[2];
      46             : 
      47             :         void add_sample(const Vector3f &sample, float temperature, LearnState &state);
      48             :         void finish_calibration(float temperature);
      49             :         bool save_calibration(float temperature);
      50             :         void reset(float temperature);
      51             :         float start_temp;
      52             :         float start_tmax;
      53             :         uint32_t last_save_ms;
      54             : 
      55             :         AP_InertialSensor_TCal &tcal;
      56         308 :         uint8_t instance(void) const {
      57         308 :             return tcal.instance();
      58             :         }
      59             :         Vector3f accel_start;
      60             :     };
      61             : 
      62             :     AP_Enum<Enable> enable;
      63             : 
      64             :     // get persistent params for this instance
      65             :     void get_persistent_params(ExpandingString &str) const;
      66             : 
      67             : private:
      68             :     AP_Float temp_max;
      69             :     AP_Float temp_min;
      70             :     AP_Vector3f accel_coeff[3];
      71             :     AP_Vector3f gyro_coeff[3];
      72             :     Vector3f accel_tref;
      73             :     Vector3f gyro_tref;
      74             :     Learn *learn;
      75             : 
      76             :     void correct_sensor(float temperature, float cal_temp, const AP_Vector3f coeff[3], Vector3f &v) const;
      77             :     Vector3f polynomial_eval(float temperature, const AP_Vector3f coeff[3]) const;
      78             : 
      79             :     // get instance number
      80             :     uint8_t instance(void) const;
      81             : };