From 88043415b901e2f00855c546fc3a664a3e0877d3 Mon Sep 17 00:00:00 2001
From: Vegard Storheil Eriksen <zyp@jvnv.net>
Date: Sun, 3 Apr 2011 21:19:54 +0200
Subject: Yaw stabilization.

---
 main.cpp | 78 +++++++++++++++++++++++++++++++---------------------------------
 1 file changed, 38 insertions(+), 40 deletions(-)

diff --git a/main.cpp b/main.cpp
index 88d8693..62f7494 100644
--- a/main.cpp
+++ b/main.cpp
@@ -27,10 +27,12 @@ class USBThread : public BaseThread<USBThread, 256> {
 	public:
 		USBSerial* usbs;
 		
-		int8_t thrust, pitch, yaw, roll;
+		uint8_t data[9];
 		
 		noreturn_t thread_main() {
-			thrust = pitch = roll = yaw = 0;
+			for(int i = 0; i < 9; i++) {
+				data[i] = 0;
+			}
 			
 			w_s_t w_s = W_S;
 			uint8_t w_n = 0;
@@ -40,25 +42,12 @@ class USBThread : public BaseThread<USBThread, 256> {
 				if(buffer >= 0 && buffer < 256) {
 					if(w_s == W_S && buffer == 'S') {
 						w_s = W_N;
-					} else if(w_s == W_N && buffer >= '1' && buffer <= '4') {
+					} else if(w_s == W_N && buffer >= '1' && buffer <= '9') {
 						w_s = W_V;
 						w_n = buffer - '1';
 					} else if(w_s == W_V) {
 						w_s = W_S;
-						switch(w_n) {
-							case 0:
-								thrust = buffer;
-								break;
-							case 1:
-								pitch = buffer - 64;
-								break;
-							case 2:
-								roll = buffer - 64;
-								break;
-							case 3:
-								yaw = buffer - 64;
-								break;
-						}
+						data[w_n] = buffer;
 					} else {
 						w_s = W_S;
 					}
@@ -68,27 +57,6 @@ class USBThread : public BaseThread<USBThread, 256> {
 };
 
 USBThread usb_thread;
-
-void foo(PWMDriver*) {
-	pwmEnableChannel(&PWMD2, 0, 1000 + usb_thread.thrust * 10 + usb_thread.pitch * 3 + usb_thread.roll * 3 - usb_thread.yaw * 3);
-	pwmEnableChannel(&PWMD2, 1, 1000 + usb_thread.thrust * 10 + usb_thread.pitch * 3 - usb_thread.roll * 3 + usb_thread.yaw * 3);
-	pwmEnableChannel(&PWMD2, 2, 1000 + usb_thread.thrust * 10 - usb_thread.pitch * 3 + usb_thread.roll * 3 + usb_thread.yaw * 3);
-	pwmEnableChannel(&PWMD2, 3, 1000 + usb_thread.thrust * 10 - usb_thread.pitch * 3 - usb_thread.roll * 3 - usb_thread.yaw * 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
-};
-
 USBSerial usbs;
 
 I2CConfig i2cconfig = {
@@ -190,6 +158,38 @@ class I2CThread : public BaseThread<I2CThread, 256> {
 
 I2CThread i2c_thread;
 
+void foo(PWMDriver*) {
+	
+	int16_t thrust, pitch, roll, yaw;
+	
+	pitch = roll = 0;
+	
+	uint8_t shift = 8 - (usb_thread.data[8] >> 3);
+	
+	thrust = usb_thread.data[0] * 10;
+	
+	yaw = i2c_thread.z >> shift;
+	
+	
+	pwmEnableChannel(&PWMD2, 0, 1000 + thrust + pitch + roll - yaw);
+	pwmEnableChannel(&PWMD2, 1, 1000 + thrust + pitch - roll + yaw);
+	pwmEnableChannel(&PWMD2, 2, 1000 + thrust - pitch + roll + yaw);
+	pwmEnableChannel(&PWMD2, 3, 1000 + thrust - pitch - roll - yaw);
+}
+
+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();
@@ -200,7 +200,6 @@ int main(void) {
 	
 	i2c_thread.start();
 	
-	/*
 	pwmStart(&PWMD2, &pwmcfg);
 	palSetPadMode(GPIOA, 0, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
 	palSetPadMode(GPIOA, 1, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
@@ -214,7 +213,6 @@ int main(void) {
 	
 	usb_thread.usbs = &usbs;
 	usb_thread.start();
-	*/
 	
 	while (1) {
 		chThdSleepMilliseconds(1000);
-- 
cgit v1.2.3