From b1ffabc9a3fa353bfd224e746859ca43ff6ec8ee Mon Sep 17 00:00:00 2001
From: jasonshort <jasonshort@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Sat, 19 Mar 2011 19:21:43 +0000
Subject: [PATCH] Added Y6 support

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1792 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 ArduCopterMega/defines.h  |  1 +
 ArduCopterMega/motors.pde | 45 ++++++++++++++++++++++++++++++++-------
 ArduCopterMega/setup.pde  |  7 +++++-
 3 files changed, 44 insertions(+), 9 deletions(-)

diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h
index d89d9607c5..febf8da346 100644
--- a/ArduCopterMega/defines.h
+++ b/ArduCopterMega/defines.h
@@ -21,6 +21,7 @@
 #define X_FRAME 1
 #define TRI_FRAME 2
 #define HEXA_FRAME 3
+#define Y6_FRAME 4
 
 // Internal defines, don't edit and expect things to work
 // -------------------------------------------------------
diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde
index 5ec7b16fc8..386136396b 100644
--- a/ArduCopterMega/motors.pde
+++ b/ArduCopterMega/motors.pde
@@ -100,11 +100,13 @@ set_servos_4()
 			int roll_out 		= (float)g.rc_1.pwm_out * .866;
 			int pitch_out 		= g.rc_2.pwm_out / 2;
 
-			// front two motors
+			//left front
 			motor_out[CH_2]		= g.rc_3.radio_out + roll_out + pitch_out;
+
+			//right front
 			motor_out[CH_1]		= g.rc_3.radio_out - roll_out + pitch_out;
 
-			// rear motors
+			// rear
 			motor_out[CH_4] 	= g.rc_3.radio_out - g.rc_2.pwm_out;
 
 			// this is a compensation for the angle of the yaw motor. Its linear, but should work ok.
@@ -130,13 +132,40 @@ set_servos_4()
             motor_out[CH_7] 	= g.rc_3.radio_out - roll_out + pitch_out;	// CCW
 			motor_out[CH_4] 	= g.rc_3.radio_out - roll_out - pitch_out;	// CCW
 
-			motor_out[CH_7]		+= g.rc_4.pwm_out;	// CCW
 			motor_out[CH_2]		+= g.rc_4.pwm_out;	// CCW
+			motor_out[CH_7]		+= g.rc_4.pwm_out;	// CCW
 			motor_out[CH_4] 	+= g.rc_4.pwm_out;	// CCW
 
 			motor_out[CH_3]		-= g.rc_4.pwm_out;	// CW
 			motor_out[CH_1]		-= g.rc_4.pwm_out;	// CW
-			motor_out[CH_8]     -= g.rc_4.pwm_out;  // CW
+			motor_out[CH_8]		-= g.rc_4.pwm_out;  // CW
+
+		}else if (g.frame_type == Y6_FRAME) {
+			//Serial.println("Y6_FRAME");
+
+			int roll_out 		= (float)g.rc_1.pwm_out * .866;
+			int pitch_out 		=  g.rc_2.pwm_out / 2;
+
+			//left
+			motor_out[CH_2]		= (g.rc_3.radio_out + roll_out + pitch_out) * 0.95;  // CCW TOP
+			motor_out[CH_3]		=  g.rc_3.radio_out + roll_out + pitch_out;			// CW
+
+			//right
+			motor_out[CH_7]		= (g.rc_3.radio_out - roll_out + pitch_out) * 0.95;	// CCW TOP
+			motor_out[CH_1]		=  g.rc_3.radio_out - roll_out + pitch_out;			// CW
+
+			//back
+			motor_out[CH_8]     = (g.rc_3.radio_out - g.rc_2.pwm_out) * 0.95;	// CW TOP
+			motor_out[CH_4] 	=  g.rc_3.radio_out - g.rc_2.pwm_out;			// CCW
+
+			//yaw
+			motor_out[CH_2]		+= g.rc_4.pwm_out;	// CCW
+			motor_out[CH_7]		+= g.rc_4.pwm_out;	// CCW
+			motor_out[CH_4] 	+= g.rc_4.pwm_out;	// CCW
+
+			motor_out[CH_3]		-= g.rc_4.pwm_out;	// CW
+			motor_out[CH_1]		-= g.rc_4.pwm_out;	// CW
+			motor_out[CH_8]		-= g.rc_4.pwm_out;  // CW
 
     	}else{
 
@@ -151,7 +180,7 @@ set_servos_4()
 		motor_out[CH_3]		= constrain(motor_out[CH_3], 	out_min, g.rc_3.radio_max.get());
 		motor_out[CH_4] 	= constrain(motor_out[CH_4], 	out_min, g.rc_3.radio_max.get());
 
-		if (g.frame_type == HEXA_FRAME) {
+		if ((g.frame_type == HEXA_FRAME) || (g.frame_type == Y6_FRAME)) {
 			motor_out[CH_7]		= constrain(motor_out[CH_7], 	out_min, g.rc_3.radio_max.get());
 			motor_out[CH_8]		= constrain(motor_out[CH_8], 	out_min, g.rc_3.radio_max.get());
 		}
@@ -244,7 +273,7 @@ set_servos_4()
 			APM_RC.Force_Out0_Out1();
 			APM_RC.Force_Out2_Out3();
 
-			if (g.frame_type == HEXA_FRAME) {
+			if ((g.frame_type == HEXA_FRAME) || (g.frame_type == Y6_FRAME)) {
 				APM_RC.OutputCh(CH_7, motor_out[CH_7]);
 				APM_RC.OutputCh(CH_8, motor_out[CH_8]);
 				APM_RC.Force_Out6_Out7();
@@ -260,7 +289,7 @@ set_servos_4()
 			APM_RC.Force_Out0_Out1();
 			APM_RC.Force_Out2_Out3();
 
-			if (g.frame_type == HEXA_FRAME) {
+			if ((g.frame_type == HEXA_FRAME) || (g.frame_type == Y6_FRAME)) {
 				APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
 				APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
 				APM_RC.Force_Out6_Out7();
@@ -289,7 +318,7 @@ set_servos_4()
 		APM_RC.OutputCh(CH_4, motor_out[CH_4]);
 
 
-		if (g.frame_type == HEXA_FRAME) {
+		if ((g.frame_type == HEXA_FRAME) || (g.frame_type == Y6_FRAME)){
 			APM_RC.OutputCh(CH_7, motor_out[CH_7]);
 			APM_RC.OutputCh(CH_8, motor_out[CH_8]);
 		}
diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde
index 82a3221db7..f67733e996 100644
--- a/ArduCopterMega/setup.pde
+++ b/ArduCopterMega/setup.pde
@@ -451,8 +451,11 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
 	} else if (!strcmp_P(argv[1].str, PSTR("hexa"))) {
 		g.frame_type = HEXA_FRAME;
 
+	} else if (!strcmp_P(argv[1].str, PSTR("y6"))) {
+		g.frame_type = Y6_FRAME;
+
 	}else{
-		Serial.printf_P(PSTR("\nOptions:[+, x, tri, hexa]\n"));
+		Serial.printf_P(PSTR("\nOptions:[+, x, tri, hexa, y6]\n"));
 		report_frame();
 		return 0;
 	}
@@ -780,6 +783,8 @@ void report_frame()
 		Serial.printf_P(PSTR("TRI "));
 	else if(g.frame_type == HEXA_FRAME)
 		Serial.printf_P(PSTR("HEXA "));
+	else if(g.frame_type == Y6_FRAME)
+		Serial.printf_P(PSTR("Y6 "));
 
 	Serial.printf_P(PSTR("frame (%d)"), (int)g.frame_type);
 	print_blanks(2);