summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--main.cpp66
-rw-r--r--motormixer.cpp69
-rw-r--r--motormixer.h12
3 files changed, 86 insertions, 61 deletions
diff --git a/main.cpp b/main.cpp
index 3b37344..7589f35 100644
--- a/main.cpp
+++ b/main.cpp
@@ -4,6 +4,7 @@
#include "bma150.h"
#include "ak8975.h"
#include "ppmsum.h"
+#include "motormixer.h"
#include <ch.h>
#include <hal.h>
@@ -28,6 +29,7 @@ class LEDThread : public BaseThread<LEDThread, 128> {
LEDThread led_thread;
PPMSum ppmsum;
+MotorMixer motors;
class USBThread : public BaseThread<USBThread, 256> {
private:
@@ -119,8 +121,6 @@ class I2CThread : public BaseThread<I2CThread, 256> {
sensordata[10] = roll;
sensordata[11] = yaw;
-
-
usbs.write(syncword, sizeof(syncword));
usbs.write(buf, sizeof(buf));
@@ -131,6 +131,8 @@ class I2CThread : public BaseThread<I2CThread, 256> {
int(q0 * 10000), int(q1 * 10000), int(q2 * 10000), int(q3 * 10000),
int(pitch * 10000), int(roll * 10000), int(yaw * 10000));*/
+ motors.update(ppmsum.data[2], 0, 0, 0);
+
time += MS2ST(10);
if(time > chTimeNow()) {
chThdSleepUntil(time);
@@ -176,55 +178,6 @@ class ADCThread : public BaseThread<ADCThread, 128> {
ADCThread adc_thread;
-void foo(PWMDriver*) {
-
- int16_t thrust, pitch, roll, yaw;
-
- thrust = ppmsum.data[2];
- if(thrust < 0) {
- thrust = 0;
- }
- if(thrust > 1000) {
- thrust = 1000;
- }
-
- pitch = ((ppmsum.data[1] - 500) * thrust) >> 10;
- roll = ((ppmsum.data[0] - 500) * thrust) >> 10;
- yaw = ((ppmsum.data[3] - 500) * thrust) >> 10;
-
- //pitch = (-i2c_thread.x) >> 5;
- //roll = i2c_thread.y >> 5;
- //yaw = i2c_thread.z >> 5;
-
- int16_t motor_1 = thrust + pitch + roll - yaw;
- int16_t motor_2 = thrust + pitch - roll + yaw;
- int16_t motor_3 = thrust - pitch + roll + yaw;
- int16_t motor_4 = thrust - pitch - roll - yaw;
-
- sensordata[20] = motor_1;
- sensordata[21] = motor_2;
- sensordata[22] = motor_3;
- sensordata[23] = motor_4;
-
- pwmEnableChannel(&PWMD2, 0, 1000 + motor_1);
- pwmEnableChannel(&PWMD2, 1, 1000 + motor_2);
- pwmEnableChannel(&PWMD2, 2, 1000 + motor_3);
- pwmEnableChannel(&PWMD2, 3, 1000 + motor_4);
-}
-
-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();
@@ -239,16 +192,7 @@ int main(void) {
adc_thread.start();
- 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);
+ motors.start();
usb_thread.usbs = &usbs;
usb_thread.start();
diff --git a/motormixer.cpp b/motormixer.cpp
new file mode 100644
index 0000000..1951f29
--- /dev/null
+++ b/motormixer.cpp
@@ -0,0 +1,69 @@
+#include "motormixer.h"
+
+#include <ch.h>
+#include <hal.h>
+
+namespace {
+ uint16_t motors[4];
+
+ void foo(PWMDriver*) {
+ pwmEnableChannel(&PWMD2, 0, 1000 + motors[0]);
+ pwmEnableChannel(&PWMD2, 1, 1000 + motors[1]);
+ pwmEnableChannel(&PWMD2, 2, 1000 + motors[2]);
+ pwmEnableChannel(&PWMD2, 3, 1000 + motors[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
+ };
+};
+
+void MotorMixer::start() {
+ 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);
+}
+
+void MotorMixer::update(int16_t thrust, int16_t pitch, int16_t roll, int16_t yaw) {
+ //thrust = ppmsum.data[2];
+ if(thrust < 0) {
+ thrust = 0;
+ }
+ if(thrust > 1000) {
+ thrust = 1000;
+ }
+
+ //pitch = ((ppmsum.data[1] - 500) * thrust) >> 10;
+ //roll = ((ppmsum.data[0] - 500) * thrust) >> 10;
+ //yaw = ((ppmsum.data[3] - 500) * thrust) >> 10;
+
+ //pitch = (-i2c_thread.x) >> 5;
+ //roll = i2c_thread.y >> 5;
+ //yaw = i2c_thread.z >> 5;
+
+ motors[0] = thrust + pitch + roll - yaw;
+ motors[1] = thrust + pitch - roll + yaw;
+ motors[2] = thrust - pitch + roll + yaw;
+ motors[3] = thrust - pitch - roll - yaw;
+
+ //sensordata[20] = motor_1;
+ //sensordata[21] = motor_2;
+ //sensordata[22] = motor_3;
+ //sensordata[23] = motor_4;
+}
diff --git a/motormixer.h b/motormixer.h
new file mode 100644
index 0000000..5ec1869
--- /dev/null
+++ b/motormixer.h
@@ -0,0 +1,12 @@
+#ifndef MOTORMIXER_H
+#define MOTORMIXER_H
+
+#include <stdint.h>
+
+class MotorMixer {
+ public:
+ void start();
+ void update(int16_t thrust, int16_t pitch, int16_t roll, int16_t yaw);
+};
+
+#endif