Navigation bug fixes, tuning.

better handling of sonar at angles and baro mixing. - optimized for flats.
Crosstrack test bug found and fixed.

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

ArduCopterMega/APM_Config.h

@@ -11,15 +11,15 @@
 //#define GCS_PROTOCOL        GCS_PROTOCOL_NONE
 //#define GCS_PORT 0
 
-#define SERIAL0_BAUD			38400
+//#define SERIAL0_BAUD			38400
 
 #define STABILIZE_ROLL_P 		0.75
 #define STABILIZE_PITCH_P		0.75
 //#define STABILIZE_DAMPENER		0.1
 
 
-#define CHANNEL_6_TUNING CH6_NONE
-//#define CHANNEL_6_TUNING CH6_STABLIZE_KP
+//#define CHANNEL_6_TUNING CH6_NONE
+#define CHANNEL_6_TUNING CH6_PMAX
 
 	/*
 	CH6_NONE

ArduCopterMega/ArduCopterMega.pde

@@ -244,7 +244,7 @@ boolean timer_light;						// status of the Motor safety
 const 	float t7			= 10000000.0;	// used to scale GPS values for EEPROM storage
 float 	scaleLongUp			= 1;			// used to reverse longtitude scaling
 float 	scaleLongDown 		= 1;			// used to reverse longtitude scaling
-byte 	ground_start_count	= 5;			// have we achieved first lock and set Home?
+byte 	ground_start_count	= 10;			// have we achieved first lock and set Home?
 
 // Location & Navigation
 // ---------------------
@@ -308,8 +308,9 @@ int 			ground_temperature;
 
 // Altitude Sensor variables
 // ----------------------
-long	sonar_alt;
-long	baro_alt;
+int		sonar_alt;
+int		baro_alt;
+int		baro_alt_offset;
 byte 	altitude_sensor = BARO;				// used to know which sensor is active, BARO or SONAR
 
 // flight mode specific
@@ -1108,29 +1109,46 @@ void update_alt()
 #if HIL_MODE == HIL_MODE_ATTITUDE
 	current_loc.alt = g_gps->altitude;
 #else
-	altitude_sensor = BARO;
+	if(g.sonar_enabled){
+		// read barometer
 		baro_alt 		= read_barometer();
-	//Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt);
 
-	if(g.sonar_enabled){
-		// decide which sensor we're usings
-		sonar_alt 		= sonar.read();
+		//filter out bad sonar reads
+		int temp 		= sonar.read();
 
-		if(baro_alt < 500 && sonar_alt < 600){  // less than 5m or 15 feet
+		if(abs(temp - sonar_alt) < 300){
+			sonar_alt = temp;
+		}
+
+		// correct alt for angle of the sonar
+		sonar_alt = (float)sonar_alt * (cos_pitch_x * cos_roll_x);
+
+		// output a light to show sonar is working
+		update_sonar_light(sonar_alt > 100);
+
+		// decide which sensor we're usings
+		if(sonar_alt < 500){  // less than 5m or 15 feet
 			altitude_sensor = SONAR;
+
+			// XXX this is a hack for now. it kills accuracy from GPS reading of altitude and focuses
+			// on altitude above flat ground.
+			baro_alt_offset = sonar_alt - baro_alt;
+
 		}else{
 			altitude_sensor = BARO;
 		}
 
-		//altitude_sensor = (target_altitude > (home.alt + 500)) ? BARO : SONAR;
-
+		// calculate our altitude
 		if(altitude_sensor == BARO){
-			current_loc.alt = baro_alt + home.alt;
+			current_loc.alt = baro_alt + baro_alt_offset + home.alt;
 		}else{
 			current_loc.alt = sonar_alt + home.alt;
 		}
 
 	}else{
+
+		altitude_sensor = BARO;
+		baro_alt 		= read_barometer();
 		// no sonar altitude
 		current_loc.alt = baro_alt + home.alt;
 	}
@@ -1197,5 +1215,9 @@ void tuning(){
 
 	#elif CHANNEL_6_TUNING == CH6_Y6_SCALING
 		Y6_scaling = (float)g.rc_6.control_in / 1000.0;
+
+	#elif CHANNEL_6_TUNING == CH6_PMAX
+		g.pitch_max.set(g.rc_6.control_in * 2);  // 0 to 2000
+
 	#endif
 }

ArduCopterMega/Attitude.pde

@@ -144,13 +144,13 @@ output_yaw_with_hold(boolean hold)
 		yaw_error 				= wrap_180(yaw_error);
 
 		// limit the error we're feeding to the PID
-		yaw_error				= constrain(yaw_error,	 -6000, 6000);						// limit error to 60 degees
+		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); 		// .5 * 6000 = 3000
+		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, -2400, 2400);
+		g.rc_4.servo_out		-= constrain(dampener, -1600, 1600);
 		yaw_debug 				= YAW_HOLD; //0
 
 	}else{
@@ -239,15 +239,16 @@ void calc_nav_throttle()
 	float scaler = 1.0;
 
 	if(error < 0){
-		scaler = (altitude_sensor == BARO) ? .5 : .5;
+		// try and prevent rapid fall
+		scaler = (altitude_sensor == BARO) ? .3 : .3;
 	}
 
 	if(altitude_sensor == BARO){
 		nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, scaler);	// .25
-		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 80);
+		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 120);
 	}else{
 		nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, scaler);	// .5
-		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -60, 100);
+		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -50, 150);
 	}
 
 	nav_throttle = (nav_throttle + nav_throttle_old) >> 1;
@@ -259,7 +260,7 @@ void calc_nav_throttle()
 float angle_boost()
 {
 	float temp = cos_pitch_x * cos_roll_x;
-	temp = 2.0 - constrain(temp, .7, 1.0);
+	temp = 2.0 - constrain(temp, .5, 1.0);
 	return temp;
 }
 

ArduCopterMega/Log.pde

@@ -20,6 +20,9 @@ static int8_t	select_logs(uint8_t argc, 		const Menu::arg *argv);
 // printf_P is a version of print_f that reads from flash memory
 static int8_t	help_log(uint8_t argc, 			const Menu::arg *argv)
 {
+	// log hack
+	//Serial.begin(115200, 128, 128);
+
 	Serial.printf_P(PSTR("\n"
 						 "Commands:\n"
 						 "  dump <n>"
@@ -380,26 +383,28 @@ void Log_Write_Nav_Tuning()
 	DataFlash.WriteInt((int)(target_bearing/100));		// 3
 	DataFlash.WriteInt((int)(nav_bearing/100));			// 4
 
-	DataFlash.WriteByte(altitude_sensor);				// 5
-	DataFlash.WriteInt((int)sonar_alt);					// 6
-	DataFlash.WriteInt((int)baro_alt);					// 7
+	DataFlash.WriteInt((int)(g.rc_3.servo_out));		// 5
+	DataFlash.WriteByte(altitude_sensor);				// 6
+	DataFlash.WriteInt((int)sonar_alt);					// 7
+	DataFlash.WriteInt((int)baro_alt);					// 8
 
-	DataFlash.WriteInt(home.alt);						// 8
-	DataFlash.WriteInt((int)next_WP.alt);				// 9
-	DataFlash.WriteInt((int)altitude_error);			// 10
+	DataFlash.WriteInt(home.alt);						// 9
+	DataFlash.WriteInt((int)next_WP.alt);				// 11
+	DataFlash.WriteInt((int)altitude_error);			// 11
 
 	DataFlash.WriteByte(END_BYTE);
 }
 
 void Log_Read_Nav_Tuning()
 {
-							 //	1	2	3	4	5	6	7	8	9	10
-	Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"),
+							 //	1	2	3	4	5	6	7	8	9	10  11
+	Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"),
 				DataFlash.ReadInt(),	//yaw sensor
 				DataFlash.ReadInt(),	//distance
 				DataFlash.ReadInt(),	//target bearing
 				DataFlash.ReadInt(),	//nav bearing
 
+				DataFlash.ReadInt(),	//throttle
 				DataFlash.ReadByte(),	//Alt sensor
 				DataFlash.ReadInt(),	//Sonar
 				DataFlash.ReadInt(),	//Baro

ArduCopterMega/commands.pde

@@ -189,6 +189,7 @@ void set_next_WP(struct Location *wp)
 	wp_totalDistance 	= get_distance(&current_loc, &next_WP);
 	wp_distance 		= wp_totalDistance;
 	target_bearing 		= get_bearing(&current_loc, &next_WP);
+	nav_bearing 		= target_bearing;
 
 	// to check if we have missed the WP
 	// ----------------------------
@@ -196,7 +197,7 @@ void set_next_WP(struct Location *wp)
 
 	// set a new crosstrack bearing
 	// ----------------------------
-	reset_crosstrack();
+	crosstrack_bearing 	= target_bearing;	// Used for track following
 
 	gcs.print_current_waypoints();
 }

ArduCopterMega/config.h

@@ -343,13 +343,13 @@
 // YAW Control
 //
 #ifndef  YAW_P
-# define YAW_P					0.5		// increase for more aggressive Yaw Hold, decrease if it's bouncy
+# define YAW_P					0.4		// increase for more aggressive Yaw Hold, decrease if it's bouncy
 #endif
 #ifndef  YAW_I
 # define YAW_I					0.0			// Always 0
 #endif
 #ifndef  YAW_D
-# define YAW_D					0.05		// Trying a lower value to prevent odd behavior
+# define YAW_D					0.13		// Trying a lower value to prevent odd behavior
 #endif
 #ifndef  YAW_IMAX
 # define YAW_IMAX				0			//	Always 0
@@ -385,13 +385,13 @@
 // Throttle control gains
 //
 #ifndef THROTTLE_BARO_P
-# define THROTTLE_BARO_P		0.25
+# define THROTTLE_BARO_P		0.4
 #endif
 #ifndef THROTTLE_BARO_I
-# define THROTTLE_BARO_I		0.04
+# define THROTTLE_BARO_I		0.1
 #endif
 #ifndef THROTTLE_BARO_D
-# define THROTTLE_BARO_D		0.0	// lowered to 0 to debug effects
+# define THROTTLE_BARO_D		0.03	// lowered to 0 to debug effects
 #endif
 #ifndef THROTTLE_BARO_IMAX
 # define THROTTLE_BARO_IMAX		50
@@ -399,13 +399,13 @@
 
 
 #ifndef THROTTLE_SONAR_P
-# define THROTTLE_SONAR_P		.65		// upped a hair from .5
+# define THROTTLE_SONAR_P		.8		// upped a hair from .5
 #endif
 #ifndef THROTTLE_SONAR_I
-# define THROTTLE_SONAR_I		0.1
+# define THROTTLE_SONAR_I		0.4
 #endif
 #ifndef THROTTLE_SONAR_D
-# define THROTTLE_SONAR_D		0.03
+# define THROTTLE_SONAR_D		0.15
 #endif
 #ifndef THROTTLE_SONAR_IMAX
 # define THROTTLE_SONAR_IMAX	50
@@ -416,10 +416,10 @@
 // Crosstrack compensation
 //
 #ifndef XTRACK_GAIN
-# define XTRACK_GAIN          1 // deg/m
+# define XTRACK_GAIN          5 // deg/m
 #endif
 #ifndef XTRACK_ENTRY_ANGLE
-# define XTRACK_ENTRY_ANGLE   30 // deg
+# define XTRACK_ENTRY_ANGLE   40 // deg
 #endif
 
 

ArduCopterMega/defines.h

@@ -111,6 +111,7 @@
 #define CH6_SONAR_KP 5
 #define CH6_SONAR_KD 6
 #define CH6_Y6_SCALING 7
+#define CH6_PMAX 8
 
 // nav byte mask
 // -------------

ArduCopterMega/navigation.pde

@@ -199,13 +199,22 @@ void update_crosstrack(void)
 {
 	// Crosstrack Error
 	// ----------------
-	if (abs(target_bearing - crosstrack_bearing) < 4500) {	 // If we are too far off or too close we don't do track following
+	if (cross_track_test() < 9000) {	 // 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);
+	}else{
+		reset_crosstrack();
 	}
 }
 
+long cross_track_test()
+{
+	long temp 	= target_bearing - crosstrack_bearing;
+	temp 		= wrap_180(temp);
+	return abs(temp);
+}
+
 void reset_crosstrack()
 {
 	crosstrack_bearing 	= get_bearing(&current_loc, &next_WP);	// Used for track following

ArduCopterMega/setup.pde

@@ -42,7 +42,7 @@ int8_t
 setup_mode(uint8_t argc, const Menu::arg *argv)
 {
 	// Give the user some guidance
-	Serial.printf_P(PSTR("Setup Mode\n"));
+	Serial.printf_P(PSTR("Setup Mode\n\n\n"));
 						 //"\n"
 						 //"IMPORTANT: if you have not previously set this system up, use the\n"
 						 //"'reset' command to initialize the EEPROM to sensible default values\n"

ArduCopterMega/system.pde

@@ -419,7 +419,7 @@ void update_motor_light(void)
 	}
 }
 
-void update_timer_light(bool light)
+void update_sonar_light(bool light)
 {
 	if(light == true){
 		digitalWrite(B_LED_PIN, HIGH);