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

APM_Control: added rate and angle steering controllers

Browse Source
This commit is contained in:
Andrew Tridgell 2013-10-03 21:54:11 +10:00
parent 03aca1bd8d
commit bd848a6a7f
2 changed files with 54 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/APM_Control/AP_SteerController.cpp
View File

@ -80,10 +80,10 @@ const AP_Param::GroupInfo AP_SteerController::var_info[] PROGMEM = {
/*
internal rate controller, called by attitude and rate controller
public functions
steering rate controller. Returns servo out -4500 to 4500 given
desired yaw rate in degrees/sec. Positive yaw rate means clockwise yaw.
*/
int32_t AP_SteerController::get_steering_out(float desired_accel)
int32_t AP_SteerController::get_steering_out_rate(float desired_rate)
{
uint32_t tnow = hal.scheduler->millis();
uint32_t dt = tnow - _last_t;
@ -103,8 +103,7 @@ int32_t AP_SteerController::get_steering_out(float desired_accel)
float scaler = 1.0f / speed;
// Calculate the steering rate error (deg/sec) and apply gain scaler
float desired_rate = desired_accel / speed;
float rate_error = (ToDeg(desired_rate) - ToDeg(_ahrs.get_gyro().z)) * scaler;
float rate_error = (desired_rate - ToDeg(_ahrs.get_gyro().z)) * 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
@ -138,12 +137,46 @@ int32_t AP_SteerController::get_steering_out(float desired_accel)
_integrator = constrain_float(_integrator, -intLimScaled, intLimScaled);
// Calculate the demanded control surface deflection
_last_out = (rate_error * _K_D * 4.0f) + (desired_rate * kp_ff) * scaler + _integrator;
_last_out = (rate_error * _K_D * 4.0f) + (ToRad(desired_rate) * kp_ff) * scaler + _integrator;
// Convert to centi-degrees and constrain
return constrain_float(_last_out * 100, -4500, 4500);
}
/*
lateral acceleration controller. Returns servo value -4500 to 4500
given a desired lateral acceleration
*/
int32_t AP_SteerController::get_steering_out_lat_accel(float desired_accel)
{
float speed = _ahrs.groundspeed();
if (speed < _minspeed) {
// assume a minimum speed. This reduces osciallations when first starting to move
speed = _minspeed;
}
// Calculate the desired steering rate given desired_accel and speed
float desired_rate = ToDeg(desired_accel / speed);
return get_steering_out_rate(desired_rate);
}
/*
return a steering servo value from -4500 to 4500 given an angular
steering error in centidegrees.
*/
int32_t AP_SteerController::get_steering_out_angle_error(int32_t angle_err)
{
if (_tau < 0.1) {
_tau = 0.1;
}
// Calculate the desired steering rate (deg/sec) from the angle error
float desired_rate = angle_err * 0.01f / _tau;
return get_steering_out_rate(desired_rate);
}
void AP_SteerController::reset_I()
{
_integrator = 0;

17
libraries/APM_Control/AP_SteerController.h
View File

@ -18,9 +18,22 @@ public:
/*
return a steering servo output from -4500 to 4500 given a
desired lateral acceleration rate in m/s/s.
desired lateral acceleration rate in m/s/s. Positive lateral
acceleration is to the right.
*/
int32_t get_steering_out(float desired_accel);
int32_t get_steering_out_lat_accel(float desired_accel);
/*
return a steering servo output from -4500 to 4500 given a
desired yaw rate in degrees/sec. Positive yaw is to the right.
*/
int32_t get_steering_out_rate(float desired_rate);
/*
return a steering servo output from -4500 to 4500 given a
yaw error in centi-degrees
*/
int32_t get_steering_out_angle_error(int32_t angle_err);
/*
return the steering radius (half diameter). Assumed to be half