aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 1 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
failsafe_disable();
// disable inertial nav errors temporarily
inertial_nav.ignore_next_error();
#if LOGGING_ENABLED == ENABLED
// start dataflash
start_logging();
@ -289,19 +292,13 @@ 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) || 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"));
}
if ((mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !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())) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos"));
}
if(!fence.pre_arm_check() || (((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) && !pre_arm_gps_checks(display_failure))) {
return;
}
#endif
@ -415,12 +412,23 @@ static void pre_arm_rc_checks()
}
// 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
// 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 (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;
}
@ -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
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 (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Bad GPS Pos"));
}
if ((mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !pre_arm_gps_checks(display_failure)) {
return false;
}
}
@ -484,6 +489,9 @@ static void init_disarm_motors()
motors.armed(false);
// disable inertial nav errors temporarily
inertial_nav.ignore_next_error();
compass.save_offsets();
g.throttle_cruise.save();