|
|
|
@ -410,13 +410,21 @@ static bool pre_arm_gps_checks(bool display_failure)
@@ -410,13 +410,21 @@ static bool pre_arm_gps_checks(bool display_failure)
|
|
|
|
|
float speed_cms = inertial_nav.get_velocity().length(); // speed according to inertial nav in cm/s |
|
|
|
|
|
|
|
|
|
// ensure GPS is ok and our speed is below 50cm/s |
|
|
|
|
if (!GPS_ok() || g_gps->hdop > g.gps_hdop_good || gps_glitch.glitching() || speed_cms == 0 || speed_cms > PREARM_MAX_VELOCITY_CMS) { |
|
|
|
|
if (!GPS_ok() || gps_glitch.glitching() || speed_cms == 0 || speed_cms > PREARM_MAX_VELOCITY_CMS) { |
|
|
|
|
if (display_failure) { |
|
|
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos")); |
|
|
|
|
} |
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// warn about hdop separately - to prevent user confusion with no gps lock |
|
|
|
|
if (g_gps->hdop > g.gps_hdop_good) { |
|
|
|
|
if (display_failure) { |
|
|
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: High GPS HDOP")); |
|
|
|
|
} |
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// if we got here all must be ok |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|