Move pwm output code into MotorMixer-class.

1 files changed, 5 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();