From 3d13d984104f0c62cb95aa47c3e20c33d5111ca8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 21 Jul 2017 13:17:19 +0900 Subject: [PATCH] Rover: steering mode direction fix when reversing this corrects the rotation direction when moving backwards in steering mode so that it's consistent with manual mode --- APMrover2/mode_steering.cpp | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/APMrover2/mode_steering.cpp b/APMrover2/mode_steering.cpp index a6578a7a0e..bbf57c2db6 100644 --- a/APMrover2/mode_steering.cpp +++ b/APMrover2/mode_steering.cpp @@ -3,6 +3,11 @@ void ModeSteering::update() { + // convert pilot throttle input to desired speed + // speed in proportion to cruise speed, up to 50% throttle, then uses nudging above that. + float target_speed = channel_throttle->get_control_in() * 0.01f * 2.0f * g.speed_cruise; + target_speed = constrain_float(target_speed, -g.speed_cruise, g.speed_cruise); + // in steering mode we control lateral acceleration directly. We first calculate the maximum lateral // acceleration at full steering lock for this speed. That is V^2/R where R is the radius of turn. // We get the radius of turn from half the STEER2SRV_P. @@ -15,17 +20,17 @@ void ModeSteering::update() // convert pilot steering input to desired lateral acceleration lateral_acceleration = max_g_force * (channel_steer->get_control_in() / 4500.0f); - // run steering controller - calc_nav_steer(); - - // convert pilot throttle input to desired speed - // speed in proportion to cruise speed, up to 50% throttle, then uses nudging above that. - float target_speed = channel_throttle->get_control_in() * 0.01f * 2.0f * g.speed_cruise; - target_speed = constrain_float(target_speed, -g.speed_cruise, g.speed_cruise); + // reverse target lateral acceleration if backing up + if (is_negative(target_speed)) { + lateral_acceleration = -lateral_acceleration; + } // mark us as in_reverse when using a negative throttle to stop AHRS getting off rover.set_reverse(is_negative(target_speed)); + // run steering controller + calc_nav_steer(); + // run speed to throttle output controller calc_throttle(target_speed); }