mirror of https://github.com/ArduPilot/ardupilot
Copter: add safe switch arming check
This commit is contained in:
parent
365e82aecb
commit
efc02161c4
|
@ -329,8 +329,11 @@ static void pre_arm_checks(bool display_failure)
|
|||
return;
|
||||
}
|
||||
|
||||
// pass arming checks at least once
|
||||
if (!arm_checks(display_failure)) {
|
||||
// check gps is ok if required - note this same check is repeated again in arm_checks
|
||||
if(mode_requires_GPS(control_mode) && (!GPS_ok() || g_gps->hdop > g.gps_hdop_good)) {
|
||||
if (display_failure) {
|
||||
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos"));
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
|
@ -374,7 +377,7 @@ static bool arm_checks(bool display_failure)
|
|||
return true;
|
||||
}
|
||||
|
||||
// check gps is ok if required
|
||||
// check gps is ok if required - note this same check is also done in pre-arm checks
|
||||
if(mode_requires_GPS(control_mode) && (!GPS_ok() || g_gps->hdop > g.gps_hdop_good)) {
|
||||
if (display_failure) {
|
||||
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Bad GPS Pos"));
|
||||
|
@ -382,6 +385,14 @@ static bool arm_checks(bool display_failure)
|
|||
return false;
|
||||
}
|
||||
|
||||
// check if safety switch has been pushed
|
||||
if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
|
||||
if (display_failure) {
|
||||
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Safety Switch"));
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// if we've gotten this far all is ok
|
||||
return true;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue