Rover: guided heading-and-speed control slows using yaw error

This commit is contained in:
Randy Mackay 2018-07-07 16:38:16 +09:00
parent 5dc4b8e66d
commit 82aba6fe56

View File

@ -47,7 +47,7 @@ void ModeGuided::update()
if (have_attitude_target) {
// run steering and throttle controllers
calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
calc_throttle(_desired_speed, true, true);
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, true);
} else {
stop_vehicle();
}