diff options
Diffstat (limited to 'main.cpp')
| -rw-r--r-- | main.cpp | 122 |
1 files changed, 122 insertions, 0 deletions
diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..d548ab5 --- /dev/null +++ b/main.cpp @@ -0,0 +1,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);
+ }
+}
|