1 files changed, 0 insertions, 86 deletions
diff --git a/motormixer.cpp b/motormixer.cpp
deleted file mode 100644
index 5718526..0000000
--- a/motormixer.cpp
+++ /dev/null
@@ -1,86 +0,0 @@
-#include "motormixer.h"
-
-#include <ch.h>
-#include <hal.h>
-
-template<class T>
-inline T abs(T val) {
-	if(val > 0) {
-		return val;
-	} else {
-		return -val;
-	}
-}
-
-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;
-	}
-	
-	// Check that total thrust will not be exceeded.
-	if(abs(pitch) + abs(roll) > thrust) {
-		// If saturated by pitch/roll, drop yaw component.
-		yaw = 0;
-		
-		// Scale pitch and roll.
-		int32_t df = ((abs(pitch) + abs(roll)) << 16) / thrust;
-		
-		pitch = (pitch << 16) / df;
-		roll = (roll << 16) / df;
-	
-	} else if(abs(pitch) + abs(roll) + abs(yaw) > thrust) {
-		// Scale yaw value.
-		yaw = (yaw > 0 ? 1 : -1) * (thrust - abs(pitch) - abs(roll));
-	}
-	
-	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;
-}