blob: 51aa633c6936fef02272b8cc8b1ba962ea33019d (
plain)
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
|
#include "ref.h"
float32_t ref_pid_f32(
arm_pid_instance_f32 * S,
float32_t in)
{
float32_t out;
/* y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2] */
out = S->state[2] + S->A0 * in + S->A1 * S->state[0] + S->A2 * S->state[1];
/* Update state */
S->state[1] = S->state[0];
S->state[0] = in;
S->state[2] = out;
/* return to application */
return (out);
}
q31_t ref_pid_q31(
arm_pid_instance_q31 * S,
q31_t in)
{
q63_t acc;
q31_t out;
/* acc = A0 * x[n] */
acc = (q63_t) S->A0 * in;
/* acc += A1 * x[n-1] */
acc += (q63_t) S->A1 * S->state[0];
/* acc += A2 * x[n-2] */
acc += (q63_t) S->A2 * S->state[1];
/* convert output to 1.31 format to add y[n-1] */
out = (q31_t) (acc >> 31U);
/* out += y[n-1] */
out += S->state[2];
/* Update state */
S->state[1] = S->state[0];
S->state[0] = in;
S->state[2] = out;
/* return to application */
return (out);
}
q15_t ref_pid_q15(
arm_pid_instance_q15 * S,
q15_t in)
{
q63_t acc;
q15_t out;
q15_t A1, A2;
#if defined (ARM_MATH_DSP)
#ifndef ARM_MATH_BIG_ENDIAN
A2 = S->A1 >> 16;
A1 = (q15_t)S->A1;
#else
A1 = S->A1 >> 16;
A2 = (q15_t)S->A1;
#endif
#else
A1 = S->A1;
A2 = S->A2;
#endif
/* acc = A0 * x[n] */
acc = ((q31_t) S->A0) * in;
/* acc += A1 * x[n-1] + A2 * x[n-2] */
acc += (q31_t) A1 * S->state[0];
acc += (q31_t) A2 * S->state[1];
/* acc += y[n-1] */
acc += (q31_t) S->state[2] << 15;
/* saturate the output */
out = ref_sat_q15(acc >> 15);
/* Update state */
S->state[1] = S->state[0];
S->state[0] = in;
S->state[2] = out;
/* return to application */
return (out);
}
|
