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

Copter: Rework arming proceedures for interlock/Estop to fix race condition.

This commit is contained in:
Robert Lefebvre 2015-09-10 22:43:54 -05:00 committed by Randy Mackay
parent 45445635d0
commit 61406a32d2
2 changed files with 26 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/ArduCopter.cpp
View File

@ -464,10 +464,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

56
ArduCopter/motors.cpp
View File

@ -190,32 +190,6 @@ bool Copter::init_arm_motors(bool arming_from_gcs)
did_ground_start = true;
}
#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_P(SEVERITY_HIGH,PSTR("Arm: Motor Interlock Enabled"));
AP_Notify::flags.armed = 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_P(SEVERITY_HIGH,PSTR("Arm: Motor Emergency Stopped"));
AP_Notify::flags.armed = false;
return false;
}
// enable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(true);
hal.util->set_soft_armed(true);
@ -271,15 +245,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_P(SEVERITY_HIGH,PSTR("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
@ -731,8 +696,8 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
return false;
}
// heli specific arming check
#if FRAME_CONFIG == HELI_FRAME
// heli specific arming check
// check if rotor is spinning on heli because this could disrupt gyro calibration
if (!motors.allow_arming()){
if (display_failure) {
@ -740,7 +705,24 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
}
return false;
}
#endif // HELI_FRAME
#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_P(SEVERITY_HIGH,PSTR("Arm: Motor Interlock Enabled"));
AP_Notify::flags.armed = 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_P(SEVERITY_HIGH,PSTR("Arm: Motor Emergency Stopped"));
return false;
}
// succeed if arming checks are disabled
if (g.arming_check == ARMING_CHECK_NONE) {