From b1a4cd55fdeeb8d275957d8b5130afc62dbfaa2b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 8 Sep 2018 12:36:06 +0900 Subject: [PATCH] Rover: support disabling rudder arming --- APMrover2/radio.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/APMrover2/radio.cpp b/APMrover2/radio.cpp index 562eeedd27..e10d4e6fb3 100644 --- a/APMrover2/radio.cpp +++ b/APMrover2/radio.cpp @@ -47,6 +47,12 @@ void Rover::init_rc_out() */ void Rover::rudder_arm_disarm_check() { + // check if arming/disarm using rudder is allowed + AP_Arming::ArmingRudder arming_rudder = arming.rudder_arming(); + if (arming_rudder == AP_Arming::ARMING_RUDDER_DISABLED) { + return; + } + // In Rover we need to check that its set to the throttle trim and within the DZ // if throttle is not within trim dz, then pilot cannot rudder arm/disarm if (!channel_throttle->in_trim_dz()) { @@ -79,7 +85,7 @@ void Rover::rudder_arm_disarm_check() // not at full right rudder rudder_arm_timer = 0; } - } else if (!g2.motors.active()) { + } else if ((arming_rudder == AP_Arming::ARMING_RUDDER_ARMDISARM) && !g2.motors.active()) { // when armed and motor not active (not moving), full left rudder starts disarming counter if (channel_steer->get_control_in() < -4000) { const uint32_t now = millis();