From 3b7e84ce7a764ad837c184e071f13b8ca6ba55c1 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 7 Jul 2018 16:04:54 +0900 Subject: [PATCH] Rover: calc_steering_to_heading updates yaw error this allow the vehicle's speed to be reduced based on the heading error --- APMrover2/mode.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index 0845ae314b..94b1b54939 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -430,7 +430,10 @@ void Mode::calc_steering_from_lateral_acceleration(float lat_accel, bool reverse // calculate steering output to drive towards desired heading void Mode::calc_steering_to_heading(float desired_heading_cd, float rate_max, bool reversed) { - // calculate yaw error (in radians) and pass to steering angle controller + // calculate yaw error so it can be used for reporting and slowing the vehicle + _yaw_error_cd = wrap_180_cd(desired_heading_cd - ahrs.yaw_sensor); + + // call heading controller const float steering_out = attitude_control.get_steering_out_heading(radians(desired_heading_cd*0.01f), rate_max, g2.motors.limit.steer_left,