diff options
| -rw-r--r-- | main.cpp | 66 | ||||
| -rw-r--r-- | motormixer.cpp | 69 | ||||
| -rw-r--r-- | motormixer.h | 12 |
3 files changed, 86 insertions, 61 deletions
@@ -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 |