1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
|
#include "pid.h"
#include <px4/attitude_estimator_bm/matrix.h> //TODO: move matrix.h to somewhere else?
void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
uint8_t mode, uint8_t plot_i)
{
pid->kp = kp;
pid->ki = ki;
pid->kd = kd;
pid->intmax = intmax;
pid->mode = mode;
pid->plot_i = plot_i;
pid->count = 0;
pid->saturated = 0;
pid->sp = 0;
pid->error_previous = 0;
pid->integral = 0;
}
void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax)
{
pid->kp = kp;
pid->ki = ki;
pid->kd = kd;
pid->intmax = intmax;
// pid->mode = mode;
// pid->sp = 0;
// pid->error_previous = 0;
// pid->integral = 0;
}
//void pid_set(PID_t *pid, float sp)
//{
// pid->sp = sp;
// pid->error_previous = 0;
// pid->integral = 0;
//}
/**
*
* @param pid
* @param val
* @param dt
* @return
*/
float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
{
/* error = setpoint - actual_position
integral = integral + (error*dt)
derivative = (error - previous_error)/dt
output = (Kp*error) + (Ki*integral) + (Kd*derivative)
previous_error = error
wait(dt)
goto start
*/
float i, d;
pid->sp = sp;
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);
}
// Anti-Windup. Needed if we don't use the saturation above.
if (pid->intmax != 0.0) {
if (i > pid->intmax) {
pid->integral = pid->intmax;
} else if (i < -pid->intmax) {
pid->integral = -pid->intmax;
} else {
pid->integral = i;
}
//Send Controller integrals
// Disabled because of new possibilities with debug_vect.
// Now sent in Main Loop at 5 Hz. 26.06.2010 Laurens
// if (pid->plot_i && (pid->count++ % 16 == 0)&&(global_data.param[PARAM_SEND_SLOT_DEBUG_2] == 1))
// {
// mavlink_msg_debug_send(MAVLINK_COMM_1, pid->plot_i, pid->integral);
// }
}
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;
}
pid->error_previous = error;
return (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
}
|
