2018-07-16 05:21:53 -03:00
|
|
|
#pragma once
|
|
|
|
|
2015-08-11 03:28:42 -03:00
|
|
|
#include <AP_Math/AP_Math.h>
|
2015-03-07 11:49:11 -04:00
|
|
|
|
|
|
|
#define COMPASS_CAL_NUM_SPHERE_PARAMS 4
|
2015-03-09 11:37:12 -03:00
|
|
|
#define COMPASS_CAL_NUM_ELLIPSOID_PARAMS 9
|
2015-03-07 11:49:11 -04:00
|
|
|
#define COMPASS_CAL_NUM_SAMPLES 300
|
|
|
|
|
|
|
|
//RMS tolerance
|
|
|
|
#define COMPASS_CAL_DEFAULT_TOLERANCE 5.0f
|
|
|
|
|
|
|
|
enum compass_cal_status_t {
|
2019-11-14 22:14:24 -04:00
|
|
|
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,
|
|
|
|
COMPASS_CAL_BAD_ORIENTATION = 6,
|
2015-03-07 11:49:11 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
class CompassCalibrator {
|
|
|
|
public:
|
2016-04-22 19:21:18 -03:00
|
|
|
typedef uint8_t completion_mask_t[10];
|
|
|
|
|
2015-03-07 11:49:11 -04:00
|
|
|
CompassCalibrator();
|
|
|
|
|
2018-07-17 03:11:28 -03:00
|
|
|
void start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx);
|
2015-03-07 11:49:11 -04:00
|
|
|
void clear();
|
|
|
|
|
2015-03-18 21:48:46 -03:00
|
|
|
void update(bool &failure);
|
2015-03-07 11:49:11 -04:00
|
|
|
void new_sample(const Vector3f &sample);
|
|
|
|
|
2015-03-09 23:59:57 -03:00
|
|
|
bool check_for_timeout();
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2015-03-09 22:54:05 -03:00
|
|
|
bool running() const;
|
|
|
|
|
2018-07-17 21:09:21 -03:00
|
|
|
void set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation) {
|
|
|
|
_check_orientation = true;
|
2018-07-16 05:21:53 -03:00
|
|
|
_orientation = orientation;
|
2018-07-17 03:11:28 -03:00
|
|
|
_orig_orientation = orientation;
|
2018-07-16 05:21:53 -03:00
|
|
|
_is_external = is_external;
|
2018-07-17 21:09:21 -03:00
|
|
|
_fix_orientation = fix_orientation;
|
2018-07-16 05:21:53 -03:00
|
|
|
}
|
2019-11-14 22:14:24 -04:00
|
|
|
|
2015-03-07 11:49:11 -04:00
|
|
|
void set_tolerance(float tolerance) { _tolerance = tolerance; }
|
|
|
|
|
|
|
|
void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals);
|
2019-11-15 00:02:05 -04:00
|
|
|
enum Rotation get_orientation() const { return _orientation; }
|
|
|
|
enum Rotation get_original_orientation() const { return _orig_orientation; }
|
2015-03-07 11:49:11 -04:00
|
|
|
|
|
|
|
float get_completion_percent() const;
|
2019-11-14 22:15:47 -04:00
|
|
|
const completion_mask_t& get_completion_mask() const { return _completion_mask; }
|
2015-03-07 11:49:11 -04:00
|
|
|
enum compass_cal_status_t get_status() const { return _status; }
|
|
|
|
float get_fitness() const { return sqrtf(_fitness); }
|
2018-07-17 03:11:28 -03:00
|
|
|
float get_orientation_confidence() const { return _orientation_confidence; }
|
2015-03-07 11:49:11 -04:00
|
|
|
uint8_t get_attempt() const { return _attempt; }
|
|
|
|
|
|
|
|
private:
|
2015-03-09 22:02:46 -03:00
|
|
|
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;
|
2015-03-07 11:49:11 -04:00
|
|
|
};
|
|
|
|
|
2018-07-16 05:21:53 -03:00
|
|
|
// compact class for approximate attitude, to save memory
|
|
|
|
class AttitudeSample {
|
|
|
|
public:
|
|
|
|
Matrix3f get_rotmat();
|
|
|
|
void set_from_ahrs();
|
|
|
|
private:
|
|
|
|
int8_t roll;
|
|
|
|
int8_t pitch;
|
|
|
|
int8_t yaw;
|
|
|
|
};
|
|
|
|
|
2015-03-07 11:49:11 -04:00
|
|
|
class CompassSample {
|
|
|
|
public:
|
|
|
|
Vector3f get() const;
|
|
|
|
void set(const Vector3f &in);
|
2018-07-16 05:21:53 -03:00
|
|
|
AttitudeSample att;
|
2015-03-07 11:49:11 -04:00
|
|
|
private:
|
|
|
|
int16_t x;
|
|
|
|
int16_t y;
|
|
|
|
int16_t z;
|
|
|
|
};
|
|
|
|
|
2018-07-16 05:21:53 -03:00
|
|
|
enum Rotation _orientation;
|
2018-07-17 03:11:28 -03:00
|
|
|
enum Rotation _orig_orientation;
|
2018-07-16 05:21:53 -03:00
|
|
|
bool _is_external;
|
2018-07-17 21:09:21 -03:00
|
|
|
bool _check_orientation;
|
|
|
|
bool _fix_orientation;
|
2018-07-17 03:11:28 -03:00
|
|
|
uint8_t _compass_idx;
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2015-03-09 22:02:46 -03:00
|
|
|
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;
|
2015-03-07 11:49:11 -04:00
|
|
|
bool _retry;
|
2015-03-09 22:02:46 -03:00
|
|
|
float _tolerance;
|
2015-03-07 11:49:11 -04:00
|
|
|
uint8_t _attempt;
|
2017-04-03 19:38:14 -03:00
|
|
|
uint16_t _offset_max;
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2016-04-22 19:21:18 -03:00
|
|
|
completion_mask_t _completion_mask;
|
|
|
|
|
2015-03-09 22:02:46 -03:00
|
|
|
//fit state
|
2016-03-01 09:28:25 -04:00
|
|
|
class param_t _params;
|
2015-03-09 22:02:46 -03:00
|
|
|
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;
|
2018-07-17 03:11:28 -03:00
|
|
|
float _orientation_confidence;
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2015-03-09 22:02:46 -03:00
|
|
|
bool set_status(compass_cal_status_t status);
|
2015-03-07 11:49:11 -04:00
|
|
|
|
|
|
|
// returns true if sample should be added to buffer
|
|
|
|
bool accept_sample(const Vector3f &sample);
|
|
|
|
bool accept_sample(const CompassSample &sample);
|
|
|
|
|
2015-03-09 22:02:46 -03:00
|
|
|
// returns true if fit is acceptable
|
|
|
|
bool fit_acceptable();
|
2015-03-07 11:49:11 -04:00
|
|
|
|
|
|
|
void reset_state();
|
2015-03-09 22:02:46 -03:00
|
|
|
void initialize_fit();
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2015-03-09 22:54:05 -03:00
|
|
|
bool fitting() const;
|
|
|
|
|
2015-03-09 22:02:46 -03:00
|
|
|
// thins out samples between step one and step two
|
|
|
|
void thin_samples();
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2015-03-09 22:02:46 -03:00
|
|
|
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;
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2016-09-16 16:14:51 -03:00
|
|
|
void calc_initial_offset();
|
2015-03-09 22:02:46 -03:00
|
|
|
void calc_sphere_jacob(const Vector3f& sample, const param_t& params, float* ret) const;
|
|
|
|
void run_sphere_fit();
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2015-03-09 22:02:46 -03:00
|
|
|
void calc_ellipsoid_jacob(const Vector3f& sample, const param_t& params, float* ret) const;
|
|
|
|
void run_ellipsoid_fit();
|
2015-03-08 14:52:52 -03:00
|
|
|
|
2016-04-22 19:21:18 -03:00
|
|
|
/**
|
|
|
|
* Update #_completion_mask for the geodesic section of \p v. Corrections
|
|
|
|
* are applied to \p v with #_params.
|
|
|
|
*
|
|
|
|
* @param v[in] A vector representing one calibration sample.
|
|
|
|
*/
|
|
|
|
void update_completion_mask(const Vector3f& v);
|
|
|
|
/**
|
|
|
|
* Reset and update #_completion_mask with the current samples.
|
|
|
|
*/
|
|
|
|
void update_completion_mask();
|
2018-07-16 05:21:53 -03:00
|
|
|
|
|
|
|
Vector3f calculate_earth_field(CompassSample &sample, enum Rotation r);
|
2018-07-16 05:42:06 -03:00
|
|
|
bool calculate_orientation();
|
2015-03-07 11:49:11 -04:00
|
|
|
};
|