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;
}
// remove any samples that are close together
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];
_samples_collected--;
_samples_thinned++;
@ -426,7 +427,7 @@ void CompassCalibrator::thin_samples()
* The above equation was proved after solving for spherical triangular excess
* 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 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);
for (uint16_t i = 0; i<_samples_collected; i++) {
float distance = (sample - _sample_buffer[i].get()).length();
if (distance < min_distance) {
return false;
if (i != skip_index) {
float distance = (sample - _sample_buffer[i].get()).length();
if (distance < min_distance) {
return false;
}
}
}
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

4
libraries/AP_Compass/CompassCalibrator.h
View File

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