Browse Source

AP_Compass: do not use GSF if any model has been clipped

apm_2208
Peter Barker 3 years ago committed by Andrew Tridgell
parent
commit
437b313544
  1. 4
      libraries/AP_Compass/Compass_learn.cpp

4
libraries/AP_Compass/Compass_learn.cpp

@ -51,8 +51,10 @@ void CompassLearn::update(void) @@ -51,8 +51,10 @@ void CompassLearn::update(void)
AP_Notify::flags.compass_cal_running = true;
ftype yaw_rad, yaw_variance;
if (!gsf->getYawData(yaw_rad, yaw_variance) ||
uint8_t n_clips;
if (!gsf->getYawData(yaw_rad, yaw_variance, &n_clips) ||
!is_positive(yaw_variance) ||
n_clips > 1 ||
yaw_variance >= sq(radians(YAW_ACCURACY_THRESHOLD_DEG))) {
// not converged
return;

Loading…
Cancel
Save