From 14d9e932ab5330df0c401ea54364a1d3f04a3da5 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 22 Nov 2017 17:39:16 +0900 Subject: [PATCH] Rover: allow disarming from transmitter on skid-steering vehicles --- APMrover2/control_modes.cpp | 19 ++++++++++++++----- APMrover2/radio.cpp | 4 +--- 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/APMrover2/control_modes.cpp b/APMrover2/control_modes.cpp index 8ec5ae9988..bc2a27fa6d 100644 --- a/APMrover2/control_modes.cpp +++ b/APMrover2/control_modes.cpp @@ -186,13 +186,22 @@ void Rover::read_aux_switch() } } +// return true if motors are moving bool Rover::motor_active() { - // Check if armed and output throttle servo is not neutral - if (hal.util->get_soft_armed()) { - if (!is_zero(g2.motors.get_throttle())) { - return true; - } + // if soft disarmed, motors not active + if (!hal.util->get_soft_armed()) { + return false; + } + + // check throttle is active + if (!is_zero(g2.motors.get_throttle())) { + return true; + } + + // skid-steering vehicles active when steering + if (g2.motors.have_skid_steering() && !is_zero(g2.motors.get_steering())) { + return true; } return false; diff --git a/APMrover2/radio.cpp b/APMrover2/radio.cpp index d418ebc6e7..58394b25b7 100644 --- a/APMrover2/radio.cpp +++ b/APMrover2/radio.cpp @@ -77,10 +77,8 @@ void Rover::rudder_arm_disarm_check() // not at full right rudder rudder_arm_timer = 0; } - } else if (!motor_active() && !g2.motors.have_skid_steering()) { + } else if (!motor_active()) { // when armed and motor not active (not moving), full left rudder starts disarming counter - // This is disabled for skid steering otherwise when tring to turn a skid steering rover around - // the rover would disarm if (channel_steer->get_control_in() < -4000) { const uint32_t now = millis();