blob: 6bd8f6e7da7ba01c5c8a8774b267bf20a21c35d9 (
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
|
#include "stm32.h"
#include "interrupt.h"
#include "ppmsum.h"
#include "i2c.h"
#include "itg3200.h"
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 = 20000;
TIM2.CCER = 0x1111;
TIM2.CCMR1 = 0x6868;
TIM2.CCMR2 = 0x6868;
TIM2.CR1 = 0x05;
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 = 0;
int16_t yaw = gyro.z >> 7;
int16_t roll = 0;
TIM2.CCR1 = 1000 + throttle + pitch + roll - yaw;
TIM2.CCR2 = 1000 + throttle + pitch - roll + yaw;
TIM2.CCR3 = 1000 + throttle - pitch + roll + yaw;
TIM2.CCR4 = 1000 + throttle - pitch - roll - yaw;
}
}
|