diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 5919a2b303..2b42833660 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -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 diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index ab0eed5cf1..0682e3b9bb 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -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(); } diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 7b54b0e544..75ad614124 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -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