aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Compass: initialize offset before beginning sphere fit

Browse Source
This commit is contained in:
Jonathan Challinger 2016-09-16 12:14:51 -07:00 committed by Andrew Tridgell
parent 419f1bc00e
commit 17fb7dc88f
2 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -187,6 +187,9 @@ void CompassCalibrator::update(bool &failure) {
}
set_status(COMPASS_CAL_RUNNING_STEP_TWO);
} else {
if (_fit_step == 0) {
calc_initial_offset();
}
run_sphere_fit();
_fit_step++;
}
@ -472,6 +475,16 @@ 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;
}
void CompassCalibrator::run_sphere_fit()
{
if(_sample_buffer == NULL) {

1
libraries/AP_Compass/CompassCalibrator.h
View File

@ -119,6 +119,7 @@ private:
float calc_mean_squared_residuals(const param_t& params) const;
float calc_mean_squared_residuals() const;
void calc_initial_offset();
void calc_sphere_jacob(const Vector3f& sample, const param_t& params, float* ret) const;
void run_sphere_fit();