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
110
111
112
113
114
115
116
117
118
119
120
121
|
#include "stm32.h"
#include "interrupt.h"
#include "ppmsum.h"
#include "i2c.h"
#include "itg3200.h"
template<class T>
inline void saturate(T& var, T absmax) {
if(var > absmax) {
var = absmax;
} else if(var < -absmax) {
var = -absmax;
}
}
template<class T>
inline T limit(T var, T min, T max) {
if(var < min) {
return min;
} else if(var > max) {
return max;
} else {
return var;
}
}
class PID {
private:
uint16_t Kp, Ki, Kd;
int16_t last;
int32_t accum;
public:
PID(uint16_t p, uint16_t i, uint16_t d) : Kp(p), Ki(i), Kd(d), last(0), accum(0) {}
int16_t update(int16_t error) {
// P
int32_t corr_p = Kp * error;
// I
accum += Ki * error;
int32_t corr_i = accum;
// D
int32_t corr_d = Kd * (error - last);
last = error;
return (corr_p + corr_i + corr_d) >> 16;
}
};
int main() {
RCC.enable(RCC.AFIO);
RCC.enable(RCC.IOPA);
RCC.enable(RCC.IOPB);
GPIOA.CRL = 0x4434bbbb;
GPIOA.CRH = 0x444444b4;
GPIOA.ODR = 1 << 5;
GPIOB.CRH = 0x4444ff44;
I2C i2c;
i2c.enable();
ITG3200 gyro(i2c);
gyro.init();
PPMSum ppmsum;
ppmsum.enable();
RCC.enable(RCC.TIM2);
TIM2.PSC = 72;
TIM2.ARR = 5000;
TIM2.CCER = 0x1111;
TIM2.CCMR1 = 0x6868;
TIM2.CCMR2 = 0x6868;
TIM2.CR1 = 0x05;
PID pid_pitch(3000, 0, 0);
PID pid_roll(3000, 0, 0);
PID pid_yaw(2000, 0, 0);
while(1) {
// Wait for a new update.
while(!(TIM2.SR & 0x01));
TIM2.SR = 0;
// Read sensors.
gyro.update();
// Update filter.
// Generate motor mix.
int16_t throttle = ppmsum.channels[2] - 1000;
int16_t pitch = pid_pitch.update((ppmsum.channels[1] - 1500) * 3 - gyro.x);
int16_t roll = pid_roll.update((ppmsum.channels[0] - 1500) * 3 - gyro.y);
int16_t yaw = pid_yaw.update((ppmsum.channels[3] - 1500) * 5 - gyro.z);
int16_t max = throttle > 250 ? 250 : throttle;
saturate(pitch, max);
saturate(roll, max);
saturate(yaw, max);
int cmds[] = {
1000 + throttle + pitch + roll + yaw,
1000 + throttle + pitch - roll - yaw,
1000 + throttle - pitch + roll - yaw,
1000 + throttle - pitch - roll + yaw,
};
TIM2.CCR1 = limit(cmds[0], 1000, 2000);
TIM2.CCR2 = limit(cmds[1], 1000, 2000);
TIM2.CCR3 = limit(cmds[2], 1000, 2000);
TIM2.CCR4 = limit(cmds[3], 1000, 2000);
}
}
|
