aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: pre-arm check that INAV has no errors

Browse Source
This commit is contained in:
Randy Mackay 2013-11-21 15:34:50 +09:00
parent e47e1f21d8
commit fc4b5c962f
1 changed files with 21 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
ArduCopter/motors.pde
View File

@ -125,6 +125,9 @@ static void init_arm_motors()
// disable cpu failsafe because initialising everything takes a while // disable cpu failsafe because initialising everything takes a while
failsafe_disable(); failsafe_disable();
// disable inertial nav errors temporarily
inertial_nav.ignore_next_error();
#if LOGGING_ENABLED == ENABLED #if LOGGING_ENABLED == ENABLED
// start dataflash // start dataflash
start_logging(); start_logging();
@ -289,19 +292,13 @@ static void pre_arm_checks(bool display_failure)
// check GPS // check GPS
if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_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 // 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()) { if ((mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !pre_arm_gps_checks(display_failure)) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos"));
}
return; return;
} }
#if AC_FENCE == ENABLED #if AC_FENCE == ENABLED
// check fence is initialised // check fence is initialised
if(!fence.pre_arm_check() || (((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) && !pre_arm_gps_checks())) { if(!fence.pre_arm_check() || (((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) && !pre_arm_gps_checks(display_failure))) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos"));
}
return; return;
} }
#endif #endif
@ -415,12 +412,23 @@ static void pre_arm_rc_checks()
} }
// performs pre_arm gps related checks and returns true if passed // performs pre_arm gps related checks and returns true if passed
static bool pre_arm_gps_checks() 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 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 // 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() || g_gps->hdop > g.gps_hdop_good || 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;
}
// check for missed gps updates (i.e. not arriving at at least 4hz)
if (inertial_nav.error_count() > 0) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Slow GPS"));
}
return false; return false;
} }
@ -439,10 +447,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 // 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 ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_GPS)) {
if ((mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !pre_arm_gps_checks()) { if ((mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !pre_arm_gps_checks(display_failure)) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Bad GPS Pos"));
}
return false; return false;
} }
} }
@ -484,6 +489,9 @@ static void init_disarm_motors()
motors.armed(false); motors.armed(false);
// disable inertial nav errors temporarily
inertial_nav.ignore_next_error();
compass.save_offsets(); compass.save_offsets();
g.throttle_cruise.save(); g.throttle_cruise.save();