Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-29 02:38:07 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[fix] add some ref and set_integral function to PID lib

Browse Source
This commit is contained in:
Gautier Hattenberger
2019-07-08 16:05:09 +02:00
parent c8dba29208
commit a7b05cf296
3 changed files with 25 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/filters/pid.h
+21
View File
@@ -117,6 +117,27 @@ static inline void set_gains_pid_f(struct PID_f *pid, float Kp, float Kd, float
pid->g[2] = Ki;
}
/** Set integral part, can be used to reset.
* The new sum of errors is calculated from current gains and bounds.
*
* @param pid pointer to PID structure
* @param value integral part of the PID control, 0. will reset it
*/
static inline void set_integral_pid_f(struct PID_f *pid, float value)
{
float integral = value;
if (integral < -pid->max_sum) {
integral = -pid->max_sum;
} else if (integral > pid->max_sum) {
integral = pid->max_sum;
}
if (fabsf(pid->g[2]) < 1e-6) {
pid->sum = 0.f; // integral gain is too low, prevent division by zero, just reset sum
} else {
pid->sum = integral / pid->g[2];
}
}
/** Distcrete time PID structure.