Move pwm output code into MotorMixer-class.

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