53 #include <px4_platform_common/defines.h> 55 #define SIGMA 0.000001f 75 if (PX4_ISFINITE(kp)) {
82 if (PX4_ISFINITE(ki)) {
89 if (PX4_ISFINITE(kd)) {
96 if (PX4_ISFINITE(integral_limit)) {
103 if (PX4_ISFINITE(output_limit)) {
115 if (!PX4_ISFINITE(sp) || !PX4_ISFINITE(val) || !PX4_ISFINITE(val_dot) || !PX4_ISFINITE(dt)) {
122 float error = sp - val;
140 if (!PX4_ISFINITE(d)) {
145 float output = (error * pid->
kp) + (d * pid->
kd);
152 if (PX4_ISFINITE(i)) {
165 if (PX4_ISFINITE(output)) {
170 }
else if (output < -pid->output_limit) {
__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min)
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
__BEGIN_DECLS enum PID_MODE pid_mode_t
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
__EXPORT void pid_reset_integral(PID_t *pid)
Definition of generic PID controller.