Merge branch 'master' of https://code.google.com/p/ardupilot-mega

ArduCopter/APM_Config.h

@@ -69,6 +69,3 @@
 // enable this for the new 'APM2' hardware
 // #define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
 // #define APM2_BETA_HARDWARE  // for developers who received an early beta board with the older BMP085
-
-#define LOITER_METHOD 0
-// set to 1 to try an alternative Loiter control method

ArduCopter/ArduCopter.pde

@@ -1,6 +1,6 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "ArduCopter V2.1.1r3 alpha"
+#define THISFIRMWARE "ArduCopter V2.1.1r4 alpha"
 /*
 ArduCopter Version 2.0 Beta
 Authors:	Jason Short
@@ -366,6 +366,7 @@ static bool	nav_ok;
 static const float radius_of_earth 	= 6378100;		// meters
 static const float gravity 			= 9.81;			// meters/ sec^2
 static int32_t	target_bearing;						// deg * 100 : 0 to 360 location of the plane to the target
+static int32_t	nav_bearing;						// deg * 100 : 0 to 360 location of the plane to the target
 static int32_t 	home_bearing;						// used to track difference in angle
 
 static byte	wp_control;								// used to control - navgation or loiter
@@ -1390,8 +1391,6 @@ static void update_navigation()
 				// reset LOITER to current position
 				next_WP 	= current_loc;
 
-				// clear Loiter Iterms
-				reset_nav();
 			}else{
 				// this is also set by GPS in update_nav
 				wp_control = LOITER_MODE;
@@ -1762,7 +1761,8 @@ static void update_nav_wp()
 		// use error as the desired rate towards the target
 		calc_nav_rate(g.waypoint_speed_max);
 		// rotate pitch and roll to the copter frame of reference
-		calc_nav_pitch_roll();
+		//calc_nav_pitch_roll();
+		calc_loiter_pitch_roll();
 
 	}else if(wp_control == NO_NAV_MODE){
 		nav_roll = 0;
@@ -1772,11 +1772,16 @@ static void update_nav_wp()
 
 static void update_auto_yaw()
 {
+	// If we Loiter, don't start Yawing, allow Yaw control
+	if(wp_control == LOITER_MODE)
+		return;
+
 	// this tracks a location so the copter is always pointing towards it.
 	if(yaw_tracking == MAV_ROI_LOCATION){
 		auto_yaw = get_bearing(&current_loc, &target_WP);
 
 	}else if(yaw_tracking == MAV_ROI_WPNEXT){
+		// Point towards next WP
 		auto_yaw = target_bearing;
 	}
 	// MAV_ROI_NONE = basic Yaw hold

ArduCopter/Attitude.pde

@@ -243,8 +243,8 @@ static void reset_nav(void)
 	g.pi_nav_lon.reset_I();
 
 	// considering not reseting wind control
-	//g.pi_loiter_lat.reset_I();
+	g.pi_loiter_lat.reset_I();
-	//g.pi_loiter_lon.reset_I();
+	g.pi_loiter_lon.reset_I();
 
 	circle_angle			= 0;
 	crosstrack_error 		= 0;

ArduCopter/GCS_Mavlink.pde

@@ -116,12 +116,12 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
         chan,
         nav_roll / 1.0e2,
         nav_pitch / 1.0e2,
+        nav_bearing / 1.0e2,
         target_bearing / 1.0e2,
-        dcm.yaw_sensor / 1.0e2, // was target_bearing
         wp_distance,
         altitude_error / 1.0e2,
-        nav_lon,	// was 0
+        0,
-        nav_lat);	// was 0
+        crosstrack_error);	// was 0
 }
 
 static void NOINLINE send_gps_raw(mavlink_channel_t chan)

ArduCopter/Parameters.h

@@ -329,7 +329,7 @@ public:
 	waypoint_radius			(WP_RADIUS_DEFAULT,			k_param_waypoint_radius,				PSTR("WP_RADIUS")),
 	loiter_radius			(LOITER_RADIUS,	    		k_param_loiter_radius,					PSTR("WP_LOITER_RAD")),
 	waypoint_speed_max		(WAYPOINT_SPEED_MAX,		k_param_waypoint_speed_max,				PSTR("WP_SPEED_MAX")),
-	crosstrack_gain			(CROSSTRACK_GAIN,			k_param_crosstrack_gain,				PSTR("XTRK_GAIN_SC")),
+	crosstrack_gain			(CROSSTRACK_GAIN * 100,		k_param_crosstrack_gain,				PSTR("XTRK_GAIN_SC")),
 
 	throttle_min			(0,							k_param_throttle_min,					PSTR("THR_MIN")),
 	throttle_max			(1000, 						k_param_throttle_max,					PSTR("THR_MAX")),

ArduCopter/commands.pde

@@ -8,6 +8,9 @@ static void init_commands()
 	prev_nav_index 			= NO_COMMAND;
 	command_cond_queue.id 	= NO_COMMAND;
 	command_nav_queue.id 	= NO_COMMAND;
+
+	// default Yaw tracking
+	yaw_tracking 			= MAV_ROI_WPNEXT;
 }
 
 // Getters
@@ -176,6 +179,7 @@ static void set_next_WP(struct Location *wp)
 	wp_totalDistance 	= get_distance(&current_loc, &next_WP);
 	wp_distance 		= wp_totalDistance;
 	target_bearing 		= get_bearing(&prev_WP, &next_WP);
+	nav_bearing 		= target_bearing;
 
 	// to check if we have missed the WP
 	// ---------------------------------

ArduCopter/commands_process.pde

@@ -122,7 +122,7 @@ static void execute_nav_command(void)
 	process_nav_command();
 
 	// clear navigation prameters
-	reset_nav();
+	//reset_nav();
 
 	// clear May indexes to force loading of more commands
 	// existing May commands are tossed.

ArduCopter/config.h

@@ -508,7 +508,7 @@
 
 
 #ifndef STABILIZE_D
-# define STABILIZE_D 		.25
+# define STABILIZE_D 		.2
 #endif
 
 // Jasons default values that are good for smaller payload motors.
@@ -606,17 +606,17 @@
 // Navigation control gains
 //
 #ifndef LOITER_P
-# define LOITER_P			.2		// .3 was too aggressive
+# define LOITER_P			.25		//
 #endif
 #ifndef LOITER_I
-# define LOITER_I			0.05	// Wind control
+# define LOITER_I			0.1	// Wind control
 #endif
 #ifndef LOITER_IMAX
 # define LOITER_IMAX		30		// degrees°
 #endif
 
 #ifndef NAV_P
-# define NAV_P				1.5			// 3 was causing rapid oscillations in Loiter
+# define NAV_P				2.2			// 3 was causing rapid oscillations in Loiter
 #endif
 #ifndef NAV_I
 # define NAV_I				0.15		// used in traverals
@@ -649,7 +649,7 @@
 
 // RATE control
 #ifndef THROTTLE_P
-# define THROTTLE_P		0.4			//
+# define THROTTLE_P		0.5			//
 #endif
 #ifndef THROTTLE_I
 # define THROTTLE_I		0.0			//
@@ -663,7 +663,7 @@
 // Crosstrack compensation
 //
 #ifndef CROSSTRACK_GAIN
-# define CROSSTRACK_GAIN		40
+# define CROSSTRACK_GAIN		1
 #endif
 
 

ArduCopter/navigation.pde

@@ -22,6 +22,10 @@ static byte navigate()
 	target_bearing 	= get_bearing(&current_loc, &next_WP);
 	home_to_copter_bearing 	= get_bearing(&home, &current_loc);
 
+	// nav_bearing will includes xtrac correction
+	// ------------------------------------------
+	nav_bearing = target_bearing;
+
 	return 1;
 }
 
@@ -55,13 +59,16 @@ static void calc_XY_velocity(){
 	x_GPS_speed = (x_GPS_speed * 3 + x_diff) / 4;
 	y_GPS_speed = (y_GPS_speed * 3 + y_diff) / 4;
 
-	if(g_gps->ground_speed > 120){
+	// Above simply works better than GPS groundspeed
+	// which is proving to be problematic
+
+	/*if(g_gps->ground_speed > 120){
 		// Derive X/Y speed from GPS
 		// this is far more accurate when traveling about 1.5m/s
 		float temp		= g_gps->ground_course * RADX100;
 		x_GPS_speed 	= sin(temp) * (float)g_gps->ground_speed;
 		y_GPS_speed 	= cos(temp) * (float)g_gps->ground_speed;
-	}
+	}*/
 
 	last_longitude 	= g_gps->longitude;
 	last_latutude 	= g_gps->latitude;
@@ -110,7 +117,7 @@ static void calc_loiter(int x_error, int y_error)
 	nav_lat_p				= constrain(nav_lat_p, -3500, 3500);
 	nav_lat					= nav_lat_p + y_iterm;
 
-	///*
+	/*
 	int8_t ttt = 1.0/dTnav;
 	int16_t t2 = g.pi_nav_lat.get_integrator();
 
@@ -119,6 +126,7 @@ static void calc_loiter(int x_error, int y_error)
 					wp_distance, 	//1
 					y_error,		//2
 					y_GPS_speed,	//3
+
 					y_actual_speed,	//4  ;
 					y_target_speed,	//5
 					y_rate_error,	//6
@@ -152,16 +160,24 @@ static void calc_loiter(int x_error, int y_error)
 static void estimate_velocity()
 {
 	// we need to extimate velocity when below GPS threshold of 1.5m/s
-	if(g_gps->ground_speed < 150){
+	//if(g_gps->ground_speed < 120){
 		// some smoothing to prevent bumpy rides
 		x_actual_speed = (x_actual_speed * 15 + x_GPS_speed) / 16;
 		y_actual_speed = (y_actual_speed * 15 + y_GPS_speed) / 16;
 
-	}else{
+		// integration of nav_p angle
+		//x_actual_speed += (nav_lon_p >>2);
+		//y_actual_speed += (nav_lat_p >>2);
+
+		// this is just what worked best in SIM
+		//x_actual_speed = (x_actual_speed * 2 + x_GPS_speed * 1) / 4;
+		//y_actual_speed = (y_actual_speed * 2 + y_GPS_speed * 1) / 4;
+
+	//}else{
 		// less smoothing needed since the GPS already filters
-		x_actual_speed = (x_actual_speed * 3 + x_GPS_speed) / 4;
+	//	x_actual_speed = (x_actual_speed * 3 + x_GPS_speed) / 4;
-		y_actual_speed = (y_actual_speed * 3 + y_GPS_speed) / 4;
+	//	y_actual_speed = (y_actual_speed * 3 + y_GPS_speed) / 4;
-	}
+	//}
 }
 
 // this calculation rotates our World frame of reference to the copter's frame of reference
@@ -206,32 +222,47 @@ static void calc_nav_rate(int max_speed)
 		max_speed 		= min(max_speed, waypoint_speed_gov);
 	}
 
-	float temp 		= (target_bearing - g_gps->ground_course) * RADX100;
-
 	// push us towards the original track
 	update_crosstrack();
 
+	// nav_bearing includes crosstrack
+	float temp 		= (9000 - nav_bearing) * RADX100;
+
 	// heading laterally, we want a zero speed here
-	x_actual_speed 	= -sin(temp) * (float)g_gps->ground_speed;
+	//x_actual_speed 	= -sin(temp) * (float)g_gps->ground_speed;
-	x_rate_error 	= crosstrack_error -x_actual_speed;
+
+	x_rate_error 		= (cos(temp) * max_speed) - x_actual_speed; // 413
 	x_rate_error 		= constrain(x_rate_error, -1400, 1400);
-	nav_lon		 	= constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500);
+	int16_t x_iterm 	= g.pi_loiter_lon.get_i(x_rate_error, dTnav);
-	/*Serial.printf("max_sp %d,\tx_actual_sp %d,\tx_rate_err: %d, Xtrack %d, \tnav_lon: %d,\ty_actual_sp %d,\ty_rate_err: %d,\tnav_lat: %d,\n",
+
-					max_speed,
+	nav_lon_p		 	= g.pi_nav_lon.get_p(x_rate_error);
-					x_actual_speed,
+	nav_lon_p			= constrain(nav_lon_p, -3500, 3500);
-					x_rate_error,
+	nav_lon				= nav_lon_p + x_iterm;
-					crosstrack_error,
-					nav_lon,
-					y_actual_speed,
-					y_rate_error,
-					nav_lat);
-	//*/
 
 	// heading towards target
-	y_actual_speed 	= cos(temp) * (float)g_gps->ground_speed;
+	//y_actual_speed 	= cos(temp) * (float)g_gps->ground_speed;
-	y_rate_error 	= max_speed - y_actual_speed; // 413
+
+	y_rate_error 		= (sin(temp) * max_speed) - y_actual_speed; // 413
 	y_rate_error 		= constrain(y_rate_error, -1400, 1400);	// added a rate error limit to keep pitching down to a minimum
-	nav_lat		 	= constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500);
+	int16_t y_iterm 	= g.pi_loiter_lat.get_i(y_rate_error, dTnav);
+
+	nav_lat_p		 	= g.pi_nav_lat.get_p(y_rate_error);
+	nav_lat_p			= constrain(nav_lat_p, -3500, 3500);
+	nav_lat				= nav_lat_p + y_iterm;
+
+	/*
+	Serial.printf("max_sp %d,\t x_sp %d, y_sp %d,\t x_re: %d, y_re: %d, \tnav_lon: %d, nav_lat: %d, Xi:%d, Yi:%d, \t XE %d \n",
+					max_speed,
+					x_actual_speed,
+					y_actual_speed,
+					x_rate_error,
+					y_rate_error,
+					nav_lon,
+					nav_lat,
+					x_iterm,
+					y_iterm,
+					crosstrack_error);
+	//*/
 
 
 	// nav_lat and nav_lon will be rotated to the angle of the quad in calc_nav_pitch_roll()
@@ -246,53 +277,21 @@ static void calc_nav_rate(int max_speed)
 					nav_lat);*/
 }
 
+
 static void update_crosstrack(void)
 {
 	// Crosstrack Error
 	// ----------------
-	if (cross_track_test() < 4000) {	 // If we are too far off or too close we don't do track following
+	if (abs(wrap_180(target_bearing - original_target_bearing)) < 4500) {	 // If we are too far off or too close we don't do track following
 		float temp = (target_bearing - original_target_bearing) * RADX100;
 		crosstrack_error = sin(temp) * (wp_distance * g.crosstrack_gain);	 // Meters we are off track line
-		crosstrack_error = constrain(crosstrack_error, -1200, 1200);
+		nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000);
+		nav_bearing = wrap_360(nav_bearing);
+	}else{
+		nav_bearing = target_bearing;
 	}
 }
 
-// used to generate the offset angle for testing crosstrack viability
-static int32_t cross_track_test()
-{
-	int32_t temp;
-	temp 	= target_bearing - original_target_bearing;
-	temp 	= wrap_180(temp);
-	return abs(temp);
-}
-
-
-// this calculation is different than loiter above because we are in a different Frame of Reference.
-// nav_lat is pointed towards the target, where as in Loiter, nav_lat is pointed north!
-static void calc_nav_pitch_roll()
-{
-	int32_t angle = wrap_360(dcm.yaw_sensor - target_bearing);
-	float temp	 = (9000l - angle) * RADX100;
-	//t: 1.5465, t1: -10.9451, t2: 1.5359, t3: 1.5465
-	float _cos_yaw_x = cos(temp);
-	float _sin_yaw_y = sin(temp);
-
-	// rotate the vector
-	nav_roll 	=  (float)nav_lon * _sin_yaw_y - (float)nav_lat * _cos_yaw_x;
-	nav_pitch 	=  (float)nav_lon * _cos_yaw_x + (float)nav_lat * _sin_yaw_y;
-
-	// flip pitch because forward is negative
-	nav_pitch = -nav_pitch;
-
-	/*Serial.printf("Yaw %d, Tbear:%d, \tangle: %d, \t_cos_yaw_x:%1.4f, _sin_yaw_y:%1.4f, nav_roll:%d, nav_pitch:%d\n",
-					dcm.yaw_sensor,
-					target_bearing,
-					angle,
-					_cos_yaw_x,
-					_sin_yaw_y,
-					nav_roll,
-					nav_pitch);*/
-}
 
 static int32_t get_altitude_error()
 {