Copter: display reason for pre-arm-check failures in GCS

This commit is contained in:
Randy Mackay 2013-05-20 13:03:18 +09:00
parent d10e3bc75f
commit 1dbe98b566
3 changed files with 28 additions and 10 deletions

View File

@ -823,6 +823,7 @@ AC_Fence fence(&inertial_nav);
// function definitions to keep compiler from complaining about undeclared functions
////////////////////////////////////////////////////////////////////////////////
void get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_climb_rate);
static void pre_arm_checks(bool display_failure);
////////////////////////////////////////////////////////////////////////////////
// Top-level logic
@ -1162,7 +1163,7 @@ static void medium_loop()
medium_loopCounter = 0;
// Check for motor arming or disarming
arm_motors();
arm_motors_check();
// agmatthews - USERHOOKS
#ifdef USERHOOK_MEDIUMLOOP
@ -1278,7 +1279,7 @@ static void super_slow_loop()
Log_Write_Current();
// perform pre-arm checks
pre_arm_checks();
pre_arm_checks(false);
// this function disarms the copter if it has been sitting on the ground for any moment of time greater than 25 seconds
// but only of the control mode is manual

View File

@ -1218,6 +1218,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_CMD_COMPONENT_ARM_DISARM:
if (packet.target_component == MAV_COMP_ID_SYSTEM_CONTROL) {
if (packet.param1 == 1.0f) {
// run pre-arm-checks and display failures
pre_arm_checks(true);
if(ap.pre_arm_check) {
init_arm_motors();
}

View File

@ -6,8 +6,9 @@
#define AUTO_TRIM_DELAY 100
// arm_motors_check - checks for pilot input to arm or disarm the copter
// called at 10hz
static void arm_motors()
static void arm_motors_check()
{
static int16_t arming_counter;
@ -17,11 +18,6 @@ static void arm_motors()
return;
}
// ensure pre-arm checks have been successful
if(!ap.pre_arm_check) {
return;
}
// ensure we are in Stabilize, Acro or TOY mode
if ((control_mode > ACRO) && ((control_mode != TOY_A) && (control_mode != TOY_M))) {
arming_counter = 0;
@ -51,7 +47,14 @@ static void arm_motors()
// arm the motors and configure for flight
if (arming_counter == ARM_DELAY && !motors.armed()) {
init_arm_motors();
// run pre-arm-checks and display failures
pre_arm_checks(true);
if(ap.pre_arm_check) {
init_arm_motors();
}else{
// reset arming counter if pre-arm checks fail
arming_counter = 0;
}
}
// arm the motors and configure for flight
@ -163,7 +166,7 @@ static void init_arm_motors()
}
// perform pre-arm checks and set ap.pre_arm_check flag
static void pre_arm_checks()
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 ) {
@ -173,22 +176,34 @@ static void pre_arm_checks()
// pre-arm rc checks a prerequisite
pre_arm_rc_checks();
if(!ap.pre_arm_rc_check) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: RC not calibrated"));
}
return;
}
// check accelerometers have been calibrated
if(!ins.calibrated()) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: INS not calibrated"));
}
return;
}
// check the compass is healthy
if(!compass.healthy) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass not healthy"));
}
return;
}
#if AC_FENCE == ENABLED
// check fence is initialised
if(!fence.pre_arm_check()) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: No GPS Lock"));
}
return;
}
#endif