From 933ffb2b109f52c607ec300b060ec220a4bed5fd Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 22 Dec 2015 15:12:15 -0800 Subject: [PATCH] Copter: move prearm logic to arming_checks.cpp --- ArduCopter/ArduCopter.cpp | 10 +- ArduCopter/Copter.h | 2 + ArduCopter/arming_checks.cpp | 659 +++++++++++++++++++++++++++++++++++ ArduCopter/config.h | 4 + ArduCopter/motors.cpp | 632 +-------------------------------- 5 files changed, 667 insertions(+), 640 deletions(-) create mode 100644 ArduCopter/arming_checks.cpp diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 8dde897a5c..27c1a306df 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -469,15 +469,7 @@ void Copter::one_hz_loop() Log_Write_Data(DATA_AP_STATE, ap.value); } - // perform pre-arm checks & display failures every 30 seconds - static uint8_t pre_arm_display_counter = 15; - pre_arm_display_counter++; - if (pre_arm_display_counter >= 30) { - pre_arm_checks(true); - pre_arm_display_counter = 0; - }else{ - pre_arm_checks(false); - } + update_arming_checks(); if (!motors.armed()) { // make it possible to change ahrs orientation at runtime during initial config diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index b6ae3b09b2..6923f01eb1 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -854,6 +854,8 @@ private: void arm_motors_check(); void auto_disarm_check(); bool init_arm_motors(bool arming_from_gcs); + void update_arming_checks(void); + bool all_arming_checks_passing(bool arming_from_gcs); bool pre_arm_checks(bool display_failure); void pre_arm_rc_checks(); bool pre_arm_gps_checks(bool display_failure); diff --git a/ArduCopter/arming_checks.cpp b/ArduCopter/arming_checks.cpp new file mode 100644 index 0000000000..d9f9c02bd8 --- /dev/null +++ b/ArduCopter/arming_checks.cpp @@ -0,0 +1,659 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Copter.h" + +// performs pre-arm checks. expects to be called at 1hz. +void Copter::update_arming_checks(void) +{ + // perform pre-arm checks & display failures every 30 seconds + static uint8_t pre_arm_display_counter = PREARM_DISPLAY_PERIOD/2; + pre_arm_display_counter++; + bool display_fail = false; + if (pre_arm_display_counter >= PREARM_DISPLAY_PERIOD) { + display_fail = true; + pre_arm_display_counter = 0; + } + + if(pre_arm_checks(display_fail)) { + set_pre_arm_check(true); + } +} + + +// performs pre-arm checks and arming checks +bool Copter::all_arming_checks_passing(bool arming_from_gcs) +{ + if (pre_arm_checks(true)) { + set_pre_arm_check(true); + } + + return ap.pre_arm_check && arm_checks(true, arming_from_gcs); +} + +// perform pre-arm checks and set ap.pre_arm_check flag +// return true if the checks pass successfully +bool Copter::pre_arm_checks(bool display_failure) +{ + // exit immediately if already armed + if (motors.armed()) { + return true; + } + + // check if motor interlock and Emergency Stop aux switches are used + // at the same time. This cannot be allowed. + if (check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK) && check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){ + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Interlock/E-Stop Conflict"); + } + return false; + } + + // check if motor interlock aux switch is in use + // if it is, switch needs to be in disabled position to arm + // otherwise exit immediately. This check to be repeated, + // as state can change at any time. + if (ap.using_interlock && motors.get_interlock()){ + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Motor Interlock Enabled"); + } + return false; + } + + // exit immediately if we've already successfully performed the pre-arm check + if (ap.pre_arm_check) { + // run gps checks because results may change and affect LED colour + // no need to display failures because arm_checks will do that if the pilot tries to arm + pre_arm_gps_checks(false); + return true; + } + + // succeed if pre arm checks are disabled + if(g.arming_check == ARMING_CHECK_NONE) { + set_pre_arm_check(true); + set_pre_arm_rc_check(true); + return true; + } + + // pre-arm rc checks a prerequisite + pre_arm_rc_checks(); + if(!ap.pre_arm_rc_check) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: RC not calibrated"); + } + return false; + } + // check Baro + if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) { + // barometer health check + if(!barometer.all_healthy()) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Barometer not healthy"); + } + return false; + } + // Check baro & inav alt are within 1m if EKF is operating in an absolute position mode. + // Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height + // that may differ from the baro height due to baro drift. + nav_filter_status filt_status = inertial_nav.get_filter_status(); + bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs); + if (using_baro_ref) { + if (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Altitude disparity"); + } + return false; + } + } + } + + // check Compass + if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_COMPASS)) { + // check the primary compass is healthy + if(!compass.healthy()) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass not healthy"); + } + return false; + } + + // check compass learning is on or offsets have been set + if(!compass.configured()) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass not calibrated"); + } + return false; + } + + // check for unreasonable compass offsets + Vector3f offsets = compass.get_offsets(); + if(offsets.length() > COMPASS_OFFSETS_MAX) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass offsets too high"); + } + return false; + } + + // check for unreasonable mag field length + float mag_field = compass.get_field().length(); + if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65f || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35f) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check mag field"); + } + return false; + } + + // check all compasses point in roughly same direction + if (!compass.consistent()) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent compasses"); + } + return false; + } + + } + + // check GPS + if (!pre_arm_gps_checks(display_failure)) { + return false; + } + + #if AC_FENCE == ENABLED + // check fence is initialised + if(!fence.pre_arm_check()) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check fence"); + } + return false; + } + #endif + + // check INS + if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) { + // check accelerometers have been calibrated + if(!ins.accel_calibrated_ok_all()) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Accels not calibrated"); + } + return false; + } + + // check accels are healthy + if(!ins.get_accel_health_all()) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Accelerometers not healthy"); + } + return false; + } + + // check all accelerometers point in roughly same direction + if (ins.get_accel_count() > 1) { + const Vector3f &prime_accel_vec = ins.get_accel(); + for(uint8_t i=0; i= 2) { + /* + * for boards with 3 IMUs we only use the first two + * in the EKF. Allow for larger accel discrepancy + * for IMU3 as it may be running at a different temperature + */ + threshold *= 2; + } + if (vec_diff.length() > threshold) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent Accelerometers"); + } + return false; + } + } + } + + // check gyros are healthy + if(!ins.get_gyro_health_all()) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Gyros not healthy"); + } + return false; + } + + // check all gyros are consistent + if (ins.get_gyro_count() > 1) { + for(uint8_t i=0; i PREARM_MAX_GYRO_VECTOR_DIFF) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent Gyros"); + } + return false; + } + } + } + + // get ekf attitude (if bad, it's usually the gyro biases) + if (!pre_arm_ekf_attitude_check()) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: gyros still settling"); + } + return false; + } + } + #if CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN + #ifndef CONFIG_ARCH_BOARD_PX4FMU_V1 + // check board voltage + if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) { + if(hal.analogin->board_voltage() < BOARD_VOLTAGE_MIN || hal.analogin->board_voltage() > BOARD_VOLTAGE_MAX) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Board Voltage"); + } + return false; + } + } + #endif + #endif + + // check battery voltage + if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) { + if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Battery"); + } + return false; + } + } + + // check various parameter values + if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) { + + // ensure ch7 and ch8 have different functions + if (check_duplicate_auxsw()) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Duplicate Aux Switch Options"); + } + return false; + } + + // 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 (channel_throttle->radio_min <= g.failsafe_throttle_value+10 || g.failsafe_throttle_value < 910) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check FS_THR_VALUE"); + } + return false; + } + } + + // lean angle parameter check + if (aparm.angle_max < 1000 || aparm.angle_max > 8000) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check ANGLE_MAX"); + } + return false; + } + + // acro balance parameter check + if ((g.acro_balance_roll > g.p_stabilize_roll.kP()) || (g.acro_balance_pitch > g.p_stabilize_pitch.kP())) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ACRO_BAL_ROLL/PITCH"); + } + return false; + } + + #if CONFIG_SONAR == ENABLED && OPTFLOW == ENABLED + // check range finder if optflow enabled + if (optflow.enabled() && !sonar.pre_arm_check()) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check range finder"); + } + return false; + } + #endif + #if FRAME_CONFIG == HELI_FRAME + // check helicopter parameters + if (!motors.parameter_check(display_failure)) { + return false; + } + #endif // HELI_FRAME + } + + // check throttle is above failsafe throttle + // this is near the bottom to allow other failures to be displayed before checking pilot throttle + if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) { + if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->radio_in < g.failsafe_throttle_value) { + if (display_failure) { + #if FRAME_CONFIG == HELI_FRAME + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Collective below Failsafe"); + #else + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Throttle below Failsafe"); + #endif + } + return false; + } + } + + return true; +} + +// perform pre_arm_rc_checks checks and set ap.pre_arm_rc_check flag +void Copter::pre_arm_rc_checks() +{ + // exit immediately if we've already successfully performed the pre-arm rc check + if( ap.pre_arm_rc_check ) { + return; + } + + // set rc-checks to success if RC checks are disabled + if ((g.arming_check != ARMING_CHECK_ALL) && !(g.arming_check & ARMING_CHECK_RC)) { + set_pre_arm_rc_check(true); + return; + } + + // check if radio has been calibrated + if(!channel_throttle->radio_min.configured() && !channel_throttle->radio_max.configured()) { + return; + } + + // check channels 1 & 2 have min <= 1300 and max >= 1700 + if (channel_roll->radio_min > 1300 || channel_roll->radio_max < 1700 || channel_pitch->radio_min > 1300 || channel_pitch->radio_max < 1700) { + return; + } + + // check channels 3 & 4 have min <= 1300 and max >= 1700 + if (channel_throttle->radio_min > 1300 || channel_throttle->radio_max < 1700 || channel_yaw->radio_min > 1300 || channel_yaw->radio_max < 1700) { + return; + } + + // check channels 1 & 2 have trim >= 1300 and <= 1700 + if (channel_roll->radio_trim < 1300 || channel_roll->radio_trim > 1700 || channel_pitch->radio_trim < 1300 || channel_pitch->radio_trim > 1700) { + return; + } + + // check channel 4 has trim >= 1300 and <= 1700 + if (channel_yaw->radio_trim < 1300 || channel_yaw->radio_trim > 1700) { + return; + } + + // if we've gotten this far rc is ok + set_pre_arm_rc_check(true); +} + +// performs pre_arm gps related checks and returns true if passed +bool Copter::pre_arm_gps_checks(bool display_failure) +{ + // always check if inertial nav has started and is ready + if(!ahrs.healthy()) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Nav Checks"); + } + return false; + } + + // check if flight mode requires GPS + bool gps_required = mode_requires_GPS(control_mode); + + #if AC_FENCE == ENABLED + // if circular fence is enabled we need GPS + if ((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) { + gps_required = true; + } + #endif + + // return true if GPS is not required + if (!gps_required) { + AP_Notify::flags.pre_arm_gps_check = true; + return true; + } + + // ensure GPS is ok + if (!position_ok()) { + if (display_failure) { + const char *reason = ahrs.prearm_failure_reason(); + if (reason) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: %s", reason); + } else { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Need 3D Fix"); + } + } + AP_Notify::flags.pre_arm_gps_check = false; + return false; + } + + // check EKF compass variance is below failsafe threshold + float vel_variance, pos_variance, hgt_variance, tas_variance; + Vector3f mag_variance; + Vector2f offset; + ahrs.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance, offset); + if (mag_variance.length() >= g.fs_ekf_thresh) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF compass variance"); + } + return false; + } + + // check home and EKF origin are not too far + if (far_from_EKF_origin(ahrs.get_home())) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF-home variance"); + } + AP_Notify::flags.pre_arm_gps_check = false; + return false; + } + + // return true immediately if gps check is disabled + if (!(g.arming_check == ARMING_CHECK_ALL || g.arming_check & ARMING_CHECK_GPS)) { + AP_Notify::flags.pre_arm_gps_check = true; + return true; + } + + // warn about hdop separately - to prevent user confusion with no gps lock + if (gps.get_hdop() > g.gps_hdop_good) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: High GPS HDOP"); + } + AP_Notify::flags.pre_arm_gps_check = false; + return false; + } + + // if we got here all must be ok + AP_Notify::flags.pre_arm_gps_check = true; + return true; +} + +// check ekf attitude is acceptable +bool Copter::pre_arm_ekf_attitude_check() +{ + // get ekf filter status + nav_filter_status filt_status = inertial_nav.get_filter_status(); + + return filt_status.flags.attitude; +} + +// arm_checks - perform final checks before arming +// always called just before arming. Return true if ok to arm +// has side-effect that logging is started +bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) +{ + #if LOGGING_ENABLED == ENABLED + // start dataflash + start_logging(); + #endif + + // check accels and gyro are healthy + if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) { + if(!ins.get_accel_health_all()) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Accelerometers not healthy"); + } + return false; + } + if(!ins.get_gyro_health_all()) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Gyros not healthy"); + } + return false; + } + // get ekf attitude (if bad, it's usually the gyro biases) + if (!pre_arm_ekf_attitude_check()) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: gyros still settling"); + } + return false; + } + } + + // always check if inertial nav has started and is ready + if(!ahrs.healthy()) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Waiting for Nav Checks"); + } + return false; + } + + if(compass.is_calibrating()) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Compass calibration running"); + } + return false; + } + + // always check if the current mode allows arming + if (!mode_allows_arming(control_mode, arming_from_gcs)) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Mode not armable"); + } + return false; + } + + // always check gps + if (!pre_arm_gps_checks(display_failure)) { + return false; + } + + // if we are using motor interlock switch and it's enabled, fail to arm + if (ap.using_interlock && motors.get_interlock()){ + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Interlock Enabled"); + return false; + } + + // if we are not using Emergency Stop switch option, force Estop false to ensure motors + // can run normally + if (!check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){ + set_motor_emergency_stop(false); + // if we are using motor Estop switch, it must not be in Estop position + } else if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop){ + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Emergency Stopped"); + return false; + } + + // succeed if arming checks are disabled + if (g.arming_check == ARMING_CHECK_NONE) { + return true; + } + + // baro checks + if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) { + // baro health check + if (!barometer.all_healthy()) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Barometer not healthy"); + } + return false; + } + // Check baro & inav alt are within 1m if EKF is operating in an absolute position mode. + // Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height + // that may differ from the baro height due to baro drift. + nav_filter_status filt_status = inertial_nav.get_filter_status(); + bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs); + if (using_baro_ref && (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM)) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Altitude disparity"); + } + return false; + } + } + + #if AC_FENCE == ENABLED + // check vehicle is within fence + if(!fence.pre_arm_check()) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: check fence"); + } + return false; + } + #endif + + // check lean angle + if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) { + if (degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()))*100.0f > aparm.angle_max) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Leaning"); + } + return false; + } + } + + // check battery voltage + if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) { + if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Check Battery"); + } + return false; + } + } + + // check throttle + if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) { + // check throttle is not too low - must be above failsafe throttle + if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->radio_in < g.failsafe_throttle_value) { + if (display_failure) { + #if FRAME_CONFIG == HELI_FRAME + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective below Failsafe"); + #else + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle below Failsafe"); + #endif + } + return false; + } + + // check throttle is not too high - skips checks if arming from GCS in Guided + if (!(arming_from_gcs && control_mode == GUIDED)) { + // above top of deadband is too always high + if (channel_throttle->control_in > get_takeoff_trigger_throttle()) { + if (display_failure) { + #if FRAME_CONFIG == HELI_FRAME + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high"); + #else + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high"); + #endif + } + return false; + } + // in manual modes throttle must be at zero + if ((mode_has_manual_throttle(control_mode) || control_mode == DRIFT) && channel_throttle->control_in > 0) { + if (display_failure) { + #if FRAME_CONFIG == HELI_FRAME + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high"); + #else + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high"); + #endif + } + return false; + } + } + } + + // check if safety switch has been pushed + if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { + if (display_failure) { + gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Safety Switch"); + } + return false; + } + + // if we've gotten this far all is ok + return true; +} diff --git a/ArduCopter/config.h b/ArduCopter/config.h index eddb69bcdb..6c2eacec97 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -232,6 +232,10 @@ # define FS_CLOSE_TO_HOME_CM 500 // if vehicle within 5m of home, vehicle will LAND instead of RTL during some failsafes #endif +#ifndef PREARM_DISPLAY_PERIOD +# define PREARM_DISPLAY_PERIOD 30 +#endif + // pre-arm baro vs inertial nav max alt disparity #ifndef PREARM_MAX_ALT_DISPARITY_CM # define PREARM_MAX_ALT_DISPARITY_CM 100 // barometer and inertial nav altitude must be within this many centimeters diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 48b888671b..8798a246b7 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -133,7 +133,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs) in_arm_motors = true; // run pre-arm-checks and display failures - if(!pre_arm_checks(true) || !arm_checks(true, arming_from_gcs)) { + if(!all_arming_checks_passing(arming_from_gcs)) { AP_Notify::events.arming_failed = true; in_arm_motors = false; return false; @@ -209,636 +209,6 @@ bool Copter::init_arm_motors(bool arming_from_gcs) return true; } -// perform pre-arm checks and set ap.pre_arm_check flag -// return true if the checks pass successfully -bool Copter::pre_arm_checks(bool display_failure) -{ - // exit immediately if already armed - if (motors.armed()) { - return true; - } - - // check if motor interlock and Emergency Stop aux switches are used - // at the same time. This cannot be allowed. - if (check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK) && check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){ - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Interlock/E-Stop Conflict"); - } - return false; - } - - // check if motor interlock aux switch is in use - // if it is, switch needs to be in disabled position to arm - // otherwise exit immediately. This check to be repeated, - // as state can change at any time. - if (ap.using_interlock && motors.get_interlock()){ - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Motor Interlock Enabled"); - } - return false; - } - - // exit immediately if we've already successfully performed the pre-arm check - if (ap.pre_arm_check) { - // run gps checks because results may change and affect LED colour - // no need to display failures because arm_checks will do that if the pilot tries to arm - pre_arm_gps_checks(false); - return true; - } - - // succeed if pre arm checks are disabled - if(g.arming_check == ARMING_CHECK_NONE) { - set_pre_arm_check(true); - set_pre_arm_rc_check(true); - return true; - } - - // pre-arm rc checks a prerequisite - pre_arm_rc_checks(); - if(!ap.pre_arm_rc_check) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: RC not calibrated"); - } - return false; - } - // check Baro - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) { - // barometer health check - if(!barometer.all_healthy()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Barometer not healthy"); - } - return false; - } - // Check baro & inav alt are within 1m if EKF is operating in an absolute position mode. - // Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height - // that may differ from the baro height due to baro drift. - nav_filter_status filt_status = inertial_nav.get_filter_status(); - bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs); - if (using_baro_ref) { - if (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Altitude disparity"); - } - return false; - } - } - } - - // check Compass - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_COMPASS)) { - // check the primary compass is healthy - if(!compass.healthy()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass not healthy"); - } - return false; - } - - // check compass learning is on or offsets have been set - if(!compass.configured()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass not calibrated"); - } - return false; - } - - // check for unreasonable compass offsets - Vector3f offsets = compass.get_offsets(); - if(offsets.length() > COMPASS_OFFSETS_MAX) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass offsets too high"); - } - return false; - } - - // check for unreasonable mag field length - float mag_field = compass.get_field().length(); - if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65f || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35f) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check mag field"); - } - return false; - } - - // check all compasses point in roughly same direction - if (!compass.consistent()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent compasses"); - } - return false; - } - - } - - // check GPS - if (!pre_arm_gps_checks(display_failure)) { - return false; - } - -#if AC_FENCE == ENABLED - // check fence is initialised - if(!fence.pre_arm_check()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check fence"); - } - return false; - } -#endif - - // check INS - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) { - // check accelerometers have been calibrated - if(!ins.accel_calibrated_ok_all()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Accels not calibrated"); - } - return false; - } - - // check accels are healthy - if(!ins.get_accel_health_all()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Accelerometers not healthy"); - } - return false; - } - - // check all accelerometers point in roughly same direction - if (ins.get_accel_count() > 1) { - const Vector3f &prime_accel_vec = ins.get_accel(); - for(uint8_t i=0; i= 2) { - /* - for boards with 3 IMUs we only use the first two - in the EKF. Allow for larger accel discrepancy - for IMU3 as it may be running at a different temperature - */ - threshold *= 2; - } - if (vec_diff.length() > threshold) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent Accelerometers"); - } - return false; - } - } - } - - // check gyros are healthy - if(!ins.get_gyro_health_all()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Gyros not healthy"); - } - return false; - } - - // check all gyros are consistent - if (ins.get_gyro_count() > 1) { - for(uint8_t i=0; i PREARM_MAX_GYRO_VECTOR_DIFF) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent Gyros"); - } - return false; - } - } - } - - // get ekf attitude (if bad, it's usually the gyro biases) - if (!pre_arm_ekf_attitude_check()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: gyros still settling"); - } - return false; - } - } -#if CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN -#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1 - // check board voltage - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) { - if(hal.analogin->board_voltage() < BOARD_VOLTAGE_MIN || hal.analogin->board_voltage() > BOARD_VOLTAGE_MAX) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Board Voltage"); - } - return false; - } - } -#endif -#endif - - // check battery voltage - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) { - if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Battery"); - } - return false; - } - } - - // check various parameter values - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) { - - // ensure ch7 and ch8 have different functions - if (check_duplicate_auxsw()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Duplicate Aux Switch Options"); - } - return false; - } - - // 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 (channel_throttle->radio_min <= g.failsafe_throttle_value+10 || g.failsafe_throttle_value < 910) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check FS_THR_VALUE"); - } - return false; - } - } - - // lean angle parameter check - if (aparm.angle_max < 1000 || aparm.angle_max > 8000) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check ANGLE_MAX"); - } - return false; - } - - // acro balance parameter check - if ((g.acro_balance_roll > g.p_stabilize_roll.kP()) || (g.acro_balance_pitch > g.p_stabilize_pitch.kP())) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ACRO_BAL_ROLL/PITCH"); - } - return false; - } - -#if CONFIG_SONAR == ENABLED && OPTFLOW == ENABLED - // check range finder if optflow enabled - if (optflow.enabled() && !sonar.pre_arm_check()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check range finder"); - } - return false; - } -#endif -#if FRAME_CONFIG == HELI_FRAME - // check helicopter parameters - if (!motors.parameter_check(display_failure)) { - return false; - } -#endif // HELI_FRAME - } - - // check throttle is above failsafe throttle - // this is near the bottom to allow other failures to be displayed before checking pilot throttle - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) { - if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->radio_in < g.failsafe_throttle_value) { - if (display_failure) { - #if FRAME_CONFIG == HELI_FRAME - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Collective below Failsafe"); - #else - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Throttle below Failsafe"); - #endif - } - return false; - } - } - - // if we've gotten this far then pre arm checks have completed - set_pre_arm_check(true); - return true; -} - -// perform pre_arm_rc_checks checks and set ap.pre_arm_rc_check flag -void Copter::pre_arm_rc_checks() -{ - // exit immediately if we've already successfully performed the pre-arm rc check - if( ap.pre_arm_rc_check ) { - return; - } - - // set rc-checks to success if RC checks are disabled - if ((g.arming_check != ARMING_CHECK_ALL) && !(g.arming_check & ARMING_CHECK_RC)) { - set_pre_arm_rc_check(true); - return; - } - - // check if radio has been calibrated - if(!channel_throttle->radio_min.configured() && !channel_throttle->radio_max.configured()) { - return; - } - - // check channels 1 & 2 have min <= 1300 and max >= 1700 - if (channel_roll->radio_min > 1300 || channel_roll->radio_max < 1700 || channel_pitch->radio_min > 1300 || channel_pitch->radio_max < 1700) { - return; - } - - // check channels 3 & 4 have min <= 1300 and max >= 1700 - if (channel_throttle->radio_min > 1300 || channel_throttle->radio_max < 1700 || channel_yaw->radio_min > 1300 || channel_yaw->radio_max < 1700) { - return; - } - - // check channels 1 & 2 have trim >= 1300 and <= 1700 - if (channel_roll->radio_trim < 1300 || channel_roll->radio_trim > 1700 || channel_pitch->radio_trim < 1300 || channel_pitch->radio_trim > 1700) { - return; - } - - // check channel 4 has trim >= 1300 and <= 1700 - if (channel_yaw->radio_trim < 1300 || channel_yaw->radio_trim > 1700) { - return; - } - - // if we've gotten this far rc is ok - set_pre_arm_rc_check(true); -} - -// performs pre_arm gps related checks and returns true if passed -bool Copter::pre_arm_gps_checks(bool display_failure) -{ - // always check if inertial nav has started and is ready - if(!ahrs.healthy()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Nav Checks"); - } - return false; - } - - // check if flight mode requires GPS - bool gps_required = mode_requires_GPS(control_mode); - -#if AC_FENCE == ENABLED - // if circular fence is enabled we need GPS - if ((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) { - gps_required = true; - } -#endif - - // return true if GPS is not required - if (!gps_required) { - AP_Notify::flags.pre_arm_gps_check = true; - return true; - } - - // ensure GPS is ok - if (!position_ok()) { - if (display_failure) { - const char *reason = ahrs.prearm_failure_reason(); - if (reason) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "PreArm: %s", reason); - } else { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Need 3D Fix"); - } - } - AP_Notify::flags.pre_arm_gps_check = false; - return false; - } - - // check EKF compass variance is below failsafe threshold - float vel_variance, pos_variance, hgt_variance, tas_variance; - Vector3f mag_variance; - Vector2f offset; - ahrs.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance, offset); - if (mag_variance.length() >= g.fs_ekf_thresh) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF compass variance"); - } - return false; - } - - // check home and EKF origin are not too far - if (far_from_EKF_origin(ahrs.get_home())) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF-home variance"); - } - AP_Notify::flags.pre_arm_gps_check = false; - return false; - } - - // return true immediately if gps check is disabled - if (!(g.arming_check == ARMING_CHECK_ALL || g.arming_check & ARMING_CHECK_GPS)) { - AP_Notify::flags.pre_arm_gps_check = true; - return true; - } - - // warn about hdop separately - to prevent user confusion with no gps lock - if (gps.get_hdop() > g.gps_hdop_good) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: High GPS HDOP"); - } - AP_Notify::flags.pre_arm_gps_check = false; - return false; - } - - // if we got here all must be ok - AP_Notify::flags.pre_arm_gps_check = true; - return true; -} - -// check ekf attitude is acceptable -bool Copter::pre_arm_ekf_attitude_check() -{ - // get ekf filter status - nav_filter_status filt_status = inertial_nav.get_filter_status(); - - return filt_status.flags.attitude; -} - -// arm_checks - perform final checks before arming -// always called just before arming. Return true if ok to arm -// has side-effect that logging is started -bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) -{ -#if LOGGING_ENABLED == ENABLED - // start dataflash - start_logging(); -#endif - - // check accels and gyro are healthy - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) { - if(!ins.get_accel_health_all()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Accelerometers not healthy"); - } - return false; - } - if(!ins.get_gyro_health_all()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Gyros not healthy"); - } - return false; - } - // get ekf attitude (if bad, it's usually the gyro biases) - if (!pre_arm_ekf_attitude_check()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: gyros still settling"); - } - return false; - } - } - - // always check if inertial nav has started and is ready - if(!ahrs.healthy()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Waiting for Nav Checks"); - } - return false; - } - - if(compass.is_calibrating()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Compass calibration running"); - } - return false; - } - - // always check if the current mode allows arming - if (!mode_allows_arming(control_mode, arming_from_gcs)) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Mode not armable"); - } - return false; - } - - // always check gps - if (!pre_arm_gps_checks(display_failure)) { - return false; - } - - // if we are using motor interlock switch and it's enabled, fail to arm - if (ap.using_interlock && motors.get_interlock()){ - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Interlock Enabled"); - return false; - } - - // if we are not using Emergency Stop switch option, force Estop false to ensure motors - // can run normally - if (!check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){ - set_motor_emergency_stop(false); - // if we are using motor Estop switch, it must not be in Estop position - } else if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop){ - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Emergency Stopped"); - return false; - } - - // succeed if arming checks are disabled - if (g.arming_check == ARMING_CHECK_NONE) { - return true; - } - - // baro checks - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) { - // baro health check - if (!barometer.all_healthy()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Barometer not healthy"); - } - return false; - } - // Check baro & inav alt are within 1m if EKF is operating in an absolute position mode. - // Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height - // that may differ from the baro height due to baro drift. - nav_filter_status filt_status = inertial_nav.get_filter_status(); - bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs); - if (using_baro_ref && (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM)) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Altitude disparity"); - } - return false; - } - } - -#if AC_FENCE == ENABLED - // check vehicle is within fence - if(!fence.pre_arm_check()) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: check fence"); - } - return false; - } -#endif - - // check lean angle - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) { - if (degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()))*100.0f > aparm.angle_max) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Leaning"); - } - return false; - } - } - - // check battery voltage - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) { - if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Check Battery"); - } - return false; - } - } - - // check throttle - if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) { - // check throttle is not too low - must be above failsafe throttle - if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->radio_in < g.failsafe_throttle_value) { - if (display_failure) { -#if FRAME_CONFIG == HELI_FRAME - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective below Failsafe"); -#else - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle below Failsafe"); -#endif - } - return false; - } - - // check throttle is not too high - skips checks if arming from GCS in Guided - if (!(arming_from_gcs && control_mode == GUIDED)) { - // above top of deadband is too always high - if (channel_throttle->control_in > get_takeoff_trigger_throttle()) { - if (display_failure) { -#if FRAME_CONFIG == HELI_FRAME - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high"); -#else - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high"); -#endif - } - return false; - } - // in manual modes throttle must be at zero - if ((mode_has_manual_throttle(control_mode) || control_mode == DRIFT) && channel_throttle->control_in > 0) { - if (display_failure) { -#if FRAME_CONFIG == HELI_FRAME - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high"); -#else - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high"); -#endif - } - return false; - } - } - } - - // check if safety switch has been pushed - if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { - if (display_failure) { - gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Safety Switch"); - } - return false; - } - - // if we've gotten this far all is ok - return true; -} - // init_disarm_motors - disarm motors void Copter::init_disarm_motors() {