mirror of https://github.com/ArduPilot/ardupilot
Copter: check if compass cal requires reboot
This commit is contained in:
parent
cd977b3142
commit
93fc595d4d
|
@ -107,6 +107,14 @@ bool Copter::pre_arm_checks(bool display_failure)
|
|||
|
||||
// check Compass
|
||||
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_COMPASS)) {
|
||||
//check if compass has calibrated and requires reboot
|
||||
if (compass.compass_cal_requires_reboot()) {
|
||||
if (display_failure) {
|
||||
gcs_send_text(MAV_SEVERITY_CRITICAL, "PreArm: Compass calibrated requires reboot");
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// check the primary compass is healthy
|
||||
if (!compass.healthy()) {
|
||||
if (display_failure) {
|
||||
|
@ -534,6 +542,14 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
|
|||
return false;
|
||||
}
|
||||
|
||||
//check if compass has calibrated and requires reboot
|
||||
if (compass.compass_cal_requires_reboot()) {
|
||||
if (display_failure) {
|
||||
gcs_send_text(MAV_SEVERITY_CRITICAL, "PreArm: Compass calibrated requires reboot");
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// always check if the current mode allows arming
|
||||
if (!mode_allows_arming(control_mode, arming_from_gcs)) {
|
||||
if (display_failure) {
|
||||
|
|
Loading…
Reference in New Issue