Yaw fix,

Logs updated to do better Yaw Debugging
Landing is now rate limited
updated command desctiption.

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

ArduCopterMega/ArduCopterMega.pde

@@ -375,6 +375,11 @@ long 	condition_value;					// used in condition commands (eg delay, change alt,
 long 	condition_start;
 int 	condition_rate;
 
+// land command
+// ------------
+long 	land_start;							// when we intiated command in millis()
+long 	original_alt;						// altitide reference for start of command
+
 // 3D Location vectors
 // -------------------
 struct 	Location home;						// home location
@@ -385,7 +390,6 @@ struct 	Location target_WP;					// where do we want to you towards?
 struct 	Location tell_command;				// command for telemetry
 struct 	Location next_command;				// command preloaded
 long 	target_altitude;					// used for
-//long 	offset_altitude;					// used for
 boolean	home_is_set; 						// Flag for if we have g_gps lock and have set the home location
 
 

ArduCopterMega/Attitude.pde

@@ -108,9 +108,9 @@ void
 output_yaw_with_hold(boolean hold)
 {
 	// rate control
-	int dampener;
 	long rate		= degrees(omega.z) * 100; 											// 3rad = 17188 , 6rad = 34377
 	rate			= constrain(rate, -36000, 36000);									// limit to something fun!
+	int dampener 	= rate * g.hold_yaw_dampener;											// 34377 * .175 = 6000
 
 	if(hold){
 		// look to see if we have exited rate control properly - ie stopped turning
@@ -147,7 +147,9 @@ output_yaw_with_hold(boolean hold)
 		// 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
 
-		yaw_debug = YAW_HOLD;
+		// add in yaw dampener
+		g.rc_4.servo_out	-= constrain(dampener, -1800, 1800);
+		yaw_debug = YAW_HOLD; //0
 
 	}else{
 																							// -error = CCW, 	+error = CW
@@ -155,21 +157,19 @@ output_yaw_with_hold(boolean hold)
 			// we are breaking;
 			//g.rc_4.servo_out = (omega.z > 0) ? -600 : 600;
 			// adaptive braking
-			g.rc_4.servo_out 	= (int)((1800.0 * omega.z) / 6.0);
+			g.rc_4.servo_out 	= (int)((-1800.0 * omega.z) / 1.5);
+								//	-1800 * 0.925 / 6 = -277
 
-			yaw_debug = YAW_BRAKE;
+			yaw_debug = YAW_BRAKE;  // 1
 
 		}else{
+			// RATE control
 			long error			= ((long)g.rc_4.control_in * 6) - rate;							// control is += 6000 * 6 = 36000
 			g.rc_4.servo_out 	= g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0);	// kP .07 * 36000 = 2520
-			yaw_debug = YAW_RATE;
+			yaw_debug = YAW_RATE;  // 2
 		}
 	}
 
-	// Limit dampening to be equal to propotional term for symmetry
-	dampener 			= rate * g.hold_yaw_dampener;											// 34377 * .175 = 6000
-	g.rc_4.servo_out	-= constrain(dampener, -1800, 1800);
-
 	// Limit Output
 	g.rc_4.servo_out 	= constrain(g.rc_4.servo_out, -1800, 1800);								// limit to 24°
 

ArduCopterMega/command description.txt

@@ -22,13 +22,12 @@ Command Structure in bytes
 Commands below MAV_CMD_NAV_LAST are commands that have a end criteria, eg "reached waypoint" or "reached altitude"
 ***********************************
 Command ID	Name								Parameter 1			Altitude			Latitude			Longitude
-0x10 / 16	MAV_CMD_NAV_WAYPOINT				delay (seconds)		altitude			lat					lon
+	0x10 / 16	MAV_CMD_NAV_WAYPOINT				delay (seconds)		altitude			lat					lon
 0x11 / 17	MAV_CMD_NAV_LOITER_UNLIM 			(forever)			altitude			lat					lon
-0x12 / 18	MAV_CMD_NAV_LOITER_TURNS			turns				altitude			lat					lon
+
-0x13 / 19	MAV_CMD_NAV_LOITER_TIME				-					time (seconds)		-					-
+0x14 / 20	MAV_CMD_NAV_RETURN_TO_LAUNCH		-					-					-					-
-0x14 / 20	MAV_CMD_NAV_RETURN_TO_LAUNCH		-					altitude			lat					lon
+0x15 / 21	MAV_CMD_NAV_LAND					-					-					-					-
-0x15 / 21	MAV_CMD_NAV_LAND					-					altitude			lat					lon
+0x16 / 22	MAV_CMD_NAV_TAKEOFF					-					altitude			-					-
-0x16 / 22	MAV_CMD_NAV_TAKEOFF					takeoff pitch		altitude			-					-
 0x17 / 23	MAV_CMD_NAV_TARGET					-					altitude			lat					lon
 
 			NOTE:  for command 0x16 the value takeoff pitch specifies the minimum pitch for the case with airspeed sensor and the target pitch for the case without.

ArduCopterMega/commands.pde

@@ -49,13 +49,13 @@ struct Location get_wp_with_index(int i)
 		temp.p1 = eeprom_read_byte((uint8_t*)mem);
 
 		mem++;
-		temp.alt = (long)eeprom_read_dword((uint32_t*)mem);	// alt is stored in CM!
+		temp.alt = (long)eeprom_read_dword((uint32_t*)mem);	// alt is stored in CM! Alt is stored relative!
 
 		mem += 4;
-		temp.lat = (long)eeprom_read_dword((uint32_t*)mem);
+		temp.lat = (long)eeprom_read_dword((uint32_t*)mem); // lat is stored in decimal * 10,000,000
 
 		mem += 4;
-		temp.lng = (long)eeprom_read_dword((uint32_t*)mem);
+		temp.lng = (long)eeprom_read_dword((uint32_t*)mem); // lon is stored in decimal * 10,000,000
 	}
 
 	// Add on home altitude if we are a nav command

ArduCopterMega/commands_logic.pde

@@ -246,18 +246,20 @@ void do_land()
 	Serial.println("dlnd ");
 	land_complete		= false;			// set flag to use g_gps ground course during TO.  IMU will be doing yaw drift correction
 	velocity_land		= 2000;
+	land_start 			= millis();
+	original_alt		= current_loc.alt;
 
-	Location temp		= current_loc;
+	//Location temp		= current_loc;
 	//temp.alt			= home.alt;
 	// just go down far
-	temp.alt			= -100000;
+	//temp.alt			= -100000;
 
-	set_next_WP(&temp);
+	set_next_WP(&current_loc);
 }
 
 void do_loiter_unlimited()
 {
-	Serial.println("dloi ");
+	//Serial.println("dloi ");
 	if(next_command.lat == 0)
 		set_next_WP(&current_loc);
 	else
@@ -310,12 +312,14 @@ bool verify_takeoff()
 
 bool verify_land()
 {
-	//Serial.print("vlnd ");
+	// land at 1 meter per second
+	next_WP.alt  = original_alt - ((millis() - land_start) / 10);			// condition_value = our initial
+
 	velocity_land  = ((old_alt - current_loc.alt) *.2) + (velocity_land * .8);
 	old_alt = current_loc.alt;
 
 	if(g.sonar_enabled){
-		// decide which sensor we're usings
+		// decide which sensor we're using
 		if(sonar_alt < 40){
 			land_complete = true;
 			Serial.println("Y");

ArduCopterMega/config.h

@@ -515,7 +515,6 @@
 #ifndef ALT_HOLD_HOME
 # define ALT_HOLD_HOME 8
 #endif
-#define ALT_HOLD_HOME_CM ALT_HOLD_HOME * 100
 
 #ifndef USE_CURRENT_ALT
 # define USE_CURRENT_ALT FALSE

ArduCopterMega/setup.pde

@@ -786,9 +786,10 @@ void report_wp(byte index = 255)
 
 void print_wp(struct Location *cmd, byte index)
 {
-	Serial.printf_P(PSTR("command #: %d id:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
+	Serial.printf_P(PSTR("command #: %d id:%d op:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
 		(int)index,
 		(int)cmd->id,
+		(int)cmd->options,
 		(int)cmd->p1,
 		cmd->alt,
 		cmd->lat,

ArduCopterMega/system.pde

@@ -105,7 +105,7 @@ void init_ardupilot()
 
 		Serial.printf_P(PSTR("Please Run Setup...\n"));
 		while (true) {
-			delay(100);
+			delay(1000);
 			if(motor_light){
 				digitalWrite(A_LED_PIN, HIGH);
 				digitalWrite(B_LED_PIN, HIGH);

ArduCopterMega/test.pde

@@ -735,59 +735,22 @@ test_xbee(uint8_t argc, const Menu::arg *argv)
 static int8_t
 test_altitude(uint8_t argc, const Menu::arg *argv)
 {
-	Serial.printf_P(PSTR("Uncalibrated relative airpressure\n"));
 	print_hit_enter();
 
-	home.alt = 0;
-	wp_distance = 0;
 	init_barometer();
-	reset_I();
-
-	// to prevent boost from skewing results
-	cos_pitch_x = cos_roll_x = 1;
 
 	while(1){
-		if (millis() - fast_loopTimer > 100) {
+		delay(100);
-			delta_ms_fast_loop 	= millis() - fast_loopTimer;
+
-			G_Dt 				= (float)delta_ms_fast_loop / 1000.f;		// used by DCM integrator
+		baro_alt 		= read_barometer();
-			fast_loopTimer		= millis();
+
-		}
+		if(g.sonar_enabled){
-
+			// decide which sensor we're usings
-		if (millis() - medium_loopTimer > 100) {
+			sonar_alt 		= sonar.read();
-			medium_loopTimer	= millis();
-
-			read_radio();			// read the radio first
-			next_WP.alt = home.alt + g.rc_6.control_in; // 0 - 2000 (20 meters)
-			next_WP.alt =  max(next_WP.alt, 30);
-
-			read_trim_switch();
-			update_alt();
-			output_auto_throttle();
-
-			Serial.printf_P(PSTR("B_alt: %ld, S_alt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \tint: %6.2f \tout:%d\n"),
-						baro_alt,
-						sonar_alt,
-						next_WP.alt,
-						altitude_error,
-						(int)g.throttle_cruise,
-						g.pid_baro_throttle.get_integrator(),
-						g.rc_3.servo_out);
-
-			/*
-			Serial.print("Altitude: ");
-			Serial.print((int)current_loc.alt,DEC);
-			Serial.print("\tnext_alt: ");
-			Serial.print((int)next_WP.alt,DEC);
-			Serial.print("\talt_err: ");
-			Serial.print((int)altitude_error,DEC);
-			Serial.print("\ttNom: ");
-			Serial.print(g.,DEC);
-			Serial.print("\ttOut: ");
-			Serial.println(g.rc_3.servo_out,DEC);
-			*/
-			//Serial.print("    Raw pressure value: ");
-			//Serial.println(abs_pressure);
 		}
+		Serial.printf_P(PSTR("B_alt: %ld, S_alt: %ld\n"),
+					baro_alt,
+					sonar_alt);
 
 		if(Serial.available() > 0){
 			return (0);