refinements to nav

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

ArduCopterMega/APM_Config.h

@@ -17,6 +17,7 @@
 #define STABILIZE_PITCH_P		0.75
 //#define STABILIZE_DAMPENER		0.1
 
+#define NAV_TEST 0	// 0 = traditional, 1 = rate controlled
 
 //#define CHANNEL_6_TUNING CH6_NONE
 #define CHANNEL_6_TUNING CH6_PMAX

ArduCopterMega/ArduCopterMega.pde

@@ -899,9 +899,6 @@ void update_current_flight_mode(void)
 				nav_pitch 		= 0;
 				nav_roll 		= 0;
 
-				// Output Pitch, Roll, Yaw and Throttle
-				// ------------------------------------
-
 				// Yaw control
 				output_manual_yaw();
 
@@ -1171,7 +1168,6 @@ void update_alt()
 		// read barometer
 		baro_alt 		= read_barometer();
 
-
 		// XXX temp removed fr debugging
 		//filter out bad sonar reads
 		//int temp 		= sonar.read();

ArduCopterMega/commands.pde

@@ -108,11 +108,9 @@ void increment_WP_index()
 {
     if (g.waypoint_index < g.waypoint_total) {
         g.waypoint_index.set_and_save(g.waypoint_index + 1);
-		SendDebug("MSG <increment_WP_index> WP index is incremented to ");
-	}else{
-		//SendDebug("MSG <increment_WP_index> Failed to increment WP index of ");
-		// This message is used excessively at the end of a mission
+		//SendDebug("MSG <increment_WP_index> WP index is incremented to ");
 	}
+
     SendDebugln(g.waypoint_index,DEC);
 }
 

ArduCopterMega/commands_logic.pde

@@ -106,11 +106,7 @@ void handle_no_commands()
 {
 	// we don't want to RTL. Maybe this will change in the future. RTL is kinda dangerous.
 	// use landing commands
-	return;
 	/*
-	if (command_must_ID)
-		return;
-
 	switch (control_mode){
 		default:
 			//set_mode(RTL);
@@ -226,6 +222,7 @@ void do_takeoff()
 {
 	wp_control 			= LOITER_MODE;
 
+	// Start with current location
 	Location temp		= current_loc;
 
 	// next_command.alt is a relative altitude!!!
@@ -233,9 +230,7 @@ void do_takeoff()
 
 	takeoff_complete	= false;			// set flag to use g_gps ground course during TO.  IMU will be doing yaw drift correction
 
-	//Serial.print("dt ");
-	//Serial.println(temp.alt, DEC);
-
+	// Set our waypoint
 	set_next_WP(&temp);
 }
 

ArduCopterMega/commands_process.pde

@@ -18,13 +18,16 @@ void update_commands(void)
 	next_command = get_command_with_index(g.waypoint_index + 1);
 
 	if(next_command.id == NO_COMMAND){
+
 		// if no commands were available from EEPROM
 		// --------------------------------------------
-
+		if (command_must_ID == NO_COMMAND)
 			handle_no_commands();
+
 		gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
 
 	} else {
+
 		// A command was loaded from EEPROM
 		// --------------------------------------------
 

ArduCopterMega/config.h

@@ -368,33 +368,31 @@
 // Navigation control gains
 //
 #ifndef NAV_LOITER_P
-# define NAV_LOITER_P			2.0
+# define NAV_LOITER_P			2.5			// upped to be a bit more aggressive
 #endif
 #ifndef NAV_LOITER_I
-# define NAV_LOITER_I			0.1
+# define NAV_LOITER_I			0.15		// upped a bit to deal with wind faster
 #endif
 #ifndef NAV_LOITER_D
 # define NAV_LOITER_D			0.00
 #endif
 #ifndef NAV_LOITER_IMAX
-# define NAV_LOITER_IMAX		250			// 250 Lat and Longtitude
+# define NAV_LOITER_IMAX		20			// 20°
 #endif
 
-
 #ifndef NAV_WP_P
 # define NAV_WP_P				4.0
 #endif
 #ifndef NAV_WP_I
-# define NAV_WP_I				0.0
+# define NAV_WP_I				0.0			// leave 0
 #endif
 #ifndef NAV_WP_D
-# define NAV_WP_D				15
+# define NAV_WP_D				15			// not sure about at all
 #endif
 #ifndef NAV_WP_IMAX
 # define NAV_WP_IMAX			20			// 20 degrees
 #endif
 
-
 //////////////////////////////////////////////////////////////////////////////
 // Throttle control gains
 //

ArduCopterMega/navigation.pde

@@ -112,6 +112,20 @@ void calc_simple_nav()
 	nav_lat	*= g.pid_nav_lat.kP();	// 1800 * 2 = 3600 or 36°
 }
 
+void calc_nav_output()
+{
+	// get the sin and cos of the bearing error - rotated 90°
+	sin_nav_y 	= sin(radians((float)(9000 - bearing_error) / 100));
+	cos_nav_x 	= cos(radians((float)(bearing_error - 9000) / 100));
+
+	// rotate the vector
+	nav_roll 	=  (float)nav_lat * cos_nav_x;
+	nav_pitch 	= -(float)nav_lat * sin_nav_y;
+}
+#define WAYPOINT_SPEED 450
+
+#if NAV_TEST == 0
+
 void calc_rate_nav()
 {
 	// calc distance error
@@ -133,22 +147,10 @@ void calc_rate_nav()
 
 }
 
-void calc_nav_output()
-{
-	// get the sin and cos of the bearing error - rotated 90°
-	sin_nav_y 	= sin(radians((float)(9000 - bearing_error) / 100));
-	cos_nav_x 	= cos(radians((float)(bearing_error - 9000) / 100));
-
-	// rotate the vector
-	nav_roll 	=  (float)nav_lat * cos_nav_x;
-	nav_pitch 	= -(float)nav_lat * sin_nav_y;
-}
-
-#define WAYPOINT_SPEED 450
-
+#else
 
 // called after we get GPS read
-void calc_rate_nav2()
+void calc_rate_nav()
 {
 	// change to rate error
 	// we want to be going 450cm/s
@@ -175,6 +177,7 @@ void calc_rate_nav2()
 	nav_lat	= constrain(nav_lat, 	-3800, 3800); // +- 20m max error
 }
 
+#endif
 
 void calc_bearing_error()
 {
@@ -220,7 +223,7 @@ void update_crosstrack(void)
 {
 	// Crosstrack Error
 	// ----------------
-	if (cross_track_test() < 9000) {	 // If we are too far off or too close we don't do track following
+	if (cross_track_test() < 5000) {	 // If we are too far off or too close we don't do track following
 		crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance;	 // Meters we are off track line
 		nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
 		nav_bearing = wrap_360(nav_bearing);