aboutsummaryrefslogtreecommitdiff
path: root/apps
diff options
context:
space:
mode:
Diffstat (limited to 'apps')
-rw-r--r--apps/multirotor_att_control/multirotor_attitude_control.c16
-rw-r--r--apps/systemlib/pid/pid.c78
-rw-r--r--apps/systemlib/pid/pid.h6
3 files changed, 42 insertions, 58 deletions
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index 2129915d1..cb34209b1 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -212,13 +212,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
parameters_update(&h, &p);
// pid_init(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu,
- // PID_MODE_DERIVATIV_SET, 154);
+ // 90.0f, PID_MODE_DERIVATIV_SET, 154); // Arbitrarily large limit added
pid_init(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu,
- PID_MODE_DERIVATIV_SET);
+ 90.0f, PID_MODE_DERIVATIV_SET); // Arbitrarily large limit added
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
- PID_MODE_DERIVATIV_SET);
+ 90.0f, PID_MODE_DERIVATIV_SET); // Arbitrarily large limit added
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu,
- PID_MODE_DERIVATIV_SET);
+ 90.0f, PID_MODE_DERIVATIV_SET); // Arbitrarily large limit added
initialized = true;
}
@@ -228,10 +228,10 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
/* update parameters from storage */
parameters_update(&h, &p);
/* apply parameters */
- // pid_set_parameters(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu);
- pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu);
- pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu);
- pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu);
+ // pid_set_parameters(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu, 90.0f);
+ pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu, 90.0f);
+ pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, 90.0f);
+ pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, 90.0f);
}
/* calculate current control outputs */
diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c
index cff5e6bbe..f9b50d030 100644
--- a/apps/systemlib/pid/pid.c
+++ b/apps/systemlib/pid/pid.c
@@ -43,12 +43,13 @@
#include <math.h>
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
- uint8_t mode)
+ float limit, uint8_t mode)
{
pid->kp = kp;
pid->ki = ki;
pid->kd = kd;
pid->intmax = intmax;
+ pid->limit = limit;
pid->mode = mode;
pid->count = 0;
pid->saturated = 0;
@@ -58,7 +59,7 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
pid->error_previous = 0;
pid->integral = 0;
}
-__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax)
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit)
{
int ret = 0;
@@ -85,8 +86,13 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
} else {
ret = 1;
}
+
+ if (isfinite(limit)) {
+ pid->limit = limit;
+ } else {
+ ret = 1;
+ }
- // pid->limit = limit;
return ret;
}
@@ -122,72 +128,48 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
float i, d;
pid->sp = sp;
+
+ // Calculated current error value
float error = pid->sp - val;
-
- if (pid->saturated && (pid->integral * error > 0)) {
- //Output is saturated and the integral would get bigger (positive or negative)
- i = pid->integral;
-
- //Reset saturation. If we are still saturated this will be set again at output limit check.
- pid->saturated = 0;
-
- } else {
- i = pid->integral + (error * dt);
+
+ if (isfinite(error)) { // Why is this necessary? DEW
+ pid->error_previous = error;
}
- // Anti-Windup. Needed if we don't use the saturation above.
- if (pid->intmax != 0.0f) {
- if (i > pid->intmax) {
- pid->integral = pid->intmax;
-
- } else if (i < -pid->intmax) {
-
- pid->integral = -pid->intmax;
-
- } else {
- pid->integral = i;
- }
- }
+ // Calculate or measured current error derivative
if (pid->mode == PID_MODE_DERIVATIV_CALC) {
d = (error - pid->error_previous) / dt;
-
} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
d = -val_dot;
-
} else {
d = 0.0f;
}
- if (pid->kd == 0.0f) {
- d = 0.0f;
- }
-
- if (pid->ki == 0.0f) {
- i = 0;
- }
-
- float p;
-
- if (pid->kp == 0.0f) {
- p = 0.0f;
+ // Calculate the error integral and check for saturation
+ i = pid->integral + (error * dt);
+ if( fabs((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit ||
+ fabs(i) > pid->intmax )
+ {
+ i = pid->integral; // If saturated then do not update integral value
+ pid->saturated = 1;
} else {
- p = error;
- }
-
- if (isfinite(error)) {
- pid->error_previous = error;
+ if (!isfinite(i)) {
+ i = 0;
+ }
+ pid->integral = i;
+ pid->saturated = 0;
}
+ // Calculate the output. Limit output magnitude to pid->limit
float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd);
+ if (output > pid->limit) output = pid->limit;
+ if (output < -pid->limit) output = -pid->limit;
if (isfinite(output)) {
pid->last_output = output;
}
- if (!isfinite(pid->integral)) {
- pid->integral = 0;
- }
return pid->last_output;
}
diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h
index 098b2bef8..b51afef9e 100644
--- a/apps/systemlib/pid/pid.h
+++ b/apps/systemlib/pid/pid.h
@@ -49,6 +49,8 @@
#define PID_MODE_DERIVATIV_CALC 0
/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
#define PID_MODE_DERIVATIV_SET 1
+// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID)
+#define PID_MODE_DERIVATIV_NONE 9
typedef struct {
float kp;
@@ -65,8 +67,8 @@ typedef struct {
uint8_t saturated;
} PID_t;
-__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode);
-__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax);
+__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode);
+__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit);
//void pid_set(PID_t *pid, float sp);
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-29 06:05:49 +0000