Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 08:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: fixed in-flight learning for all compasses

Browse Source 
after recent changes only first compass was learnt

See
https://discuss.ardupilot.org/t/break-in-latest-master-in-flight-compass-calibration/52602
This commit is contained in:
Andrew Tridgell 2020-02-24 11:34:32 +11:00
parent c247c49bbb
commit 8f83bf5e80
1 changed files with 3 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_Compass/Compass_learn.cpp
View File

@ -21,7 +21,7 @@ CompassLearn::CompassLearn(Compass &_compass) :
{ {
gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: Initialised"); gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: Initialised");
for (Compass::Priority i(0); i<compass.get_count(); i++) { for (Compass::Priority i(0); i<compass.get_count(); i++) {
if (compass._use_for_yaw[i]) { if (compass._use_for_yaw[Compass::Priority(i)]) {
// reset scale factors, we can't learn scale factors in // reset scale factors, we can't learn scale factors in
// flight // flight
compass.set_and_save_scale_factor(uint8_t(i), 0.0); compass.set_and_save_scale_factor(uint8_t(i), 0.0);
@ -124,7 +124,7 @@ void CompassLearn::update(void)
// set non-primary offsets to match primary // set non-primary offsets to match primary
Vector3f field_primary = compass.get_field(0); Vector3f field_primary = compass.get_field(0);
for (uint8_t i=1; i<compass.get_count(); i++) { for (uint8_t i=1; i<compass.get_count(); i++) {
if (!compass.use_for_yaw(i)) { if (!compass._use_for_yaw[Compass::Priority(i)]) {
continue; continue;
} }
Vector3f field2 = compass.get_field(i); Vector3f field2 = compass.get_field(i);
@ -137,10 +137,9 @@ void CompassLearn::update(void)
// set the offsets and enable compass for EKF use. Let the // set the offsets and enable compass for EKF use. Let the
// EKF learn the remaining compass offset error // EKF learn the remaining compass offset error
for (uint8_t i=0; i<compass.get_count(); i++) { for (uint8_t i=0; i<compass.get_count(); i++) {
if (compass.use_for_yaw(i)) { if (compass._use_for_yaw[Compass::Priority(i)]) {
compass.save_offsets(i); compass.save_offsets(i);
compass.set_and_save_scale_factor(i, 0.0); compass.set_and_save_scale_factor(i, 0.0);
compass.set_use_for_yaw(i, true);
} }
} }
compass.set_learn_type(Compass::LEARN_NONE, true); compass.set_learn_type(Compass::LEARN_NONE, true);