Rover: fix use-pivot-steering

absolute yaw error should be used
This commit is contained in:
Randy Mackay 2017-10-25 15:41:47 +09:00
parent fb944322c7
commit fb9f695d2f

View File

@ -12,7 +12,7 @@ bool Rover::use_pivot_steering(float yaw_error_cd)
}
// calc bearing error
const float yaw_error = yaw_error_cd / 100.0f;
const float yaw_error = fabsf(yaw_error_cd) * 0.01f;
// if error is larger than pivot_turn_angle start pivot steering
if (yaw_error > g.pivot_turn_angle) {