Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM_Control: implement single-cycle feed-forward scaler for roll and pitch

Browse Source
This commit is contained in:
Andy Piper 2023-07-03 19:50:53 +01:00 committed by Andrew Tridgell
parent df911e9e76
commit ab24f97275
4 changed files with 14 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/APM_Control/AP_PitchController.cpp
View File

@ -177,7 +177,8 @@ float AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool d
// FF should be scaled by scaler/eas2tas, but since we have scaled
// the AC_PID target above by scaler*scaler we need to instead
// divide by scaler*eas2tas to get the right scaling
const float ff = degrees(rate_pid.get_ff() / (scaler * eas2tas));
const float ff = degrees(ff_scale * rate_pid.get_ff() / (scaler * eas2tas));
ff_scale = 1.0;
if (disable_integrator) {
rate_pid.reset_I();

5
libraries/APM_Control/AP_PitchController.h
View File

@ -16,6 +16,10 @@ public:
float get_rate_out(float desired_rate, float scaler);
float get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode);
// setup a one loop FF scale multiplier. This replaces any previous scale applied
// so should only be used when only one source of scaling is needed
void set_ff_scale(float _ff_scale) { ff_scale = _ff_scale; }
void reset_I();
/*
@ -56,6 +60,7 @@ private:
float _last_out;
AC_PID rate_pid{0.04, 0.15, 0, 0.345, 0.666, 3, 0, 12, 150, 1};
float angle_err_deg;
float ff_scale = 1.0;
AP_PIDInfo _pid_info;

3
libraries/APM_Control/AP_RollController.cpp
View File

@ -165,7 +165,8 @@ float AP_RollController::_get_rate_out(float desired_rate, float scaler, bool di
// FF should be scaled by scaler/eas2tas, but since we have scaled
// the AC_PID target above by scaler*scaler we need to instead
// divide by scaler*eas2tas to get the right scaling
const float ff = degrees(rate_pid.get_ff() / (scaler * eas2tas));
const float ff = degrees(ff_scale * rate_pid.get_ff() / (scaler * eas2tas));
ff_scale = 1.0;
if (disable_integrator) {
rate_pid.reset_I();

5
libraries/APM_Control/AP_RollController.h
View File

@ -16,6 +16,10 @@ public:
float get_rate_out(float desired_rate, float scaler);
float get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode);
// setup a one loop FF scale multiplier. This replaces any previous scale applied
// so should only be used when only one source of scaling is needed
void set_ff_scale(float _ff_scale) { ff_scale = _ff_scale; }
void reset_I();
/*
@ -56,6 +60,7 @@ private:
float _last_out;
AC_PID rate_pid{0.08, 0.15, 0, 0.345, 0.666, 3, 0, 12, 150, 1};
float angle_err_deg;
float ff_scale = 1.0;
AP_PIDInfo _pid_info;