Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_Math/AP_Math.h>
       4             : #include <AP_Logger/AP_Logger_config.h>
       5             : 
       6             : #define AP_OPTICALFLOW_CAL_MAX_SAMPLES 50  // number of samples required before calibration begins
       7             : 
       8             : class AP_OpticalFlow_Calibrator {
       9             : public:
      10           1 :     AP_OpticalFlow_Calibrator() {};
      11             : 
      12             :     // start or stop the calibration
      13             :     void start();
      14             :     void stop();
      15             : 
      16             :     // update the state machine and calculate scaling
      17             :     // returns true if new scaling values have been found
      18             :     bool update();
      19             : 
      20             :     // get final scaling values
      21             :     // scaling values used during sample collection should be multiplied by these scalars
      22             :     Vector2f get_scalars();
      23             : 
      24             : private:
      25             : 
      26             :     // single sample for a single axis
      27             :     struct sample_t {
      28             :         float flow_rate;
      29             :         float body_rate;
      30             :         float los_pred;
      31             :     };
      32             : 
      33             :     // attempt to add a new sample to the buffer
      34             :     void add_sample(uint32_t timestamp_ms, const Vector2f& flow_rate, const Vector2f& body_rate, const Vector2f& los_pred);
      35             : 
      36             :     // returns true once the sample buffer is full
      37             :     bool sample_buffers_full() const;
      38             : 
      39             :     // run calibration algorithm for both axis
      40             :     // returns true on success and updates _cal_data[0,1].best_scale and best_scale_fitness
      41             :     bool run_calibration();
      42             : 
      43             :     // Run fitting algorithm for all samples of the given axis
      44             :     // returns a scalar and fitness (lower numbers mean a better result) in the arguments provided
      45             :     bool calc_scalars(uint8_t axis, float& scalar, float& fitness);
      46             : 
      47             :     // calculate a single sample's residual given a scalar parameter
      48             :     float calc_sample_residual(const sample_t& sample, float scalar) const;
      49             : 
      50             :     // calculate the scalar that minimises the residual for a single sample
      51             :     // returns true on success and populates the best_scalar argument
      52             :     bool calc_sample_best_scalar(const sample_t& sample, float& best_scalar) const;
      53             : 
      54             :     // calculate mean squared residual for all samples of a single axis (0 or 1) given a scalar parameter
      55             :     float calc_mean_squared_residuals(uint8_t axis, float scalar) const;
      56             : 
      57             : #if HAL_LOGGING_ENABLED
      58             :     // log a sample
      59             :     void log_sample(uint8_t axis, uint8_t sample_num, float flow_rate, float body_rate, float los_pred);
      60             : #endif
      61             : 
      62             :     // calibration states
      63             :     enum class CalState {
      64             :         NOT_STARTED = 0,
      65             :         RUNNING,            // collecting samples
      66             :         READY_TO_CALIBRATE, // ready to calibrate (may wait until vehicle is disarmed)
      67             :         SUCCESS,
      68             :         FAILED
      69             :     } _cal_state;
      70             : 
      71             :     // local variables
      72             :     uint32_t _start_time_ms;                                // time the calibration was started
      73             :     struct {
      74             :         sample_t samples[AP_OPTICALFLOW_CAL_MAX_SAMPLES];   // buffer of sensor samples
      75             :         uint8_t num_samples;                                // number of samples in samples buffer
      76             :         float best_scalar;                                  // best scaling value found so far
      77             :         float best_scalar_fitness;                          // fitness (rms of error) of best scaling value
      78             :     } _cal_data[2];                                         // x and y axis
      79             :     uint32_t _last_sample_timestamp_ms;                     // system time of last sample's timestamp, used to ignore duplicates
      80             :     uint32_t _last_report_ms;                               // system time of last status report
      81             : };