mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 18:08:30 -04:00
Copter: arming check for gps if GPS FS set to LAND_EVEN_STABILIZE
Setting the GPS Failsafe to LAND_EVEN_STABILIZE means the copter will LAND if it loses GPS even if it's in a manual flight mode like Stabilize. With this setting it makes sense to check the GPS quality before arming even if we're in stabilize mode.
This commit is contained in:
parent
a147eeb1e3
commit
7a2f49f7da
@ -289,7 +289,7 @@ static void pre_arm_checks(bool display_failure)
|
||||
// check GPS
|
||||
if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_GPS)) {
|
||||
// check gps is ok if required - note this same check is repeated again in arm_checks
|
||||
if (mode_requires_GPS(control_mode) && !pre_arm_gps_checks()) {
|
||||
if ((mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !pre_arm_gps_checks()) {
|
||||
if (display_failure) {
|
||||
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos"));
|
||||
}
|
||||
@ -439,7 +439,7 @@ static bool arm_checks(bool display_failure)
|
||||
|
||||
// check gps is ok if required - note this same check is also done in pre-arm checks
|
||||
if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_GPS)) {
|
||||
if (mode_requires_GPS(control_mode) && !pre_arm_gps_checks()) {
|
||||
if ((mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !pre_arm_gps_checks()) {
|
||||
if (display_failure) {
|
||||
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Bad GPS Pos"));
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user