diff options
| -rw-r--r-- | main.cpp | 78 |
1 files changed, 38 insertions, 40 deletions
@@ -27,10 +27,12 @@ class USBThread : public BaseThread<USBThread, 256> { public:
USBSerial* usbs;
- int8_t thrust, pitch, yaw, roll;
+ uint8_t data[9];
noreturn_t thread_main() {
- thrust = pitch = roll = yaw = 0;
+ for(int i = 0; i < 9; i++) {
+ data[i] = 0;
+ }
w_s_t w_s = W_S;
uint8_t w_n = 0;
@@ -40,25 +42,12 @@ class USBThread : public BaseThread<USBThread, 256> { if(buffer >= 0 && buffer < 256) {
if(w_s == W_S && buffer == 'S') {
w_s = W_N;
- } else if(w_s == W_N && buffer >= '1' && buffer <= '4') {
+ } else if(w_s == W_N && buffer >= '1' && buffer <= '9') {
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;
- }
+ data[w_n] = buffer;
} else {
w_s = W_S;
}
@@ -68,27 +57,6 @@ class USBThread : public BaseThread<USBThread, 256> { };
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 = {
- 1000000,
- 1000000 / 50,
- foo,
- {
- {PWM_OUTPUT_ACTIVE_HIGH, NULL},
- {PWM_OUTPUT_ACTIVE_HIGH, NULL},
- {PWM_OUTPUT_ACTIVE_HIGH, NULL},
- {PWM_OUTPUT_ACTIVE_HIGH, NULL}
- },
- 0
-};
-
USBSerial usbs;
I2CConfig i2cconfig = {
@@ -190,6 +158,38 @@ class I2CThread : public BaseThread<I2CThread, 256> { I2CThread i2c_thread;
+void foo(PWMDriver*) {
+
+ int16_t thrust, pitch, roll, yaw;
+
+ pitch = roll = 0;
+
+ uint8_t shift = 8 - (usb_thread.data[8] >> 3);
+
+ thrust = usb_thread.data[0] * 10;
+
+ yaw = i2c_thread.z >> shift;
+
+
+ pwmEnableChannel(&PWMD2, 0, 1000 + thrust + pitch + roll - yaw);
+ pwmEnableChannel(&PWMD2, 1, 1000 + thrust + pitch - roll + yaw);
+ pwmEnableChannel(&PWMD2, 2, 1000 + thrust - pitch + roll + yaw);
+ pwmEnableChannel(&PWMD2, 3, 1000 + thrust - pitch - roll - yaw);
+}
+
+static PWMConfig pwmcfg = {
+ 1000000,
+ 1000000 / 50,
+ foo,
+ {
+ {PWM_OUTPUT_ACTIVE_HIGH, NULL},
+ {PWM_OUTPUT_ACTIVE_HIGH, NULL},
+ {PWM_OUTPUT_ACTIVE_HIGH, NULL},
+ {PWM_OUTPUT_ACTIVE_HIGH, NULL}
+ },
+ 0
+};
+
int main(void) {
halInit();
chSysInit();
@@ -200,7 +200,6 @@ int main(void) { i2c_thread.start();
- /*
pwmStart(&PWMD2, &pwmcfg);
palSetPadMode(GPIOA, 0, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
palSetPadMode(GPIOA, 1, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
@@ -214,7 +213,6 @@ int main(void) { usb_thread.usbs = &usbs;
usb_thread.start();
- */
while (1) {
chThdSleepMilliseconds(1000);
|