2018-07-16 05:21:53 -03:00
|
|
|
#pragma once
|
|
|
|
|
2023-09-18 08:53:03 -03:00
|
|
|
#include "AP_Compass_config.h"
|
|
|
|
|
|
|
|
#if COMPASS_CAL_ENABLED
|
|
|
|
|
2015-08-11 03:28:42 -03:00
|
|
|
#include <AP_Math/AP_Math.h>
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
#define COMPASS_CAL_NUM_SPHERE_PARAMS 4
|
|
|
|
#define COMPASS_CAL_NUM_ELLIPSOID_PARAMS 9
|
|
|
|
#define COMPASS_CAL_NUM_SAMPLES 300 // number of samples required before fitting begins
|
2015-03-07 11:49:11 -04:00
|
|
|
|
|
|
|
class CompassCalibrator {
|
|
|
|
public:
|
|
|
|
CompassCalibrator();
|
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
// start or stop the calibration
|
|
|
|
void start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx, float tolerance);
|
|
|
|
void stop();
|
|
|
|
|
|
|
|
// Update point sample
|
|
|
|
void new_sample(const Vector3f& sample);
|
2019-11-19 02:02:58 -04:00
|
|
|
|
|
|
|
// set compass's initial orientation and whether it should be automatically fixed (if required)
|
2021-07-23 16:48:40 -03:00
|
|
|
void set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation, bool always_45_deg);
|
2019-11-19 02:02:58 -04:00
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
// running is true if actively calculating offsets, diagonals or offdiagonals
|
|
|
|
bool running();
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
// failed is true if either of the failure states are hit
|
|
|
|
bool failed();
|
2015-03-07 11:49:11 -04:00
|
|
|
|
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
// update the state machine and calculate offsets, diagonals and offdiagonals
|
|
|
|
void update();
|
2015-03-09 22:54:05 -03:00
|
|
|
|
2019-11-21 23:57:17 -04:00
|
|
|
// compass calibration states
|
|
|
|
enum class Status {
|
|
|
|
NOT_STARTED = 0,
|
|
|
|
WAITING_TO_START = 1,
|
|
|
|
RUNNING_STEP_ONE = 2,
|
|
|
|
RUNNING_STEP_TWO = 3,
|
|
|
|
SUCCESS = 4,
|
|
|
|
FAILED = 5,
|
|
|
|
BAD_ORIENTATION = 6,
|
|
|
|
BAD_RADIUS = 7,
|
|
|
|
};
|
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// get completion mask for mavlink reporting (a bitmask of faces/directions for which we have compass samples)
|
|
|
|
typedef uint8_t completion_mask_t[10];
|
2020-04-14 11:09:12 -03:00
|
|
|
|
|
|
|
// Structure accessed for cal status update via mavlink
|
|
|
|
struct State {
|
|
|
|
Status status;
|
|
|
|
uint8_t attempt;
|
|
|
|
float completion_pct;
|
|
|
|
completion_mask_t completion_mask;
|
|
|
|
} cal_state;
|
|
|
|
|
|
|
|
// Structure accessed after calibration is finished/failed
|
|
|
|
struct Report {
|
|
|
|
Status status;
|
|
|
|
float fitness;
|
|
|
|
Vector3f ofs;
|
|
|
|
Vector3f diag;
|
|
|
|
Vector3f offdiag;
|
|
|
|
float orientation_confidence;
|
|
|
|
Rotation original_orientation;
|
|
|
|
Rotation orientation;
|
|
|
|
float scale_factor;
|
2021-11-03 08:28:41 -03:00
|
|
|
bool check_orientation;
|
2020-04-14 11:09:12 -03:00
|
|
|
} cal_report;
|
|
|
|
|
|
|
|
// Structure setup to set calibration run settings
|
|
|
|
struct Settings {
|
|
|
|
float tolerance;
|
|
|
|
bool check_orientation;
|
|
|
|
enum Rotation orientation;
|
|
|
|
enum Rotation orig_orientation;
|
|
|
|
bool is_external;
|
|
|
|
bool fix_orientation;
|
|
|
|
uint16_t offset_max;
|
|
|
|
uint8_t attempt;
|
|
|
|
bool retry;
|
|
|
|
float delay_start_sec;
|
|
|
|
uint32_t start_time_ms;
|
|
|
|
uint8_t compass_idx;
|
2021-07-23 16:48:40 -03:00
|
|
|
bool always_45_deg;
|
2020-04-14 11:09:12 -03:00
|
|
|
} cal_settings;
|
|
|
|
|
|
|
|
// Get calibration result
|
|
|
|
const Report get_report();
|
|
|
|
|
|
|
|
// Get current Calibration state
|
|
|
|
const State get_state();
|
2019-11-19 02:02:58 -04:00
|
|
|
|
2021-07-23 16:48:40 -03:00
|
|
|
protected:
|
|
|
|
// convert index to rotation, this allows to skip some rotations
|
|
|
|
// protected so CompassCalibrator_index_test can see it
|
|
|
|
Rotation auto_rotation_index(uint8_t n) const;
|
|
|
|
|
|
|
|
// return true if this is a right angle rotation
|
|
|
|
bool right_angle_rotation(Rotation r) const;
|
|
|
|
|
2015-03-07 11:49:11 -04:00
|
|
|
private:
|
2019-11-19 02:02:58 -04:00
|
|
|
|
|
|
|
// results
|
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;
|
|
|
|
}
|
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
float radius; // magnetic field strength calculated from samples
|
|
|
|
Vector3f offset; // offsets
|
|
|
|
Vector3f diag; // diagonal scaling
|
|
|
|
Vector3f offdiag; // off diagonal scaling
|
2019-11-26 18:11:54 -04:00
|
|
|
float scale_factor; // scaling factor to compensate for radius error
|
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:
|
2021-02-01 12:26:27 -04:00
|
|
|
Matrix3f get_rotmat() const;
|
2018-07-16 05:21:53 -03:00
|
|
|
void set_from_ahrs();
|
|
|
|
private:
|
|
|
|
int8_t roll;
|
|
|
|
int8_t pitch;
|
|
|
|
int8_t yaw;
|
|
|
|
};
|
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// compact class to hold compass samples, to save memory
|
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;
|
|
|
|
};
|
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// set status including any required initialisation
|
2019-11-21 23:57:17 -04:00
|
|
|
bool set_status(Status status);
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
// consume point raw sample from intermediate structure
|
|
|
|
void pull_sample();
|
|
|
|
|
2015-03-07 11:49:11 -04:00
|
|
|
// returns true if sample should be added to buffer
|
2019-11-20 07:15:40 -04:00
|
|
|
bool accept_sample(const Vector3f &sample, uint16_t skip_index = UINT16_MAX);
|
|
|
|
bool accept_sample(const CompassSample &sample, uint16_t skip_index = UINT16_MAX);
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2015-03-09 22:02:46 -03:00
|
|
|
// returns true if fit is acceptable
|
2021-02-01 12:26:27 -04:00
|
|
|
bool fit_acceptable() const;
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// clear sample buffer and reset offsets and scaling to their defaults
|
2015-03-07 11:49:11 -04:00
|
|
|
void reset_state();
|
2019-11-19 02:02:58 -04:00
|
|
|
|
|
|
|
// initialize fitness before starting a fit
|
2015-03-09 22:02:46 -03:00
|
|
|
void initialize_fit();
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// true if enough samples have been collected and fitting has begun (aka runniong())
|
2020-04-14 11:09:12 -03:00
|
|
|
bool _fitting() const;
|
2015-03-09 22:54:05 -03:00
|
|
|
|
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
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// calc the fitness of a single sample vs a set of parameters (offsets, diagonals, off diagonals)
|
2015-03-09 22:02:46 -03:00
|
|
|
float calc_residual(const Vector3f& sample, const param_t& params) const;
|
2019-11-19 02:02:58 -04:00
|
|
|
|
|
|
|
// calc the fitness of the parameters (offsets, diagonals, off diagonals) vs all the samples collected
|
|
|
|
// returns 1.0e30f if the sample buffer is empty
|
2015-03-09 22:02:46 -03:00
|
|
|
float calc_mean_squared_residuals(const param_t& params) const;
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// calculate initial offsets by simply taking the average values of the samples
|
2016-09-16 16:14:51 -03:00
|
|
|
void calc_initial_offset();
|
2019-11-19 02:02:58 -04:00
|
|
|
|
|
|
|
// run sphere fit to calculate diagonals and offdiagonals
|
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
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// run ellipsoid fit to calculate diagonals and offdiagonals
|
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
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// update the completion mask based on a single sample
|
|
|
|
void update_completion_mask(const Vector3f& sample);
|
|
|
|
|
|
|
|
// reset and updated the completion mask using all samples in the sample buffer
|
2016-04-22 19:21:18 -03:00
|
|
|
void update_completion_mask();
|
2018-07-16 05:21:53 -03:00
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// calculate compass orientation
|
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();
|
2019-11-19 02:02:58 -04:00
|
|
|
|
2019-11-26 18:11:54 -04:00
|
|
|
// fix radius to compensate for sensor scaling errors
|
|
|
|
bool fix_radius();
|
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
// update methods to read write intermediate structures, called inside thread
|
|
|
|
inline void update_cal_status();
|
|
|
|
inline void update_cal_report();
|
|
|
|
inline void update_cal_settings();
|
|
|
|
|
|
|
|
// running method for use in thread
|
|
|
|
bool _running() const;
|
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
uint8_t _compass_idx; // index of the compass providing data
|
2019-11-21 23:57:17 -04:00
|
|
|
Status _status; // current state of calibrator
|
2019-11-19 02:02:58 -04:00
|
|
|
|
|
|
|
// values provided by caller
|
|
|
|
float _delay_start_sec; // seconds to delay start of calibration (provided by caller)
|
|
|
|
bool _retry; // true if calibration should be restarted on failured (provided by caller)
|
2019-11-22 02:24:29 -04:00
|
|
|
float _tolerance = 5.0; // worst acceptable RMS tolerance (aka fitness). see set_tolerance()
|
2019-11-19 02:02:58 -04:00
|
|
|
uint16_t _offset_max; // maximum acceptable offsets (provided by caller)
|
|
|
|
|
|
|
|
// behavioral state
|
|
|
|
uint32_t _start_time_ms; // system time start() function was last called
|
|
|
|
uint8_t _attempt; // number of attempts have been made to calibrate
|
|
|
|
completion_mask_t _completion_mask; // bitmask of directions in which we have samples
|
|
|
|
CompassSample *_sample_buffer; // buffer of sensor values
|
|
|
|
uint16_t _samples_collected; // number of samples in buffer
|
|
|
|
uint16_t _samples_thinned; // number of samples removed by the thin_samples() call (called before step 2 begins)
|
|
|
|
|
|
|
|
// fit state
|
|
|
|
class param_t _params; // latest calibration outputs
|
|
|
|
uint16_t _fit_step; // step during RUNNING_STEP_ONE/TWO which performs sphere fit and ellipsoid fit
|
|
|
|
float _fitness; // fitness (mean squared residuals) of current parameters
|
|
|
|
float _initial_fitness; // fitness before latest "fit" was attempted (used to determine if fit was an improvement)
|
|
|
|
float _sphere_lambda; // sphere fit's lambda
|
|
|
|
float _ellipsoid_lambda; // ellipsoid fit's lambda
|
|
|
|
|
|
|
|
// variables for orientation checking
|
|
|
|
enum Rotation _orientation; // latest detected orientation
|
|
|
|
enum Rotation _orig_orientation; // original orientation provided by caller
|
2020-04-14 11:09:12 -03:00
|
|
|
enum Rotation _orientation_solution; // latest solution
|
2019-11-19 02:02:58 -04:00
|
|
|
bool _is_external; // true if compass is external (provided by caller)
|
|
|
|
bool _check_orientation; // true if orientation should be automatically checked
|
|
|
|
bool _fix_orientation; // true if orientation should be fixed if necessary
|
2021-07-23 16:48:40 -03:00
|
|
|
bool _always_45_deg; // true if orentation should considder 45deg with equal tolerance
|
2019-11-19 02:02:58 -04:00
|
|
|
float _orientation_confidence; // measure of confidence in automatic orientation detection
|
2020-04-14 11:09:12 -03:00
|
|
|
CompassSample _last_sample;
|
|
|
|
|
|
|
|
Status _requested_status;
|
|
|
|
bool _status_set_requested;
|
|
|
|
|
|
|
|
bool _new_sample;
|
|
|
|
|
|
|
|
// Semaphore for state related intermediate structures
|
|
|
|
HAL_Semaphore state_sem;
|
|
|
|
|
|
|
|
// Semaphore for intermediate structure for point sample collection
|
|
|
|
HAL_Semaphore sample_sem;
|
2015-03-07 11:49:11 -04:00
|
|
|
};
|
2023-09-18 08:53:03 -03:00
|
|
|
|
|
|
|
#endif // COMPASS_CAL_ENABLED
|