From d56e2b6a3926ac21c0579de047b4fb5ec6b0cdad Mon Sep 17 00:00:00 2001 From: Grant Morphett Date: Wed, 1 Jun 2016 09:54:59 +1000 Subject: [PATCH] APM_Control: Get steer rate using earth frame. Rather then just using the standard z gyro by using the earth frame it takes into account when a rover leans over in hard corners. My rover leans 15 degrees no problem which is why this is needed. --- libraries/APM_Control/AP_SteerController.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/APM_Control/AP_SteerController.cpp b/libraries/APM_Control/AP_SteerController.cpp index 7ff1cca332..8507d9ec89 100644 --- a/libraries/APM_Control/AP_SteerController.cpp +++ b/libraries/APM_Control/AP_SteerController.cpp @@ -114,7 +114,8 @@ int32_t AP_SteerController::get_steering_out_rate(float desired_rate) _pid_info.desired = desired_rate; // Calculate the steering rate error (deg/sec) and apply gain scaler - float rate_error = (desired_rate - ToDeg(_ahrs.get_gyro().z)) * scaler; + // We do this in earth frame to allow for rover leaning over in hard corners + float rate_error = (desired_rate - ToDeg(_ahrs.get_yaw_rate_earth())) * scaler; // Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law // No conversion is required for K_D