1 files changed, 69 insertions, 0 deletions
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;
+}