zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Compass: add comments to calibrator

master
Randy Mackay 5 years ago committed by Andrew Tridgell
parent
015eed7159
commit
233e3bae61
2 changed files with 132 additions and 83 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 57
      libraries/AP_Compass/CompassCalibrator.cpp
  2. 152
      libraries/AP_Compass/CompassCalibrator.h

57
libraries/AP_Compass/CompassCalibrator.cpp
Unescape View File

@ -81,6 +81,15 @@ void CompassCalibrator::clear() @@ -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 @@ -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) @@ -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) @@ -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 @@ -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) @@ -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 @@ -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 @@ -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& @@ -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() @@ -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;
@ -552,6 +570,7 @@ void CompassCalibrator::run_sphere_fit() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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;

152
libraries/AP_Compass/CompassCalibrator.h
Unescape View File

@ -4,11 +4,10 @@ @@ -4,11 +4,10 @@
#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_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 { @@ -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_orientation_confidence() const { return _orientation_confidence; }
// get completion percentage (0 to 100) for reporting to GCS
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 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: @@ -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: @@ -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: @@ -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: @@ -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
};

Write Preview
Loading…
Cancel
Save