Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM_Control: added FF parameters to roll and pitch controllers

Browse Source 
these are much easier to tune with the new PID_TUNING messages
This commit is contained in:
Andrew Tridgell 2015-05-23 13:26:17 +10:00
parent 353879cd2b
commit 86a3bca88c
3 changed files with 30 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/APM_Control/AP_AutoTune.h
View File

@ -14,6 +14,7 @@ public:
AP_Float P;
AP_Float I;
AP_Float D;
AP_Float FF;
AP_Int16 rmax;
AP_Int16 imax;
};

17
libraries/APM_Control/AP_PitchController.cpp
View File

@ -93,6 +93,14 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] PROGMEM = {
// @User: Advanced
AP_GROUPINFO("IMAX", 7, AP_PitchController, gains.imax, 3000),
// @Param: FF
// @DisplayName: Feed forward Gain
// @Description: This is the gain from demanded rate to elevator output.
// @Range: 0.1 4.0
// @Increment: 0.1
// @User: User
AP_GROUPINFO("FF", 8, AP_PitchController, gains.FF, 0.0f),
AP_GROUPEND
};
@ -155,15 +163,18 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
// 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
float kp_ff = max((gains.P - gains.I * gains.tau) * gains.tau - gains.D , 0) / _ahrs.get_EAS2TAS();
float eas2tas = _ahrs.get_EAS2TAS();
float kp_ff = max((gains.P - gains.I * gains.tau) * gains.tau - gains.D , 0) / eas2tas;
float k_ff = gains.FF / eas2tas;
// Calculate the demanded control surface deflection
// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
// path, but want a 1/speed^2 scaler applied to the rate error path.
// This is because acceleration scales with speed^2, but rate scales with speed.
_pid_info.FF = desired_rate * kp_ff * scaler;
_pid_info.P = desired_rate * kp_ff * scaler;
_pid_info.FF = desired_rate * k_ff * scaler;
_pid_info.D = rate_error * gains.D * scaler;
_last_out = _pid_info.D + _pid_info.FF;
_last_out = _pid_info.D + _pid_info.FF + _pid_info.P;
_pid_info.desired = desired_rate;
if (autotune.running && aspeed > aparm.airspeed_min) {

19
libraries/APM_Control/AP_RollController.cpp
View File

@ -36,7 +36,7 @@ const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = {
// @Param: P
// @DisplayName: Proportional Gain
// @Description: This is the gain from bank angle to aileron. This gain works the same way as the P term in the old PID (RLL2SRV_P) and can be set to the same value.
// @Description: This is the gain from bank angle error to aileron.
// @Range: 0.1 4.0
// @Increment: 0.1
// @User: User
@ -75,6 +75,14 @@ const AP_Param::GroupInfo AP_RollController::var_info[] PROGMEM = {
// @User: Advanced
AP_GROUPINFO("IMAX", 5, AP_RollController, gains.imax, 3000),
// @Param: FF
// @DisplayName: Feed forward Gain
// @Description: This is the gain from demanded rate to aileron output.
// @Range: 0.1 4.0
// @Increment: 0.1
// @User: User
AP_GROUPINFO("FF", 6, AP_RollController, gains.FF, 0.0f),
AP_GROUPEND
};
@ -95,7 +103,9 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
// 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
float ki_rate = gains.I * gains.tau;
float kp_ff = max((gains.P - gains.I * gains.tau) * gains.tau - gains.D , 0)/_ahrs.get_EAS2TAS();
float eas2tas = _ahrs.get_EAS2TAS();
float kp_ff = max((gains.P - gains.I * gains.tau) * gains.tau - gains.D , 0) / eas2tas;
float k_ff = gains.FF / eas2tas;
float delta_time = (float)dt * 0.001f;
// Limit the demanded roll rate
@ -150,10 +160,11 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
// path, but want a 1/speed^2 scaler applied to the rate error path.
// This is because acceleration scales with speed^2, but rate scales with speed.
_pid_info.D = rate_error * gains.D * scaler;
_pid_info.FF = desired_rate * kp_ff * scaler;
_pid_info.P = desired_rate * kp_ff * scaler;
_pid_info.FF = desired_rate * k_ff * scaler;
_pid_info.desired = desired_rate;
_last_out = _pid_info.FF + _pid_info.D;
_last_out = _pid_info.FF + _pid_info.P + _pid_info.D;
if (autotune.running && aspeed > aparm.airspeed_min) {
// let autotune have a go at the values