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
122
|
#include "thread.h"
#include "usbserial.h"
#include <ch.h>
#include <hal.h>
#include <pwm.h>
class LEDThread : public BaseThread<LEDThread, 128> {
public:
noreturn_t thread_main() {
while (1) {
palClearPad(GPIOA, 5);
chThdSleepMilliseconds(500);
palSetPad(GPIOA, 5);
chThdSleepMilliseconds(500);
}
}
};
LEDThread led_thread;
class USBThread : public BaseThread<USBThread, 256> {
private:
typedef enum {W_S, W_N, W_V} w_s_t;
public:
USBSerial* usbs;
int8_t thrust, pitch, yaw, roll;
noreturn_t thread_main() {
thrust = pitch = roll = yaw = 0;
w_s_t w_s = W_S;
uint8_t w_n = 0;
while (TRUE) {
palSetPad(GPIOA, 5);
size_t buffer = usbs->getc();
if(buffer >= 0 && buffer < 256) {
usbs->putc(buffer);
if(w_s == W_S && buffer == 'S') {
w_s = W_N;
} else if(w_s == W_N && buffer >= '1' && buffer <= '4') {
w_s = W_V;
w_n = buffer - '1';
} else if(w_s == W_V) {
w_s = W_S;
switch(w_n) {
case 0:
thrust = buffer;
break;
case 1:
pitch = buffer - 64;
break;
case 2:
roll = buffer - 64;
break;
case 3:
yaw = buffer - 64;
break;
}
} else {
w_s = W_S;
}
}
}
}
};
USBThread usb_thread;
void foo(PWMDriver*) {
pwmEnableChannel(&PWMD2, 0, 1000 + usb_thread.thrust * 10 + usb_thread.pitch * 3 + usb_thread.roll * 3 - usb_thread.yaw * 3);
pwmEnableChannel(&PWMD2, 1, 1000 + usb_thread.thrust * 10 + usb_thread.pitch * 3 - usb_thread.roll * 3 + usb_thread.yaw * 3);
pwmEnableChannel(&PWMD2, 2, 1000 + usb_thread.thrust * 10 - usb_thread.pitch * 3 + usb_thread.roll * 3 + usb_thread.yaw * 3);
pwmEnableChannel(&PWMD2, 3, 1000 + usb_thread.thrust * 10 - usb_thread.pitch * 3 - usb_thread.roll * 3 - usb_thread.yaw * 3);
}
static PWMConfig pwmcfg = {
foo,
{
{PWM_OUTPUT_ACTIVE_HIGH, NULL},
{PWM_OUTPUT_ACTIVE_HIGH, NULL},
{PWM_OUTPUT_ACTIVE_HIGH, NULL},
{PWM_OUTPUT_ACTIVE_HIGH, NULL}
},
PWM_COMPUTE_PSC(STM32_TIMCLK1, 1000000),
20000,
0
};
USBSerial usbs;
int main(void) {
halInit();
chSysInit();
pwmStart(&PWMD2, &pwmcfg);
palSetPadMode(GPIOA, 0, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
palSetPadMode(GPIOA, 1, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
palSetPadMode(GPIOA, 2, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
palSetPadMode(GPIOA, 3, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
pwmEnableChannel(&PWMD2, 0, 1000);
pwmEnableChannel(&PWMD2, 1, 1000);
pwmEnableChannel(&PWMD2, 2, 1000);
pwmEnableChannel(&PWMD2, 3, 1000);
led_thread.start();
usbs.init();
usb_thread.usbs = &usbs;
usb_thread.start();
while (1) {
chThdSleepMilliseconds(1000);
}
}
|
