diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 3be4687e75..50d62531d2 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -123,7 +123,7 @@ public: const Vector3f &get_field(void) const { return get_field(get_primary()); } // compass calibrator interface - void compass_cal_update(); + void cal_update(); // per-motor calibration access void per_motor_calibration_start(void) { diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index e0ff65cf64..a9ff73d5d5 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -7,8 +7,12 @@ extern AP_HAL::HAL& hal; void -Compass::compass_cal_update() +Compass::cal_update() { + if (hal.util->get_soft_armed()) { + return; + } + bool running = false; for (uint8_t i=0; i