diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index e410272646..eebb7b02f4 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -81,6 +81,15 @@ void CompassCalibrator::clear() set_status(COMPASS_CAL_NOT_STARTED); } +void CompassCalibrator::set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation) +{ + _check_orientation = true; + _orientation = orientation; + _orig_orientation = orientation; + _is_external = is_external; + _fix_orientation = fix_orientation; +} + void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx) { if (running()) { @@ -127,6 +136,8 @@ float CompassCalibrator::get_completion_percent() const }; } +// update completion mask based on latest sample +// used to ensure we have collected samples in all directions void CompassCalibrator::update_completion_mask(const Vector3f& v) { Matrix3f softiron { @@ -142,6 +153,7 @@ void CompassCalibrator::update_completion_mask(const Vector3f& v) _completion_mask[section / 8] |= 1 << (section % 8); } +// reset and updated the completion mask using all samples in the sample buffer void CompassCalibrator::update_completion_mask() { memset(_completion_mask, 0, sizeof(_completion_mask)); @@ -181,6 +193,7 @@ void CompassCalibrator::update(bool &failure) { failure = false; + // collect the minimum number of samples if (!fitting()) { return; } @@ -230,17 +243,17 @@ bool CompassCalibrator::fitting() const return running() && (_samples_collected == COMPASS_CAL_NUM_SAMPLES); } +// initialize fitness before starting a fit void CompassCalibrator::initialize_fit() { - //initialize _fitness before starting a fit if (_samples_collected != 0) { _fitness = calc_mean_squared_residuals(_params); } else { _fitness = 1.0e30f; } - _ellipsoid_lambda = 1.0f; - _sphere_lambda = 1.0f; _initial_fitness = _fitness; + _sphere_lambda = 1.0f; + _ellipsoid_lambda = 1.0f; _fit_step = 0; } @@ -284,6 +297,7 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) return false; } + // on first attempt delay start if requested by caller if (_attempt == 1 && (AP_HAL::millis()-_start_time_ms)*1.0e-3f < _delay_start_sec) { return false; } @@ -452,6 +466,7 @@ float CompassCalibrator::calc_mean_squared_residuals() const return calc_mean_squared_residuals(_params); } +// calc the fitness given a set of parameters (offsets, diagonals, off diagonals) float CompassCalibrator::calc_mean_squared_residuals(const param_t& params) const { if (_sample_buffer == nullptr || _samples_collected == 0) { @@ -467,6 +482,17 @@ float CompassCalibrator::calc_mean_squared_residuals(const param_t& params) cons return sum; } +// calculate initial offsets by simply taking the average values of the samples +void CompassCalibrator::calc_initial_offset() +{ + // Set initial offset to the average value of the samples + _params.offset.zero(); + for (uint16_t k = 0; k < _samples_collected; k++) { + _params.offset -= _sample_buffer[k].get(); + } + _params.offset /= _samples_collected; +} + void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const param_t& params, float* ret) const { const Vector3f &offset = params.offset; @@ -491,16 +517,7 @@ void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const param_t& ret[3] = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C))/length); } -void CompassCalibrator::calc_initial_offset() -{ - // Set initial offset to the average value of the samples - _params.offset.zero(); - for (uint16_t k = 0; k<_samples_collected; k++) { - _params.offset -= _sample_buffer[k].get(); - } - _params.offset /= _samples_collected; -} - +// run sphere fit to calculate diagonals and offdiagonals void CompassCalibrator::run_sphere_fit() { if (_sample_buffer == nullptr) { @@ -509,6 +526,7 @@ void CompassCalibrator::run_sphere_fit() const float lma_damping = 10.0f; + // take backup of fitness and parameters so we can determine later if this fit has improved the calibration float fitness = _fitness; float fit1, fit2; param_t fit1_params, fit2_params; @@ -538,7 +556,7 @@ void CompassCalibrator::run_sphere_fit() } //------------------------Levenberg-Marquardt-part-starts-here---------------------------------// - //refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter + // refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter for (uint8_t i = 0; i < COMPASS_CAL_NUM_SPHERE_PARAMS; i++) { JTJ[i*COMPASS_CAL_NUM_SPHERE_PARAMS+i] += _sphere_lambda; JTJ2[i*COMPASS_CAL_NUM_SPHERE_PARAMS+i] += _sphere_lambda/lma_damping; @@ -552,6 +570,7 @@ void CompassCalibrator::run_sphere_fit() return; } + // extract radius, offset, diagonals and offdiagonal parameters for (uint8_t row=0; row < COMPASS_CAL_NUM_SPHERE_PARAMS; row++) { for (uint8_t col=0; col < COMPASS_CAL_NUM_SPHERE_PARAMS; col++) { fit1_params.get_sphere_params()[row] -= JTFI[col] * JTJ[row*COMPASS_CAL_NUM_SPHERE_PARAMS+col]; @@ -559,12 +578,16 @@ void CompassCalibrator::run_sphere_fit() } } + // calculate fitness of two possible sets of parameters fit1 = calc_mean_squared_residuals(fit1_params); fit2 = calc_mean_squared_residuals(fit2_params); + // decide which of the two sets of parameters is best and store in fit1_params if (fit1 > _fitness && fit2 > _fitness) { + // if neither set of parameters provided better results, increase lambda _sphere_lambda *= lma_damping; } else if (fit2 < _fitness && fit2 < fit1) { + // if fit2 was better we will use it. decrease lambda _sphere_lambda /= lma_damping; fit1_params = fit2_params; fitness = fit2; @@ -573,6 +596,7 @@ void CompassCalibrator::run_sphere_fit() } //--------------------Levenberg-Marquardt-part-ends-here--------------------------------// + // store new parameters and update fitness if (!isnan(fitness) && fitness < _fitness) { _fitness = fitness; _params = fit1_params; @@ -618,6 +642,7 @@ void CompassCalibrator::run_ellipsoid_fit() const float lma_damping = 10.0f; + // take backup of fitness and parameters so we can determine later if this fit has improved the calibration float fitness = _fitness; float fit1, fit2; param_t fit1_params, fit2_params; @@ -661,6 +686,7 @@ void CompassCalibrator::run_ellipsoid_fit() return; } + // extract radius, offset, diagonals and offdiagonal parameters for (uint8_t row=0; row < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; row++) { for (uint8_t col=0; col < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; col++) { fit1_params.get_ellipsoid_params()[row] -= JTFI[col] * JTJ[row*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+col]; @@ -668,12 +694,16 @@ void CompassCalibrator::run_ellipsoid_fit() } } + // calculate fitness of two possible sets of parameters fit1 = calc_mean_squared_residuals(fit1_params); fit2 = calc_mean_squared_residuals(fit2_params); + // decide which of the two sets of parameters is best and store in fit1_params if (fit1 > _fitness && fit2 > _fitness) { + // if neither set of parameters provided better results, increase lambda _ellipsoid_lambda *= lma_damping; } else if (fit2 < _fitness && fit2 < fit1) { + // if fit2 was better we will use it. decrease lambda _ellipsoid_lambda /= lma_damping; fit1_params = fit2_params; fitness = fit2; @@ -682,6 +712,7 @@ void CompassCalibrator::run_ellipsoid_fit() } //--------------------Levenberg-part-ends-here--------------------------------// + // store new parameters and update fitness if (fitness < _fitness) { _fitness = fitness; _params = fit1_params; diff --git a/libraries/AP_Compass/CompassCalibrator.h b/libraries/AP_Compass/CompassCalibrator.h index b9bdc58c32..16382847df 100644 --- a/libraries/AP_Compass/CompassCalibrator.h +++ b/libraries/AP_Compass/CompassCalibrator.h @@ -2,13 +2,12 @@ #include -#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 +#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 +#define COMPASS_CAL_DEFAULT_TOLERANCE 5.0f // default RMS tolerance +// compass calibration states enum compass_cal_status_t { COMPASS_CAL_NOT_STARTED = 0, COMPASS_CAL_WAITING_TO_START = 1, @@ -21,42 +20,52 @@ enum compass_cal_status_t { class CompassCalibrator { public: - typedef uint8_t completion_mask_t[10]; - CompassCalibrator(); + // set tolerance of calibration (aka fitness) + void set_tolerance(float tolerance) { _tolerance = tolerance; } + + // set compass's initial orientation and whether it should be automatically fixed (if required) + void set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation); + + // start or stop the calibration void start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx); void clear(); + // update the state machine and calculate offsets, diagonals and offdiagonals void update(bool &failure); void new_sample(const Vector3f &sample); bool check_for_timeout(); + // running is true if actively calculating offsets, diagonals or offdiagonals bool running() const; - void set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation) { - _check_orientation = true; - _orientation = orientation; - _orig_orientation = orientation; - _is_external = is_external; - _fix_orientation = fix_orientation; - } - - void set_tolerance(float tolerance) { _tolerance = tolerance; } + // get status of calibrations progress + enum compass_cal_status_t get_status() const { return _status; } + // get calibration outputs (offsets, diagonals, offdiagonals) and fitness void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals); + float get_fitness() const { return sqrtf(_fitness); } + + // get corrected (and original) orientation enum Rotation get_orientation() const { return _orientation; } enum Rotation get_original_orientation() const { return _orig_orientation; } - - float get_completion_percent() const; - const completion_mask_t& get_completion_mask() const { return _completion_mask; } - enum compass_cal_status_t get_status() const { return _status; } - float get_fitness() const { return sqrtf(_fitness); } float get_orientation_confidence() const { return _orientation_confidence; } + + // get completion percentage (0 to 100) for reporting to GCS + float get_completion_percent() const; + + // get how many attempts have been made to calibrate for reporting to GCS uint8_t get_attempt() const { return _attempt; } + // get completion mask for mavlink reporting (a bitmask of faces/directions for which we have compass samples) + typedef uint8_t completion_mask_t[10]; + const completion_mask_t& get_completion_mask() const { return _completion_mask; } + private: + + // results class param_t { public: float* get_sphere_params() { @@ -67,10 +76,10 @@ private: return &offset.x; } - float radius; - Vector3f offset; - Vector3f diag; - Vector3f offdiag; + float radius; // magnetic field strength calculated from samples + Vector3f offset; // offsets + Vector3f diag; // diagonal scaling + Vector3f offdiag; // off diagonal scaling }; // compact class for approximate attitude, to save memory @@ -84,6 +93,7 @@ private: int8_t yaw; }; + // compact class to hold compass samples, to save memory class CompassSample { public: Vector3f get() const; @@ -95,40 +105,7 @@ private: int16_t z; }; - enum Rotation _orientation; - enum Rotation _orig_orientation; - bool _is_external; - bool _check_orientation; - bool _fix_orientation; - uint8_t _compass_idx; - - 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 _retry; - float _tolerance; - uint8_t _attempt; - uint16_t _offset_max; - - completion_mask_t _completion_mask; - - //fit state - class 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; - float _orientation_confidence; - + // set status including any required initialisation bool set_status(compass_cal_status_t status); // returns true if sample should be added to buffer @@ -138,37 +115,78 @@ private: // returns true if fit is acceptable bool fit_acceptable(); + // clear sample buffer and reset offsets and scaling to their defaults void reset_state(); + + // initialize fitness before starting a fit void initialize_fit(); + // true if enough samples have been collected and fitting has begun (aka runniong()) bool fitting() const; // thins out samples between step one and step two void thin_samples(); + // calc the fitness of a single sample vs a set of parameters (offsets, diagonals, off diagonals) float calc_residual(const Vector3f& sample, const param_t& params) const; + + // calc the fitness of the parameters (offsets, diagonals, off diagonals) vs all the samples collected + // returns 1.0e30f if the sample buffer is empty float calc_mean_squared_residuals(const param_t& params) const; float calc_mean_squared_residuals() const; + // calculate initial offsets by simply taking the average values of the samples void calc_initial_offset(); + + // run sphere fit to calculate diagonals and offdiagonals void calc_sphere_jacob(const Vector3f& sample, const param_t& params, float* ret) const; void run_sphere_fit(); + // run ellipsoid fit to calculate diagonals and offdiagonals void calc_ellipsoid_jacob(const Vector3f& sample, const param_t& params, float* ret) const; void run_ellipsoid_fit(); - /** - * 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. - */ + // 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 void update_completion_mask(); + // calculate compass orientation Vector3f calculate_earth_field(CompassSample &sample, enum Rotation r); bool calculate_orientation(); + + uint8_t _compass_idx; // index of the compass providing data + enum compass_cal_status_t _status; // current state of calibrator + uint32_t _last_sample_ms; // system time of last sample received for timeout + + // 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) + float _tolerance; // worst acceptable tolerance (aka fitness). see set_tolerance() + 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 + 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 + float _orientation_confidence; // measure of confidence in automatic orientation detection };