Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: set pre_arm_gps_check flag

This commit is contained in:
Randy Mackay 2014-12-24 22:25:53 +09:00
parent d67b4a8d49
commit 2ea9b8a5a8
1 changed files with 44 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
ArduCopter/motors.pde
View File

@ -203,6 +203,7 @@ static void pre_arm_checks(bool display_failure)
{
// exit immediately if we've already successfully performed the pre-arm check
if (ap.pre_arm_check) {
pre_arm_gps_checks(display_failure);
return;
}
@ -299,18 +300,16 @@ static void pre_arm_checks(bool display_failure)
}
// check GPS
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & 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) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !pre_arm_gps_checks(display_failure)) {
return;
}
if (!pre_arm_gps_checks(display_failure)) {
return;
}
#if AC_FENCE == ENABLED
// check fence is initialised
if(!fence.pre_arm_check() || (((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) && !pre_arm_gps_checks(display_failure))) {
return;
// check fence is initialised
if(!fence.pre_arm_check()) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: check fence"));
}
#endif
return;
}
// check INS
@ -466,13 +465,39 @@ static void pre_arm_rc_checks()
// performs pre_arm gps related checks and returns true if passed
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
// return true immediately if gps check is disabled
if (!(g.arming_check == ARMING_CHECK_ALL || g.arming_check & ARMING_CHECK_GPS)) {
AP_Notify::flags.pre_arm_gps_check = true;
return true;
}
// check if flight mode requires GPS
bool gps_required = mode_requires_GPS(control_mode);
// if GPS failsafe will triggers even in stabilize mode we need GPS before arming
if (g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) {
gps_required = true;
}
#if AC_FENCE == ENABLED
// if circular fence is enabled we need GPS
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) {
gps_required = true;
}
#endif
// return true if GPS is not required
if (!gps_required) {
AP_Notify::flags.pre_arm_gps_check = true;
return true;
}
// check GPS is not glitching
if (gps_glitch.glitching()) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: GPS Glitch"));
}
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
@ -481,14 +506,17 @@ static bool pre_arm_gps_checks(bool display_failure)
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Need 3D Fix"));
}
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
// check speed is below 50cm/s
float speed_cms = inertial_nav.get_velocity().length(); // speed according to inertial nav in cm/s
if (speed_cms == 0 || speed_cms > PREARM_MAX_VELOCITY_CMS) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad Velocity"));
}
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
@ -497,10 +525,12 @@ static bool pre_arm_gps_checks(bool display_failure)
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: High GPS HDOP"));
}
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
// if we got here all must be ok
AP_Notify::flags.pre_arm_gps_check = true;
return true;
}
@ -558,11 +588,9 @@ static bool arm_checks(bool display_failure, bool arming_from_gcs)
}
}
// check gps is ok if required - note this same check is also done in pre-arm checks
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_GPS)) {
if ((mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !pre_arm_gps_checks(display_failure)) {
return false;
}
// check gps
if (!pre_arm_gps_checks(display_failure)) {
return false;
}
// check parameters