From 653714ce29ed33bfc386d3d9478460149770b137 Mon Sep 17 00:00:00 2001 From: Randy Mackay <rmackay9@yahoo.com> Date: Mon, 3 Aug 2020 14:25:39 +0900 Subject: [PATCH] Rover: manual mode avoids saturation on skid-steer vehicles --- Rover/mode.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/Rover/mode.cpp b/Rover/mode.cpp index 510cc30baf..5ad962eea4 100644 --- a/Rover/mode.cpp +++ b/Rover/mode.cpp @@ -105,6 +105,18 @@ void Mode::get_pilot_desired_steering_and_throttle(float &steering_out, float &t // do basic conversion get_pilot_input(steering_out, throttle_out); + // for skid steering vehicles, if pilot commands would lead to saturation + // we proportionally reduce steering and throttle + if (g2.motors.have_skid_steering()) { + const float steer_normalised = constrain_float(steering_out / 4500.0f, -1.0f, 1.0f); + const float throttle_normalised = constrain_float(throttle_out / 100.0f, -1.0f, 1.0f); + const float saturation_value = fabsf(steer_normalised) + fabsf(throttle_normalised); + if (saturation_value > 1.0f) { + steering_out /= saturation_value; + throttle_out /= saturation_value; + } + } + // check for special case of input and output throttle being in opposite directions float throttle_out_limited = g2.motors.get_slew_limited_throttle(throttle_out, rover.G_Dt); if ((is_negative(throttle_out) != is_negative(throttle_out_limited)) &&