Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PID: allow for slew limit scaler

Browse Source 
used by plane to have slew limiter in deg/s
This commit is contained in:
Andrew Tridgell 2021-04-01 11:12:11 +11:00
parent 62afa46924
commit 6f2e592943
2 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_PID/AC_PID.cpp
View File

@ -93,6 +93,10 @@ AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_
_flags._reset_filter = true; _flags._reset_filter = true;
memset(&_pid_info, 0, sizeof(_pid_info)); memset(&_pid_info, 0, sizeof(_pid_info));
// slew limit scaler allows for plane to use degrees/sec slew
// limit
_slew_limit_scale = 1;
} }
// set_dt - set time step in seconds // set_dt - set time step in seconds
@ -156,7 +160,7 @@ float AC_PID::update_all(float target, float measurement, bool limit)
float D_out = (_derivative * _kd); float D_out = (_derivative * _kd);
// calculate slew limit modifier for P+D // calculate slew limit modifier for P+D
_pid_info.Dmod = _slew_limiter.modifier(_pid_info.P + _pid_info.D, _dt); _pid_info.Dmod = _slew_limiter.modifier((_pid_info.P + _pid_info.D) * _slew_limit_scale, _dt);
P_out *= _pid_info.Dmod; P_out *= _pid_info.Dmod;
D_out *= _pid_info.Dmod; D_out *= _pid_info.Dmod;
@ -208,7 +212,7 @@ float AC_PID::update_error(float error, bool limit)
float D_out = (_derivative * _kd); float D_out = (_derivative * _kd);
// calculate slew limit modifier for P+D // calculate slew limit modifier for P+D
_pid_info.Dmod = _slew_limiter.modifier(_pid_info.P + _pid_info.D, _dt); _pid_info.Dmod = _slew_limiter.modifier((_pid_info.P + _pid_info.D) * _slew_limit_scale, _dt);
P_out *= _pid_info.Dmod; P_out *= _pid_info.Dmod;
D_out *= _pid_info.Dmod; D_out *= _pid_info.Dmod;

4
libraries/AC_PID/AC_PID.h
View File

@ -107,6 +107,9 @@ public:
void set_integrator(float error, float i); void set_integrator(float error, float i);
void set_integrator(float i); void set_integrator(float i);
// set slew limiter scale factor
void set_slew_limit_scale(int8_t scale) { _slew_limit_scale = scale; }
const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; } const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }
// parameter var table // parameter var table
@ -144,6 +147,7 @@ protected:
float _target; // target value to enable filtering float _target; // target value to enable filtering
float _error; // error value to enable filtering float _error; // error value to enable filtering
float _derivative; // derivative value to enable filtering float _derivative; // derivative value to enable filtering
int8_t _slew_limit_scale;
uint16_t _reset_counter; // loop counter for reset decay uint16_t _reset_counter; // loop counter for reset decay
uint64_t _reset_last_update; //time in microseconds of last update to reset_I uint64_t _reset_last_update; //time in microseconds of last update to reset_I