Hallo! Könnte man die PID ARM Core Funktion der DSP Library um den Windup-Effekt erweitern? Sehe bei dem Quelltext nicht so richtig durch.
1 | /**
|
2 | * @brief Instance structure for the Q31 PID Control.
|
3 | */
|
4 | typedef struct |
5 | {
|
6 | q31_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */ |
7 | q31_t A1; /**< The derived gain, A1 = -Kp - 2Kd. */ |
8 | q31_t A2; /**< The derived gain, A2 = Kd . */ |
9 | q31_t state[3]; /**< The state array of length 3. */ |
10 | q31_t Kp; /**< The proportional gain. */ |
11 | q31_t Ki; /**< The integral gain. */ |
12 | q31_t Kd; /**< The derivative gain. */ |
13 | |
14 | } arm_pid_instance_q31; |
15 | |
16 | void arm_pid_init_q31(arm_pid_instance_q31 * S, int32_t resetStateFlag) |
17 | {
|
18 | /* Derived coefficient A0 */
|
19 | S->A0 = __QADD(__QADD(S->Kp, S->Ki), S->Kd); |
20 | |
21 | /* Derived coefficient A1 */
|
22 | S->A1 = -__QADD(__QADD(S->Kd, S->Kd), S->Kp); |
23 | }
|
24 | |
25 | static __INLINE q31_t arm_pid_q31(arm_pid_instance_q31 * S, q31_t in) |
26 | {
|
27 | q63_t acc; |
28 | q31_t out; |
29 | |
30 | /* acc = A0 * x[n] */
|
31 | acc = (q63_t) S->A0 * in; |
32 | |
33 | /* acc += A1 * x[n-1] */
|
34 | acc += (q63_t) S->A1 * S->state[0]; |
35 | |
36 | /* acc += A2 * x[n-2] */
|
37 | acc += (q63_t) S->A2 * S->state[1]; |
38 | |
39 | /* convert output to 1.31 format to add y[n-1] */
|
40 | out = (q31_t) (acc >> 31u); |
41 | |
42 | /* out += y[n-1] */
|
43 | out += S->state[2]; |
44 | |
45 | /* Update state */
|
46 | S->state[1] = S->state[0]; |
47 | S->state[0] = in; |
48 | S->state[2] = out; |
49 | |
50 | /* return to application */
|
51 | return (out); |
52 | |
53 | }
|
VG Deepdiver99