aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_SteerController: Adding support for Rovers driving reverse

Browse Source 
This change is mostly for handling the reverse yaw.
This commit is contained in:
Grant Morphett 2016-07-14 17:49:13 +10:00 committed by Andrew Tridgell
parent b56799f444
commit 8d6b417adb
2 changed files with 14 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/APM_Control/AP_SteerController.cpp
View File

@ -115,7 +115,11 @@ int32_t AP_SteerController::get_steering_out_rate(float desired_rate)
// Calculate the steering rate error (deg/sec) and apply gain scaler // Calculate the steering rate error (deg/sec) and apply gain scaler
// We do this in earth frame to allow for rover leaning over in hard corners // 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; float yaw_rate_earth = ToDeg(_ahrs.get_yaw_rate_earth());
if (_reverse) {
yaw_rate_earth = wrap_180(180 + yaw_rate_earth);
}
float rate_error = (desired_rate - 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 // 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 // No conversion is required for K_D
@ -175,6 +179,9 @@ int32_t AP_SteerController::get_steering_out_lat_accel(float desired_accel)
// Calculate the desired steering rate given desired_accel and speed // Calculate the desired steering rate given desired_accel and speed
float desired_rate = ToDeg(desired_accel / speed); float desired_rate = ToDeg(desired_accel / speed);
if (_reverse) {
desired_rate *= -1;
}
return get_steering_out_rate(desired_rate); return get_steering_out_rate(desired_rate);
} }

6
libraries/APM_Control/AP_SteerController.h
View File

@ -45,6 +45,10 @@ public:
const DataFlash_Class::PID_Info& get_pid_info(void) const { return _pid_info; } const DataFlash_Class::PID_Info& get_pid_info(void) const { return _pid_info; }
void set_reverse(bool reverse) {
_reverse = reverse;
}
private: private:
AP_Float _tau; AP_Float _tau;
AP_Float _K_FF; AP_Float _K_FF;
@ -59,4 +63,6 @@ private:
DataFlash_Class::PID_Info _pid_info {}; DataFlash_Class::PID_Info _pid_info {};
AP_AHRS &_ahrs; AP_AHRS &_ahrs;
bool _reverse;
}; };