Added resetting of Loiter location based on stick input. Fly it to a location and let it stick.

Added Octo Support
Fixed Loiter issue regarding Yaw towards loiter location
Added Yaw control alternative for testing
decreased Yaw deadband to +-5°
revved to 2.0.10



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

ArduCopterMega/APM_Config.h

@@ -7,6 +7,7 @@
 //#define MAG_ORIENTATION		AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
 
 #define NAV_TEST 1	// 0 = traditional, 1 = rate controlled
+#define YAW_OPTION 0	// hybrid rate approach, offset Yaw approach
 
 #define FRAME_CONFIG QUAD_FRAME
 	/*
@@ -15,6 +16,7 @@
 	TRI_FRAME
 	HEXA_FRAME
 	Y6_FRAME
+	OCTA_FRAME
 	*/
 
 #define FRAME_ORIENTATION X_FRAME

ArduCopterMega/ArduCopterMega.pde

@@ -820,7 +820,7 @@ void slow_loop()
 
 
 			#if MOTOR_LEDS == 1
-				update_motor_light();
+				update_motor_leds();
 			#endif
 
 			break;
@@ -1101,7 +1101,9 @@ void update_current_flight_mode(void)
 				#if AUTO_RESET_LOITER == 1
 				if((g.rc_2.control_in + g.rc_1.control_in) != 0){
 					// reset LOITER to current position
+					long temp = next_WP.alt;
 					next_WP = current_loc;
+					next_WP.alt = temp;
 				}
 				#endif
 
@@ -1152,13 +1154,12 @@ void update_navigation()
 			update_nav_wp();
 			break;
 
-		case LOITER:
 		case RTL:
-			// are we Traversing or Loitering?
-			wp_control = (wp_distance < 10) ? LOITER_MODE : WP_MODE;
-
 			// calculates desired Yaw
 			update_nav_yaw();
+		case LOITER:
+			// are we Traversing or Loitering?
+			wp_control = (wp_distance < 10) ? LOITER_MODE : WP_MODE;
 
 			// calculates the desired Roll and Pitch
 			update_nav_wp();

ArduCopterMega/Attitude.pde

@@ -202,6 +202,7 @@ clear_yaw_control()
 	yaw_debug 			= YAW_HOLD; 		//0
 }
 
+#if YAW_OPTION == 0
 void
 output_yaw_with_hold(boolean hold)
 {
@@ -271,3 +272,35 @@ output_yaw_with_hold(boolean hold)
 
 	//Serial.printf("%d\n",g.rc_4.servo_out);
 }
+#elif YAW_OPTION == 1
+
+void
+output_yaw_with_hold(boolean hold)
+{
+	long rate		= degrees(omega.z) * 100; 											// 3rad = 17188 , 6rad = 34377
+	rate			= constrain(rate, -36000, 36000);									// limit to something fun!
+	int dampener 	= rate * g.pid_yaw.kD();											// 34377 * .175 = 6000
+
+	// set nav_yaw + or - the current location
+	nav_yaw 	= (long)g.rc_4.control_in + dcm.yaw_sensor;
+
+	// we need to wrap our value so we can be 0 to 360 (*100)
+	nav_yaw 	= wrap_360(nav_yaw);
+
+	// how far off is nav_yaw from our current yaw?
+	yaw_error 	= nav_yaw - dcm.yaw_sensor;
+
+	// we need to wrap our value so we can be -180 to 180 (*100)
+	yaw_error 	= wrap_180(yaw_error);
+
+	// limit the error we're feeding to the PID
+	yaw_error				= constrain(yaw_error,	 -4000, 4000);						// limit error to 60 degees
+
+	// Apply PID and save the new angle back to RC_Channel
+	g.rc_4.servo_out 		= g.pid_yaw.get_pid(yaw_error, delta_ms_fast_loop, 1.0); 		// .4 * 4000 = 1600
+
+	// add in yaw dampener
+	g.rc_4.servo_out		-= constrain(dampener, -1600, 1600);
+	yaw_debug 				= YAW_HOLD; //0
+}
+#endif

ArduCopterMega/config.h

@@ -350,13 +350,13 @@
 # define YAW_P					0.4			// increase for more aggressive Yaw Hold, decrease if it's bouncy
 #endif
 #ifndef  YAW_I
-# define YAW_I					0.00		// increased to .1 to try and get over user's steady state error caused by poor balance
+# define YAW_I					0.000		// set to .0001 to try and get over user's steady state error caused by poor balance
 #endif
 #ifndef  YAW_D
 # define YAW_D					0.05		// Trying a lower value to prevent odd behavior
 #endif
 #ifndef  YAW_IMAX
-# define YAW_IMAX				0			//	Always 0
+# define YAW_IMAX				1			// degrees * 100
 #endif
 
 //////////////////////////////////////////////////////////////////////////////

ArduCopterMega/defines.h

@@ -1,16 +1,6 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
 
-// ACM:
-// Motors
-#define RIGHT CH_1
-#define LEFT CH_2
-#define FRONT CH_3
-#define BACK CH_4
-#define RIGHTFRONT CH_7
-#define LEFTBACK CH_8
-#define MAX_SERVO_OUTPUT 2700
-
 // active altitude sensor
 // ----------------------
 #define SONAR 0
@@ -21,6 +11,7 @@
 #define TRI_FRAME 1
 #define HEXA_FRAME 2
 #define Y6_FRAME 3
+#define OCTA_FRAME 4
 
 #define PLUS_FRAME 0
 #define X_FRAME 1
@@ -61,6 +52,8 @@
 #define CH_6 5
 #define CH_7 6
 #define CH_8 7
+#define CH_10 9    //PB5
+#define CH_11 10   //PE3
 
 #define CH_ROLL CH_1
 #define CH_PITCH CH_2

ArduCopterMega/radio.pde

@@ -22,7 +22,7 @@ void init_rc_in()
 	g.rc_1.dead_zone = 60;		// 60 = .6 degrees
 	g.rc_2.dead_zone = 60;
 	g.rc_3.dead_zone = 60;
-	g.rc_4.dead_zone = 1000;
+	g.rc_4.dead_zone = 500;
 
 	//set auxiliary ranges
 	g.rc_5.set_range(0,1000);
@@ -93,6 +93,12 @@ void output_min()
 
 	APM_RC.OutputCh(CH_7,   g.rc_3.radio_min);
     APM_RC.OutputCh(CH_8,   g.rc_3.radio_min);
+
+	#if FRAME_CONFIG ==	OCTA_FRAME
+	APM_RC.OutputCh(CH_10,   g.rc_3.radio_min);
+    APM_RC.OutputCh(CH_11,   g.rc_3.radio_min);
+	#endif
+
 }
 void read_radio()
 {

ArduCopterMega/setup.pde

@@ -232,6 +232,12 @@ init_esc()
 		APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
 		APM_RC.OutputCh(CH_7, g.rc_3.radio_in);
 		APM_RC.OutputCh(CH_8, g.rc_3.radio_in);
+
+		#if FRAME_CONFIG ==	OCTA_FRAME
+		APM_RC.OutputCh(CH_10,   g.rc_3.radio_in);
+		APM_RC.OutputCh(CH_11,   g.rc_3.radio_in);
+		#endif
+
 	}
 }
 
@@ -495,6 +501,8 @@ void report_frame()
 	Serial.printf_P(PSTR("Hexa frame\n"));
 #elif FRAME_CONFIG == Y6_FRAME
 	Serial.printf_P(PSTR("Y6 frame\n"));
+#elif FRAME_CONFIG == OCTA_FRAME
+	Serial.printf_P(PSTR("Octa frame\n"));
 #endif
 
 	if(g.frame_orientation == X_FRAME)

ArduCopterMega/system.pde

@@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
 };
 
 // Create the top-level menu object.
-MENU(main_menu, "AC 2.0.9 Beta", main_menu_commands);
+MENU(main_menu, "AC 2.0.10 Beta", main_menu_commands);
 
 void init_ardupilot()
 {
@@ -234,6 +234,9 @@ void init_ardupilot()
 	// ---------------------------
 	//reset_control_switch();
 
+	// init the Yaw Hold output
+	clear_yaw_control();
+
 	// Logging:
 	// --------
 	if(g.log_bitmask != 0){
@@ -390,7 +393,7 @@ void set_failsafe(boolean mode)
 }
 
 #if MOTOR_LEDS == 1
-void update_motor_light(void)
+void update_motor_leds(void)
 {
 	// blink rear
 	static bool blink;

ArduCopterMega/test.pde

@@ -978,8 +978,8 @@ void fake_out_gps()
 
 void print_motor_out(){
 	Serial.printf("out: R: %d,  L: %d  F: %d  B: %d\n",
-				(motor_out[RIGHT] 	- g.rc_3.radio_min),
-				(motor_out[LEFT] 	- g.rc_3.radio_min),
-				(motor_out[FRONT] 	- g.rc_3.radio_min),
-				(motor_out[BACK] 	- g.rc_3.radio_min));
+				(motor_out[CH_1] 	- g.rc_3.radio_min),
+				(motor_out[CH_2] 	- g.rc_3.radio_min),
+				(motor_out[CH_3] 	- g.rc_3.radio_min),
+				(motor_out[CH_4] 	- g.rc_3.radio_min));
 }