Line data    Source code

       1             : /*
       2             :   per-motor compass compensation
       3             :  */
       4             : 
       5             : #include <AP_Math/AP_Math.h>
       6             : 
       7             : 
       8             : class Compass;
       9             : 
      10             : // per-motor compass compensation class. Currently tied to quadcopters
      11             : // only, and single magnetometer
      12             : class Compass_PerMotor {
      13             : public:
      14             :     static const struct AP_Param::GroupInfo var_info[];
      15             : 
      16             :     Compass_PerMotor(Compass &_compass);
      17             : 
      18    46650409 :     bool enabled(void) const {
      19    46650409 :         return enable.get() != 0;
      20             :     }
      21             :     
      22      759512 :     void set_voltage(float _voltage) {
      23             :         // simple low-pass on voltage
      24      759512 :         voltage = 0.9f * voltage + 0.1f * _voltage;
      25      759512 :     }
      26             : 
      27             :     void calibration_start(void);
      28             :     void calibration_update(void);
      29             :     void calibration_end(void);
      30             :     void compensate(Vector3f &offset);
      31             :     
      32             : private:
      33             :     Compass &compass;
      34             :     AP_Int8 enable;
      35             :     AP_Float expo;
      36             :     AP_Vector3f compensation[4];
      37             : 
      38             :     // base field on test start
      39             :     Vector3f base_field;
      40             :         
      41             :     // sum of calibration field samples
      42             :     Vector3f field_sum[4];
      43             : 
      44             :     // sum of output (voltage*scaledpwm) in calibration
      45             :     float output_sum[4];
      46             :         
      47             :     // count of calibration accumulation
      48             :     uint16_t count[4];
      49             : 
      50             :     // time a motor started in milliseconds
      51             :     uint32_t start_ms[4];
      52             :         
      53             :     // battery voltage
      54             :     float voltage;
      55             : 
      56             :     // is calibration running?
      57             :     bool running;
      58             : 
      59             :     // get scaled motor ouput
      60             :     float scaled_output(uint8_t motor);
      61             : 
      62             :     // map of motors
      63             :     bool have_motor_map;
      64             :     uint8_t motor_map[4];
      65             : };
      66             :