Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: manual mode avoids saturation on skid-steer vehicles

This commit is contained in:
Randy Mackay 2020-08-03 14:25:39 +09:00
parent 89a4f83527
commit 653714ce29
1 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Rover/mode.cpp
View File

@ -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)) &&