Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: fix thin_samples to avoid comparing sample to itself

This commit is contained in:
lukezhqin 2019-11-20 20:15:40 +09:00 committed by Andrew Tridgell
parent b87f42bc07
commit f59d22aa41
2 changed files with 12 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -399,8 +399,9 @@ void CompassCalibrator::thin_samples()
_sample_buffer[j] = temp; _sample_buffer[j] = temp;
} }
// remove any samples that are close together
for (uint16_t i=0; i < _samples_collected; i++) { for (uint16_t i=0; i < _samples_collected; i++) {
if (!accept_sample(_sample_buffer[i])) { if (!accept_sample(_sample_buffer[i], i)) {
_sample_buffer[i] = _sample_buffer[_samples_collected-1]; _sample_buffer[i] = _sample_buffer[_samples_collected-1];
_samples_collected--; _samples_collected--;
_samples_thinned++; _samples_thinned++;
@ -426,7 +427,7 @@ void CompassCalibrator::thin_samples()
* The above equation was proved after solving for spherical triangular excess * The above equation was proved after solving for spherical triangular excess
* and related equations. * and related equations.
*/ */
bool CompassCalibrator::accept_sample(const Vector3f& sample) bool CompassCalibrator::accept_sample(const Vector3f& sample, uint16_t skip_index)
{ {
static const uint16_t faces = (2 * COMPASS_CAL_NUM_SAMPLES - 4); static const uint16_t faces = (2 * COMPASS_CAL_NUM_SAMPLES - 4);
static const float a = (4.0f * M_PI / (3.0f * faces)) + M_PI / 3.0f; static const float a = (4.0f * M_PI / (3.0f * faces)) + M_PI / 3.0f;
@ -439,17 +440,19 @@ bool CompassCalibrator::accept_sample(const Vector3f& sample)
float min_distance = _params.radius * 2*sinf(theta/2); float min_distance = _params.radius * 2*sinf(theta/2);
for (uint16_t i = 0; i<_samples_collected; i++) { for (uint16_t i = 0; i<_samples_collected; i++) {
float distance = (sample - _sample_buffer[i].get()).length(); if (i != skip_index) {
if (distance < min_distance) { float distance = (sample - _sample_buffer[i].get()).length();
return false; if (distance < min_distance) {
return false;
}
} }
} }
return true; return true;
} }
bool CompassCalibrator::accept_sample(const CompassSample& sample) bool CompassCalibrator::accept_sample(const CompassSample& sample, uint16_t skip_index)
{ {
return accept_sample(sample.get()); return accept_sample(sample.get(), skip_index);
} }
float CompassCalibrator::calc_residual(const Vector3f& sample, const param_t& params) const float CompassCalibrator::calc_residual(const Vector3f& sample, const param_t& params) const

4
libraries/AP_Compass/CompassCalibrator.h
View File

@ -109,8 +109,8 @@ private:
bool set_status(compass_cal_status_t status); bool set_status(compass_cal_status_t status);
// returns true if sample should be added to buffer // returns true if sample should be added to buffer
bool accept_sample(const Vector3f &sample); bool accept_sample(const Vector3f &sample, uint16_t skip_index = UINT16_MAX);
bool accept_sample(const CompassSample &sample); bool accept_sample(const CompassSample &sample, uint16_t skip_index = UINT16_MAX);
// returns true if fit is acceptable // returns true if fit is acceptable
bool fit_acceptable(); bool fit_acceptable();