Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM_Control: Get steer rate using earth frame.

Browse Source 
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.
This commit is contained in:
Grant Morphett 2016-06-01 09:54:59 +10:00 committed by Randy Mackay
parent 27ae46dfda
commit d56e2b6a39
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/APM_Control/AP_SteerController.cpp
View File

@ -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