diff --git a/ArduCopter/AP_State.pde b/ArduCopter/AP_State.pde index a7328f9f28..4395965ff6 100644 --- a/ArduCopter/AP_State.pde +++ b/ArduCopter/AP_State.pde @@ -131,3 +131,10 @@ void set_pre_arm_check(bool b) } } +void set_pre_arm_rc_check(bool b) +{ + if(ap.pre_arm_rc_check != b) { + ap.pre_arm_rc_check = b; + } +} + diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 72ece20c71..4dcb0378fe 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -397,10 +397,10 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: ARMING_CHECK // @DisplayName: Arming check - // @Description: Allows enabling or disabling of pre-arming checks of receiver, accelerometer, barometer and compass - // @Values: 0:Disabled, 1:Enabled + // @Description: Allows enabling or disabling of pre-arming checks of receiver, accelerometer, barometer, compass and GPS + // @Values: 0:Disabled, 1:Enabled, -3:Skip Baro, -5:Skip Compass, -9:Skip GPS, -17:Skip INS, -33:Skip Parameters, -65:Skip RC, 127:Skip Voltage // @User: Standard - GSCALAR(arming_check_enabled, "ARMING_CHECK", 1), + GSCALAR(arming_check_enabled, "ARMING_CHECK", ARMING_CHECK_ALL), // @Param: ANGLE_MAX // @DisplayName: Angle Max diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index aef7efdc1c..e3914e6313 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -467,5 +467,15 @@ enum ap_message { // subsystem specific error codes -- crash checker #define ERROR_CODE_CRASH_CHECK_CRASH 1 +// Arming Check Enable/Disable bits +#define ARMING_CHECK_NONE 0x00 +#define ARMING_CHECK_ALL 0x01 +#define ARMING_CHECK_BARO 0x02 +#define ARMING_CHECK_COMPASS 0x04 +#define ARMING_CHECK_GPS 0x08 +#define ARMING_CHECK_INS 0x10 +#define ARMING_CHECK_PARAMETERS 0x20 +#define ARMING_CHECK_RC 0x40 +#define ARMING_CHECK_VOLTAGE 0x80 #endif // _DEFINES_H diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index e6072c6b1e..f2fcf06ba4 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -211,13 +211,14 @@ static void init_arm_motors() 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 ) { + if (ap.pre_arm_check) { return; } // succeed if pre arm checks are disabled - if(!g.arming_check_enabled) { + if(g.arming_check_enabled == ARMING_CHECK_NONE) { set_pre_arm_check(true); + set_pre_arm_rc_check(true); return; } @@ -230,117 +231,135 @@ static void pre_arm_checks(bool display_failure) return; } - // pre-arm check to ensure ch7 and ch8 have different functions - if ((g.ch7_option != 0 || g.ch8_option != 0) && g.ch7_option == g.ch8_option) { - if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Ch7&Ch8 Opt cannot be same")); - } - 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 accels and gyros are healthy - if(!ins.healthy()) { - if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: INS not healthy")); - } - return; - } - - // check the compass is healthy - if(!compass.healthy) { - if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass not healthy")); - } - return; - } - - // check compass learning is on or offsets have been set - Vector3f offsets = compass.get_offsets(); - if(!compass._learn && offsets.length() == 0) { - if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass not calibrated")); - } - return; - } - - // check for unreasonable compass offsets - if(offsets.length() > 500) { - if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass offsets too high")); - } - return; - } - - // check for unreasonable mag field length - float mag_field = pythagorous3(compass.mag_x, compass.mag_y, compass.mag_z); - if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65 || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35) { - if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check mag field")); - } - return; - } - - // barometer health check - if(!barometer.healthy) { - if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Baro not healthy")); - } - 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")); - } - return; - } -#endif - -#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1 - // check board voltage - if(board_voltage() < BOARD_VOLTAGE_MIN || board_voltage() > BOARD_VOLTAGE_MAX) { - if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check Board Voltage")); - } - return; - } -#endif - - // failsafe parameter checks - if (g.failsafe_throttle) { - // check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900 - if (g.rc_3.radio_min <= g.failsafe_throttle_value+10 || g.failsafe_throttle_value < 910) { + // check Baro + if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_BARO)) { + // barometer health check + if(!barometer.healthy) { if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check FS_THR_VALUE")); + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Baro not healthy")); } return; } } - // lean angle parameter check - if (g.angle_max < 1000 || g.angle_max > 8000) { - if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check ANGLE_MAX")); + // check Compass + if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_COMPASS)) { + // check the compass is healthy + if(!compass.healthy) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass not healthy")); + } + return; + } + + // check compass learning is on or offsets have been set + Vector3f offsets = compass.get_offsets(); + if(!compass._learn && offsets.length() == 0) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass not calibrated")); + } + return; + } + + // check for unreasonable compass offsets + if(offsets.length() > 500) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass offsets too high")); + } + return; + } + + // check for unreasonable mag field length + float mag_field = pythagorous3(compass.mag_x, compass.mag_y, compass.mag_z); + if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65 || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check mag field")); + } + return; } - return; } - // check gps is ok if required - note this same check is repeated again in arm_checks - if (mode_requires_GPS(control_mode) && !pre_arm_gps_checks()) { - if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos")); + // 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) && !pre_arm_gps_checks()) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad GPS Pos")); + } + 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")); + } + return; + } +#endif + } + + // check INS + if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_INS)) { + // check accelerometers have been calibrated + if(!ins.calibrated()) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: INS not calibrated")); + } + return; + } + + // check accels and gyros are healthy + if(!ins.healthy()) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: INS not healthy")); + } + return; + } + } + +#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1 + // check board voltage + if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_VOLTAGE)) { + if(board_voltage() < BOARD_VOLTAGE_MIN || board_voltage() > BOARD_VOLTAGE_MAX) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check Board Voltage")); + } + return; + } + } +#endif + + // check various parameter values + if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_PARAMETERS)) { + + // ensure ch7 and ch8 have different functions + if ((g.ch7_option != 0 || g.ch8_option != 0) && g.ch7_option == g.ch8_option) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Ch7&Ch8 Opt cannot be same")); + } + return; + } + + // failsafe parameter checks + if (g.failsafe_throttle) { + // check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900 + if (g.rc_3.radio_min <= g.failsafe_throttle_value+10 || g.failsafe_throttle_value < 910) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check FS_THR_VALUE")); + } + return; + } + } + + // lean angle parameter check + if (g.angle_max < 1000 || g.angle_max > 8000) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check ANGLE_MAX")); + } + return; } - return; } // if we've gotten this far then pre arm checks have completed @@ -355,6 +374,12 @@ static void pre_arm_rc_checks() return; } + // set rc-checks to success if RC checks are disabled + if ((g.arming_check_enabled != ARMING_CHECK_ALL) && !(g.arming_check_enabled & ARMING_CHECK_RC)) { + set_pre_arm_rc_check(true); + return; + } + // check if radio has been calibrated if(!g.rc_3.radio_min.load() && !g.rc_3.radio_max.load()) { return; @@ -371,7 +396,7 @@ static void pre_arm_rc_checks() } // if we've gotten this far rc is ok - ap.pre_arm_rc_check = true; + set_pre_arm_rc_check(true); } // performs pre_arm gps related checks and returns true if passed @@ -393,24 +418,29 @@ static bool pre_arm_gps_checks() static bool arm_checks(bool display_failure) { // succeed if arming checks are disabled - if (!g.arming_check_enabled) { + if (g.arming_check_enabled == ARMING_CHECK_NONE) { return true; } - // check throttle is above failsafe throttle - if (g.failsafe_throttle != FS_THR_DISABLED && g.rc_3.radio_in < g.failsafe_throttle_value) { - if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Thr below FS")); + // 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) && !pre_arm_gps_checks()) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Bad GPS Pos")); + } + return false; } - return false; } - // check gps is ok if required - note this same check is also done in pre-arm checks - if (mode_requires_GPS(control_mode) && !pre_arm_gps_checks()) { - if (display_failure) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Bad GPS Pos")); + // check parameters + if ((g.arming_check_enabled == ARMING_CHECK_ALL) || (g.arming_check_enabled & ARMING_CHECK_PARAMETERS)) { + // check throttle is above failsafe throttle + if (g.failsafe_throttle != FS_THR_DISABLED && g.rc_3.radio_in < g.failsafe_throttle_value) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Thr below FS")); + } + return false; } - return false; } // check if safety switch has been pushed