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

Copter: move all arm check logic into arm_checks

Browse Source
This commit is contained in:
Jonathan Challinger 2014-10-10 02:29:10 -07:00 committed by Randy Mackay
parent e05f8d3087
commit c65cb45c07
2 changed files with 27 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/GCS_Mavlink.pde
View File

@ -1158,7 +1158,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
if (packet.param1 == 1.0f) {
// run pre_arm_checks and arm_checks and display failures
pre_arm_checks(true);
if(ap.pre_arm_check && arm_checks(true)) {
if(ap.pre_arm_check && arm_checks(true, true)) {
if (init_arm_motors()) {
result = MAV_RESULT_ACCEPTED;
} else {
@ -1169,7 +1169,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
AP_Notify::flags.arming_failed = true; // init_arm_motors function will reset flag back to false
result = MAV_RESULT_UNSUPPORTED;
}
} else if (packet.param1 == 0.0f) {
} else if (packet.param1 == 0.0f && (manual_flight_mode(control_mode) || ap.land_complete)) {
init_disarm_motors();
result = MAV_RESULT_ACCEPTED;
} else {

52
ArduCopter/motors.pde
View File

@ -12,7 +12,6 @@ static uint8_t auto_disarming_counter;
static void arm_motors_check()
{
static int16_t arming_counter;
bool allow_arming = false;
// ensure throttle is down
if (g.rc_3.control_in > 0) {
@ -20,30 +19,6 @@ static void arm_motors_check()
return;
}
// allow arming/disarming in fully manual flight modes ACRO, STABILIZE, SPORT and DRIFT
if (manual_flight_mode(control_mode)) {
allow_arming = true;
}
// allow arming/disarming in Loiter and AltHold if landed
if (ap.land_complete && (control_mode == LOITER || control_mode == ALT_HOLD || control_mode == POSHOLD || control_mode == AUTOTUNE)) {
allow_arming = true;
}
// kick out other flight modes
if (!allow_arming) {
arming_counter = 0;
return;
}
#if FRAME_CONFIG == HELI_FRAME
// heli specific arming check
if (!motors.allow_arming()){
arming_counter = 0;
return;
}
#endif // HELI_FRAME
int16_t tmp = g.rc_4.control_in;
// full right
@ -58,7 +33,7 @@ static void arm_motors_check()
if (arming_counter == ARM_DELAY && !motors.armed()) {
// run pre-arm-checks and display failures
pre_arm_checks(true);
if(ap.pre_arm_check && arm_checks(true)) {
if(ap.pre_arm_check && arm_checks(true,false)) {
if (!init_arm_motors()) {
// reset arming counter if arming fail
arming_counter = 0;
@ -80,6 +55,10 @@ static void arm_motors_check()
// full left
}else if (tmp < -4000) {
if (!manual_flight_mode(control_mode) && !ap.land_complete) {
arming_counter = 0;
return;
}
// increase the counter to a maximum of 1 beyond the disarm delay
if( arming_counter <= DISARM_DELAY ) {
@ -549,8 +528,27 @@ static bool pre_arm_gps_checks(bool display_failure)
// arm_checks - perform final checks before arming
// always called just before arming. Return true if ok to arm
static bool arm_checks(bool display_failure)
static bool arm_checks(bool display_failure, bool request_from_gcs)
{
// always check if the current mode allows arming
if (!mode_allows_arming(control_mode) || (!request_from_gcs && control_mode == GUIDED)) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Mode not armable"));
}
return false;
}
// always check if rotor is spinning on heli
#if FRAME_CONFIG == HELI_FRAME
// heli specific arming check
if (!motors.allow_arming()){
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Rotor not spinning"));
}
return false;
}
#endif // HELI_FRAME
// succeed if arming checks are disabled
if (g.arming_check == ARMING_CHECK_NONE) {
return true;