Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: use check_failed function

This commit is contained in:
Peter Barker 2017-08-09 21:36:19 +10:00
parent cba61598b1
commit 291102360a
1 changed files with 61 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

192
ArduCopter/AP_Arming.cpp
View File

@ -35,9 +35,7 @@ bool AP_Arming_Copter::pre_arm_checks(bool display_failure)
// check if motor interlock and Emergency Stop aux switches are used
// at the same time. This cannot be allowed.
if (copter.check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK) && copter.check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Interlock/E-Stop Conflict");
}
check_failed(ARMING_CHECK_NONE, display_failure, "Interlock/E-Stop Conflict");
return false;
}
@ -46,9 +44,7 @@ bool AP_Arming_Copter::pre_arm_checks(bool display_failure)
// otherwise exit immediately. This check to be repeated,
// as state can change at any time.
if (copter.ap.using_interlock && copter.ap.motor_interlock_switch) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Motor Interlock Enabled");
}
check_failed(ARMING_CHECK_NONE, display_failure, "Motor Interlock Enabled");
}
// succeed if pre arm checks are disabled
@ -79,9 +75,7 @@ bool AP_Arming_Copter::barometer_checks(bool display_failure)
bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs);
if (using_baro_ref) {
if (fabsf(_inav.get_altitude() - copter.baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Altitude disparity");
}
check_failed(ARMING_CHECK_BARO, display_failure, "Altitude disparity");
ret = false;
}
}
@ -97,9 +91,7 @@ bool AP_Arming_Copter::compass_checks(bool display_failure)
// check compass offsets have been set. AP_Arming only checks
// this if learning is off; Copter *always* checks.
if (!_compass.configured()) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass not calibrated");
}
check_failed(ARMING_CHECK_COMPASS, display_failure, "Compass not calibrated");
ret = false;
}
}
@ -113,8 +105,10 @@ bool AP_Arming_Copter::fence_checks(bool display_failure)
// check fence is initialised
const char *fail_msg = nullptr;
if (!copter.fence.pre_arm_check(fail_msg)) {
if (display_failure && fail_msg != nullptr) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: %s", fail_msg);
if (fail_msg == nullptr) {
check_failed(ARMING_CHECK_NONE, display_failure, "Check fence");
} else {
check_failed(ARMING_CHECK_NONE, display_failure, "%s", fail_msg);
}
return false;
}
@ -130,9 +124,7 @@ bool AP_Arming_Copter::ins_checks(bool display_failure)
// 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");
}
check_failed(ARMING_CHECK_INS, display_failure, "gyros still settling");
ret = false;
}
}
@ -149,9 +141,7 @@ bool AP_Arming_Copter::board_voltage_checks(bool display_failure)
// check battery voltage
if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_VOLTAGE)) {
if (copter.battery.has_failsafed()) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Battery failsafe");
}
check_failed(ARMING_CHECK_VOLTAGE, display_failure, "Battery failsafe");
return false;
}
@ -171,9 +161,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
// ensure ch7 and ch8 have different functions
if (copter.check_duplicate_auxsw()) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Duplicate Aux Switch Options");
}
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Duplicate Aux Switch Options");
return false;
}
@ -181,27 +169,21 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
if (copter.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 (copter.channel_throttle->get_radio_min() <= copter.g.failsafe_throttle_value+10 || copter.g.failsafe_throttle_value < 910) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check FS_THR_VALUE");
}
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check FS_THR_VALUE");
return false;
}
}
// lean angle parameter check
if (copter.aparm.angle_max < 1000 || copter.aparm.angle_max > 8000) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check ANGLE_MAX");
}
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check ANGLE_MAX");
return false;
}
// acro balance parameter check
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
if ((copter.g.acro_balance_roll > copter.attitude_control->get_angle_roll_p().kP()) || (copter.g.acro_balance_pitch > copter.attitude_control->get_angle_pitch_p().kP())) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: ACRO_BAL_ROLL/PITCH");
}
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "ACRO_BAL_ROLL/PITCH");
return false;
}
#endif
@ -209,9 +191,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
#if RANGEFINDER_ENABLED == ENABLED && OPTFLOW == ENABLED
// check range finder if optflow enabled
if (copter.optflow.enabled() && !copter.rangefinder.pre_arm_check()) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: check range finder");
}
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "check range finder");
return false;
}
#endif
@ -219,6 +199,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
#if FRAME_CONFIG == HELI_FRAME
// check helicopter parameters
if (!copter.motors->parameter_check(display_failure)) {
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Heli motors checks failed");
return false;
}
// Inverted flight feature disabled for Heli Single and Dual frames
@ -238,9 +219,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
// check adsb avoidance failsafe
#if ADSB_ENABLED == ENABLE
if (copter.failsafe.adsb) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: ADSB threat detected");
}
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "ADSB threat detected");
return false;
}
#endif
@ -308,9 +287,9 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
void AP_Arming_Copter::parameter_checks_pid_warning_message(bool display_failure, const char *error_msg)
{
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check PIDs - %s", error_msg);
}
check_failed(ARMING_CHECK_PARAMETERS,
display_failure,
"Check PIDs - %s", error_msg);
}
// check motor setup was successful
@ -318,9 +297,7 @@ bool AP_Arming_Copter::motor_checks(bool display_failure)
{
// check motors initialised correctly
if (!copter.motors->initialised_ok()) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: check firmware or FRAME_CLASS");
}
check_failed(ARMING_CHECK_NONE, display_failure, "check firmware or FRAME_CLASS");
return false;
}
return true;
@ -332,13 +309,12 @@ bool AP_Arming_Copter::pilot_throttle_checks(bool display_failure)
// this is near the bottom to allow other failures to be displayed before checking pilot throttle
if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC)) {
if (copter.g.failsafe_throttle != FS_THR_DISABLED && copter.channel_throttle->get_radio_in() < copter.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
}
#if FRAME_CONFIG == HELI_FRAME
const char *failmsg = "Collective below Failsafe";
#else
const char *failmsg = "Throttle below Failsafe";
#endif
check_failed(ARMING_CHECK_RC, display_failure, failmsg);
return false;
}
}
@ -366,9 +342,7 @@ bool AP_Arming_Copter::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");
}
check_failed(ARMING_CHECK_NONE, display_failure, "Waiting for Nav Checks");
return false;
}
@ -390,19 +364,16 @@ bool AP_Arming_Copter::gps_checks(bool display_failure)
// ensure GPS is ok
if (!copter.position_ok()) {
if (display_failure) {
const char *reason = ahrs.prearm_failure_reason();
if (reason) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: %s", reason);
const char *reason = ahrs.prearm_failure_reason();
if (reason == nullptr) {
if (!mode_requires_gps && fence_requires_gps) {
// clarify to user why they need GPS in non-GPS flight mode
reason = "Fence enabled, need 3D Fix";
} else {
if (!mode_requires_gps && fence_requires_gps) {
// clarify to user why they need GPS in non-GPS flight mode
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Fence enabled, need 3D Fix");
} else {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Need 3D Fix");
}
reason = "Need 3D Fix";
}
}
check_failed(ARMING_CHECK_NONE, display_failure, "%s", reason);
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
@ -411,9 +382,7 @@ bool AP_Arming_Copter::gps_checks(bool display_failure)
nav_filter_status filt_status;
if (_ahrs_navekf.get_filter_status(filt_status)) {
if (filt_status.flags.gps_glitching) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: GPS glitching");
}
check_failed(ARMING_CHECK_NONE, display_failure, "GPS glitching");
return false;
}
}
@ -424,17 +393,13 @@ bool AP_Arming_Copter::gps_checks(bool display_failure)
Vector2f offset;
_ahrs_navekf.get_variances(vel_variance, pos_variance, hgt_variance, mag_variance, tas_variance, offset);
if (mag_variance.length() >= copter.g.fs_ekf_thresh) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF compass variance");
}
check_failed(ARMING_CHECK_NONE, display_failure, "EKF compass variance");
return false;
}
// check home and EKF origin are not too far
if (copter.far_from_EKF_origin(ahrs.get_home())) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: EKF-home variance");
}
check_failed(ARMING_CHECK_NONE, display_failure, "EKF-home variance");
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
@ -447,9 +412,7 @@ bool AP_Arming_Copter::gps_checks(bool display_failure)
// warn about hdop separately - to prevent user confusion with no gps lock
if (copter.gps.get_hdop() > copter.g.gps_hdop_good) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: High GPS HDOP");
}
check_failed(ARMING_CHECK_GPS, display_failure, "PreArm: High GPS HDOP");
AP_Notify::flags.pre_arm_gps_check = false;
return false;
}
@ -487,7 +450,7 @@ bool AP_Arming_Copter::pre_arm_terrain_check(bool display_failure)
// To-Do: modify RTL return path to fly at or above the RTL_ALT and remove this check
if (copter.rangefinder_state.enabled && (copter.g.rtl_altitude > copter.rangefinder.max_distance_cm_orient(ROTATION_PITCH_270))) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: RTL_ALT above rangefinder max range");
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "RTL_ALT above rangefinder max range");
return false;
}
@ -495,8 +458,8 @@ bool AP_Arming_Copter::pre_arm_terrain_check(bool display_failure)
uint16_t terr_pending, terr_loaded;
copter.terrain.get_statistics(terr_pending, terr_loaded);
bool have_all_data = (terr_pending <= 0);
if (!have_all_data && display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Terrain data");
if (!have_all_data) {
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Waiting for Terrain data");
}
return have_all_data;
#else
@ -516,9 +479,7 @@ bool AP_Arming_Copter::pre_arm_proximity_check(bool display_failure)
// return false if proximity sensor unhealthy
if (copter.g2.proximity.get_status() < AP_Proximity::Proximity_Good) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: check proximity sensor");
}
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "check proximity sensor");
return false;
}
@ -528,9 +489,7 @@ bool AP_Arming_Copter::pre_arm_proximity_check(bool display_failure)
if (copter.avoid.proximity_avoidance_enabled() && copter.g2.proximity.get_closest_object(angle_deg, distance)) {
// display error if something is within 60cm
if (distance <= 0.6f) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: Proximity %d deg, %4.2fm", (int)angle_deg, (double)distance);
}
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Proximity %d deg, %4.2fm", (int)angle_deg, (double)distance);
return false;
}
}
@ -549,34 +508,26 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs)
{
// 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");
}
check_failed(ARMING_CHECK_NONE, display_failure, "Waiting for Nav Checks");
return false;
}
#ifndef ALLOW_ARM_NO_COMPASS
// check compass health
if (!_compass.healthy()) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Compass not healthy");
}
check_failed(ARMING_CHECK_NONE, display_failure, "Compass not healthy");
return false;
}
#endif
if (_compass.is_calibrating()) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Compass calibration running");
}
check_failed(ARMING_CHECK_NONE, display_failure, "Compass calibration running");
return false;
}
//check if compass has calibrated and requires reboot
if (_compass.compass_cal_requires_reboot()) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Arm: Compass calibrated requires reboot");
}
check_failed(ARMING_CHECK_NONE, display_failure, "Compass calibrated requires reboot");
return false;
}
@ -584,9 +535,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs)
// always check if the current mode allows arming
if (!copter.flightmode->allows_arming(arming_from_gcs)) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Mode not armable");
}
check_failed(ARMING_CHECK_NONE, display_failure, "Mode not armable");
return false;
}
@ -598,7 +547,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs)
// if we are using motor interlock switch and it's enabled, fail to arm
// skip check in Throw mode which takes control of the motor interlock
if (copter.ap.using_interlock && copter.ap.motor_interlock_switch) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Interlock Enabled");
check_failed(ARMING_CHECK_NONE, display_failure, "Motor Interlock Enabled");
return false;
}
@ -620,9 +569,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs)
// check lean angle
if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_INS)) {
if (degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()))*100.0f > copter.aparm.angle_max) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Leaning");
}
check_failed(ARMING_CHECK_INS, display_failure, "Leaning");
return false;
}
}
@ -631,9 +578,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs)
#if ADSB_ENABLED == ENABLE
if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_PARAMETERS)) {
if (copter.failsafe.adsb) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: ADSB threat detected");
}
check_failed(ARMING_CHECK_PARAMETERS, display_failure, "ADSB threat detected");
return false;
}
}
@ -641,15 +586,14 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs)
// check throttle
if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC)) {
#if FRAME_CONFIG == HELI_FRAME
const char *rc_item = "Collective";
#else
const char *rc_item = "Throttle";
#endif
// check throttle is not too low - must be above failsafe throttle
if (copter.g.failsafe_throttle != FS_THR_DISABLED && copter.channel_throttle->get_radio_in() < copter.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
}
check_failed(ARMING_CHECK_RC, display_failure, "%s below failsafe", rc_item);
return false;
}
@ -657,24 +601,12 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs)
if (!(arming_from_gcs && (control_mode == GUIDED || control_mode == GUIDED_NOGPS))) {
// above top of deadband is too always high
if (copter.get_pilot_desired_climb_rate(copter.channel_throttle->get_control_in()) > 0.0f) {
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
}
check_failed(ARMING_CHECK_RC, display_failure, "%s too high", rc_item);
return false;
}
// in manual modes throttle must be at zero
if ((copter.flightmode->has_manual_throttle() || control_mode == DRIFT) && copter.channel_throttle->get_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
}
check_failed(ARMING_CHECK_RC, display_failure, "%s too high", rc_item);
return false;
}
}
@ -682,9 +614,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs)
// 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");
}
check_failed(ARMING_CHECK_NONE, display_failure, "Safety Switch");
return false;
}