mirror of https://github.com/ArduPilot/ardupilot
APM_Control: implement single-cycle feed-forward scaler for roll and pitch
This commit is contained in:
parent
df911e9e76
commit
ab24f97275
|
@ -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();
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
Loading…
Reference in New Issue