ardupilot/libraries/AP_Compass/Compass_PerMotor.h

70 lines
1.4 KiB
C++

/*
per-motor compass compensation
*/
#include <AP_Math/AP_Math.h>
class Compass;
// per-motor compass compensation class. Currently tied to quadcopters
// only, and single magnetometer
class Compass_PerMotor {
public:
static const struct AP_Param::GroupInfo var_info[];
Compass_PerMotor(Compass &_compass);
bool enabled(void) const {
return enable.get() != 0;
}
void set_voltage(float _voltage) {
// simple low-pass on voltage
voltage = 0.9 * voltage + 0.1 * _voltage;
}
void calibration_start(void);
void calibration_update(void);
void calibration_end(void);
void compensate(Vector3f &offset);
private:
Compass &compass;
AP_Int8 enable;
AP_Float expo;
AP_Vector3f compensation[4];
// base field on test start
Vector3f base_field;
// sum of calibration field samples
Vector3f field_sum[4];
// sum of output (voltage*scaledpwm) in calibration
float output_sum[4];
// count of calibration accumulation
uint16_t count[4];
// time a motor started in milliseconds
uint32_t start_ms[4];
// battery voltage
float voltage;
// what rcout channel is being calibrated
uint8_t channel;
// is calibration running?
bool running;
// get scaled motor ouput
float scaled_output(uint8_t motor);
// map of motors
bool have_motor_map;
uint8_t motor_map[4];
};