hexa frame setp

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1536 f9c3cf11-9bcb-44bc-f272-b75c42450872

ArduCopterMega/APM_Config.h

@@ -2,23 +2,26 @@
 // Once you upload the code, run the factory "reset" to save all config values to EEPROM.
 // After reset, use the setup mode to set your radio limits for CH1-4, and to set your flight modes.
 
-#define GPS_PROTOCOL  		GPS_PROTOCOL_MTK
+#define GPS_PROTOCOL  	    GPS_PROTOCOL_MTK
 #define GCS_PROTOCOL        GCS_PROTOCOL_NONE
 
-#define MAGORIENTATION	AP_COMPASS_COMPONENTS_UP_PINS_BACK
+//#define MAGORIENTATION	AP_COMPASS_COMPONENTS_UP_PINS_BACK
 //#define MAGORIENTATION	AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
 
+#define MAGORIENTATION	AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
 #define ARM_AT_STARTUP	0
 
 
 // For future development, don't enable unless you know them
 // These are all experimental and underwork, jp 23-12-10
-//#define ENABLE_EXTRAS     ENABLED
+#define ENABLE_EXTRAS     ENABLED
-//#define ENABLE_EXTRAINIT  ENABLED
+#define ENABLE_EXTRAINIT  ENABLED
 //#define ENABLE_CAM        ENABLED
-//#define ENABLE_AM         ENABLED
+#define ENABLE_AM         ENABLED
 //#define ENABLE_xx         ENABLED
 
 
 // Logging
 #define LOG_CURRENT ENABLED
+
+

ArduCopterMega/ArduCopterMega.pde

@@ -426,6 +426,7 @@ boolean SW_DIP2;
 boolean SW_DIP3;
 boolean SW_DIP4;  // closest to header pins
 
+int counteri = 0;
 
 
 // Basic Initialization
@@ -956,4 +957,4 @@ void read_AHRS(void)
 	roll_sensor 	= dcm.roll_sensor;
 	pitch_sensor 	= dcm.pitch_sensor;
 	yaw_sensor 		= dcm.yaw_sensor;
-}
+}

ArduCopterMega/defines.h

@@ -51,6 +51,9 @@
 #define FRONT 2
 #define BACK 3
 
+#define RIGHTFRONT 6
+#define LEFTBACK 7
+
 #define MAX_SERVO_OUTPUT 2700
 
 #define SONAR 0
@@ -71,6 +74,7 @@
 #define PLUS_FRAME 0
 #define X_FRAME 1
 #define TRI_FRAME 2
+#define HEXA_FRAME 3
 
 // Auto Pilot modes
 // ----------------

ArduCopterMega/radio.pde

@@ -112,6 +112,7 @@ void set_servos_4(void)
 {
 	static byte num;
 
+
 	// Quadcopter mix
 	if (motor_armed == true) {
 		int out_min = rc_3.radio_min;
@@ -170,7 +171,32 @@ void set_servos_4(void)
 			APM_RC.OutputCh(CH_7,rc_4.radio_out);
 
 			
-		}else{
+		}else if (frame_type == HEXA_FRAME) {
+
+//  			int roll_out 	= rc_1.pwm_out / 2;
+//			int pitch_out 	= rc_2.pwm_out / 2;
+
+			int roll_out 	= (float)rc_1.pwm_out * .866;
+			int pitch_out 	= rc_2.pwm_out / 2;
+
+			motor_out[FRONT]	= rc_3.radio_out + roll_out + pitch_out + rc_4.pwm_out;  // CCW
+                        motor_out[RIGHTFRONT]   = rc_3.radio_out - roll_out + pitch_out - rc_4.pwm_out;  // CW
+
+			motor_out[LEFT]		= rc_3.radio_out + rc_1.pwm_out - rc_4.pwm_out;          // CW
+			motor_out[RIGHT]	= rc_3.radio_out - rc_1.pwm_out + rc_4.pwm_out;          // CCW
+
+                        motor_out[LEFTBACK]     = rc_3.radio_out + roll_out - pitch_out + rc_4.pwm_out;  // CW
+			motor_out[BACK] 	= rc_3.radio_out - roll_out - pitch_out - rc_4.pwm_out;  // CCW
+/*
+  
+            if(counteri == 5) {
+  		Serial.printf(" %d  %d \n%d    %d        %d   %d \n %d  %d \n\n", motor_out[FRONT], motor_out[RIGHTFRONT], motor_out[LEFT], motor_out[RIGHT], roll_out, pitch_out, motor_out[LEFTBACK], motor_out[BACK]);
+                counteri = 0;
+            }
+            counteri++;
+            
+*/
+                } else {
 		
 			Serial.print("frame error");
 			
@@ -225,6 +251,12 @@ void set_servos_4(void)
 			APM_RC.OutputCh(CH_2, motor_out[LEFT]);
 			APM_RC.OutputCh(CH_3, motor_out[FRONT]);
 			APM_RC.OutputCh(CH_4, motor_out[BACK]);
+
+                  if (frame_type == HEXA_FRAME) {
+                        APM_RC.OutputCh(CH_7, motor_out[RIGHTFRONT]);
+                        APM_RC.OutputCh(CH_8, motor_out[LEFTBACK]);
+                  }
+
 			
 		}else{
 		
@@ -232,6 +264,12 @@ void set_servos_4(void)
 			APM_RC.OutputCh(CH_2, rc_3.radio_min);
 			APM_RC.OutputCh(CH_3, rc_3.radio_min);
 			APM_RC.OutputCh(CH_4, rc_3.radio_min);
+
+                  if (frame_type == HEXA_FRAME) {
+                        APM_RC.OutputCh(CH_7, rc_3.radio_min);
+                        APM_RC.OutputCh(CH_8, rc_3.radio_min);
+                  }
+
 			
 		}
 	
@@ -253,4 +291,4 @@ void set_servos_4(void)
 		// Initialize yaw command to actual yaw when throttle is down...
 		rc_4.control_in = ToDeg(yaw);
 	}
- }
+ }

ArduCopterMega/setup.pde

@@ -425,8 +425,11 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
 	} else if (!strcmp_P(argv[1].str, PSTR("tri"))) {
 		frame_type = TRI_FRAME;
 
+	} else if (!strcmp_P(argv[1].str, PSTR("hexa"))) {
+		frame_type = HEXA_FRAME;
+
 	} else {
-		Serial.printf_P(PSTR("\nOptions:[+, x, tri]\n"));
+		Serial.printf_P(PSTR("\nOptions:[+, x, tri, hexa]\n"));
 		report_frame();
 		return 0;
 	}
@@ -732,6 +735,8 @@ void report_frame()
 		Serial.printf_P(PSTR("Plus "));
 	else if(frame_type == TRI_FRAME)
 		Serial.printf_P(PSTR("TRI "));
+	else if(frame_type == HEXA_FRAME)
+		Serial.printf_P(PSTR("HEXA "));
 
 	Serial.printf_P(PSTR("frame (%d)"), (int)frame_type);
 	print_blanks(2);