mirror of https://github.com/ArduPilot/ardupilot
125 lines
3.0 KiB
C++
125 lines
3.0 KiB
C++
#include <AP_Math/AP_Math.h>
|
|
|
|
#define COMPASS_CAL_NUM_SPHERE_PARAMS 4
|
|
#define COMPASS_CAL_NUM_ELLIPSOID_PARAMS 9
|
|
#define COMPASS_CAL_NUM_SAMPLES 300
|
|
|
|
//RMS tolerance
|
|
#define COMPASS_CAL_DEFAULT_TOLERANCE 5.0f
|
|
|
|
enum compass_cal_status_t {
|
|
COMPASS_CAL_NOT_STARTED=0,
|
|
COMPASS_CAL_WAITING_TO_START=1,
|
|
COMPASS_CAL_RUNNING_STEP_ONE=2,
|
|
COMPASS_CAL_RUNNING_STEP_TWO=3,
|
|
COMPASS_CAL_SUCCESS=4,
|
|
COMPASS_CAL_FAILED=5
|
|
};
|
|
|
|
class CompassCalibrator {
|
|
public:
|
|
CompassCalibrator();
|
|
|
|
void start(bool retry=false, bool autosave=false, float delay=0.0f);
|
|
void clear();
|
|
|
|
void update(bool &failure);
|
|
void new_sample(const Vector3f &sample);
|
|
|
|
bool check_for_timeout();
|
|
|
|
bool running() const;
|
|
|
|
void set_tolerance(float tolerance) { _tolerance = tolerance; }
|
|
|
|
void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals);
|
|
|
|
float get_completion_percent() const;
|
|
enum compass_cal_status_t get_status() const { return _status; }
|
|
float get_fitness() const { return sqrtf(_fitness); }
|
|
bool get_autosave() const { return _autosave; }
|
|
uint8_t get_attempt() const { return _attempt; }
|
|
|
|
private:
|
|
class param_t {
|
|
public:
|
|
float* get_sphere_params() {
|
|
return &radius;
|
|
}
|
|
|
|
float* get_ellipsoid_params() {
|
|
return &offset.x;
|
|
}
|
|
|
|
float radius;
|
|
Vector3f offset;
|
|
Vector3f diag;
|
|
Vector3f offdiag;
|
|
};
|
|
|
|
class CompassSample {
|
|
public:
|
|
Vector3f get() const;
|
|
void set(const Vector3f &in);
|
|
private:
|
|
int16_t x;
|
|
int16_t y;
|
|
int16_t z;
|
|
};
|
|
|
|
|
|
|
|
enum compass_cal_status_t _status;
|
|
|
|
// timeout watchdog state
|
|
uint32_t _last_sample_ms;
|
|
|
|
// behavioral state
|
|
float _delay_start_sec;
|
|
uint32_t _start_time_ms;
|
|
bool _autosave;
|
|
bool _retry;
|
|
float _tolerance;
|
|
uint8_t _attempt;
|
|
|
|
//fit state
|
|
struct param_t _params;
|
|
uint16_t _fit_step;
|
|
CompassSample *_sample_buffer;
|
|
float _fitness; // mean squared residuals
|
|
float _initial_fitness;
|
|
float _sphere_lambda;
|
|
float _ellipsoid_lambda;
|
|
uint16_t _samples_collected;
|
|
uint16_t _samples_thinned;
|
|
|
|
bool set_status(compass_cal_status_t status);
|
|
|
|
// returns true if sample should be added to buffer
|
|
bool accept_sample(const Vector3f &sample);
|
|
bool accept_sample(const CompassSample &sample);
|
|
|
|
// returns true if fit is acceptable
|
|
bool fit_acceptable();
|
|
|
|
void reset_state();
|
|
void initialize_fit();
|
|
|
|
bool fitting() const;
|
|
|
|
// thins out samples between step one and step two
|
|
void thin_samples();
|
|
|
|
float calc_residual(const Vector3f& sample, const param_t& params) const;
|
|
float calc_mean_squared_residuals(const param_t& params) const;
|
|
float calc_mean_squared_residuals() const;
|
|
|
|
void calc_sphere_jacob(const Vector3f& sample, const param_t& params, float* ret) const;
|
|
void run_sphere_fit();
|
|
|
|
void calc_ellipsoid_jacob(const Vector3f& sample, const param_t& params, float* ret) const;
|
|
void run_ellipsoid_fit();
|
|
|
|
uint16_t get_random();
|
|
};
|