Yaw stabilization.

1 files 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);