mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Rover: guided heading-and-speed control slows using yaw error
This commit is contained in:
parent
5dc4b8e66d
commit
82aba6fe56
@ -47,7 +47,7 @@ void ModeGuided::update()
|
|||||||
if (have_attitude_target) {
|
if (have_attitude_target) {
|
||||||
// run steering and throttle controllers
|
// run steering and throttle controllers
|
||||||
calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
|
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 {
|
} else {
|
||||||
stop_vehicle();
|
stop_vehicle();
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user