Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Rework arming proceedures for interlock/Estop

Browse Source 
to fix race condition.
This commit is contained in:
Robert Lefebvre 2015-09-10 22:43:54 -05:00 committed by Randy Mackay
parent 8630df00b3
commit 2632a2e348
2 changed files with 23 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/ArduCopter.cpp
View File

@ -480,10 +480,16 @@ void Copter::one_hz_loop()
// make it possible to change ahrs orientation at runtime during initial config
ahrs.set_orientation();
#if FRAME_CONFIG == HELI_FRAME
// helicopters are always using motor interlock
set_using_interlock(true);
#else
// check the user hasn't updated the frame orientation
motors.set_frame_orientation(g.frame_orientation);
#if FRAME_CONFIG != HELI_FRAME
// check if we are using motor interlock control on an aux switch
set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK));
// set all throttle channel settings
motors.set_throttle_range(g.throttle_min, channel_throttle->radio_min, channel_throttle->radio_max);
// set hover throttle

53
ArduCopter/motors.cpp
View File

@ -164,34 +164,6 @@ bool Copter::init_arm_motors(bool arming_from_gcs)
}
calc_distance_and_bearing();
#if FRAME_CONFIG == HELI_FRAME
// helicopters are always using motor interlock
set_using_interlock(true);
#else
// check if we are using motor interlock control on an aux switch
set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK));
#endif
// 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");
AP_Notify::flags.armed = false;
in_arm_motors = false;
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");
AP_Notify::flags.armed = false;
in_arm_motors = false;
return false;
}
// enable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(true);
hal.util->set_soft_armed(true);
@ -259,15 +231,6 @@ bool Copter::pre_arm_checks(bool display_failure)
return false;
}
// if we are using Motor Emergency Stop aux switch, check it is not enabled
// and warn if it is
if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop){
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Motor Emergency Stopped");
}
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
@ -743,6 +706,22 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
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;