updates to get alt hold from 2.0.7.

Cleanups, better logs for Alt hold, yaw debugging
I tuned up Hein's Hexa Yaw. I think we may need to adjust Yaw gains based on Frame type.
Crossing fingers, the next rev is the final.

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

ArduCopterMega/ArduCopterMega.pde

@@ -272,7 +272,7 @@ float cos_roll_x 	= 1;
 float cos_pitch_x 	= 1;
 float cos_yaw_x 	= 1;
 float sin_pitch_y, sin_yaw_y, sin_roll_y;
-float sin_nav_y, cos_nav_x;					// used in calc_waypoint_nav
+float sin_nav_y, cos_nav_x;					// used in calc_nav_output XXX move to local funciton
 bool simple_bearing_is_set = false;
 long initial_simple_bearing;				// used for Simple mode
 
@@ -922,20 +922,11 @@ void update_current_flight_mode(void)
 		}
 
 		switch(command_must_ID){
-			//case MAV_CMD_NAV_TAKEOFF:
-			//	break;
-
-			//case MAV_CMD_NAV_LAND:
-			//	break;
-
 			default:
 				// Output Pitch, Roll, Yaw and Throttle
 				// ------------------------------------
 				auto_yaw();
 
-				//if(invalid_nav)
-					//calc_waypoint_nav();
-
 				// mix in user control
 				control_nav_mixer();
 
@@ -1107,6 +1098,13 @@ void update_current_flight_mode(void)
 
 				adjust_altitude();
 
+				#if AUTO_RESET_LOITER == 1
+				if((g.rc_2.control_in + g.rc_1.control_in) != 0){
+					// reset LOITER to current position
+					next_WP = current_loc;
+				}
+				#endif
+
 				// Output Pitch, Roll, Yaw and Throttle
 				// ------------------------------------
 
@@ -1136,66 +1134,51 @@ void update_current_flight_mode(void)
 	}
 }
 
-void update_nav_wp()
-{
-	if(wp_control == LOITER_MODE){
-		// calc a pitch to the target
-		calc_loiter_nav();
-
-		// rotate pitch and roll to the copter frame of reference
-		calc_loiter_output();
-
-	} else {
-		// how far are we from the ideal trajectory?
-		// this pushes us back on course
-		update_crosstrack();
-
-		// calc a rate dampened pitch to the target
-		calc_rate_nav();
-
-		// rotate that pitch to the copter frame of reference
-		calc_nav_output();
-	}
-
-}
-
 // called after a GPS read
 void update_navigation()
 {
 	// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
 	// ------------------------------------------------------------------------
+	switch(control_mode){
+		case AUTO:
+			verify_commands();
 
-	// distance and bearing calcs only
+			// note: wp_control is handled by commands_logic
-	if(control_mode == AUTO){
-		verify_commands();
 
-		update_nav_wp();
+			// calculates desired Yaw
+			update_nav_yaw();
 
-		// this tracks a location so the copter is always pointing towards it.
+			// calculates the desired Roll and Pitch
-		if(yaw_tracking == MAV_ROI_LOCATION){
+			update_nav_wp();
-			nav_yaw = get_bearing(&current_loc, &target_WP);
+			break;
 
-		}else if(yaw_tracking == MAV_ROI_WPNEXT){
+		case LOITER:
-			nav_yaw = target_bearing;
+		case RTL:
-		}
+			// are we Traversing or Loitering?
+			wp_control = (wp_distance < 10) ? LOITER_MODE : WP_MODE;
 
-	}else{
+			// calculates desired Yaw
+			update_nav_yaw();
 
-		switch(control_mode){
+			// calculates the desired Roll and Pitch
-			case LOITER:
+			update_nav_wp();
-				wp_control = LOITER_MODE;
+			break;
-				update_nav_wp();
+
-				break;
+		#if YAW_HACK == 1
+		case SIMPLE:
+			// calculates desired Yaw
+			// exprimental_hack
+			if(g.rc_6.control_in > 900)
+				update_nav_yaw();
+			if(g.rc_6.control_in < 100){
+				nav_yaw = dcm.yaw_sensor;
+			}
+			break;
+		#endif
 
-			case RTL:
-				wp_control = (wp_distance < 700) ? LOITER_MODE : WP_MODE;
-				update_nav_wp();
-				break;
-		}
 	}
 }
 
-
 void read_AHRS(void)
 {
 	// Perform IMU calculations and get attitude info
@@ -1337,3 +1320,35 @@ void tuning(){
 	#endif
 }
 
+void update_nav_wp()
+{
+	if(wp_control == LOITER_MODE){
+		// calc a pitch to the target
+		calc_loiter_nav();
+
+		// rotate pitch and roll to the copter frame of reference
+		calc_loiter_output();
+
+	} else {
+		// how far are we from the ideal trajectory?
+		// this pushes us back on course
+		update_crosstrack();
+
+		// calc a rate dampened pitch to the target
+		calc_rate_nav();
+
+		// rotate that pitch to the copter frame of reference
+		calc_nav_output();
+	}
+}
+
+void update_nav_yaw()
+{
+	// this tracks a location so the copter is always pointing towards it.
+	if(yaw_tracking == MAV_ROI_LOCATION){
+		nav_yaw = get_bearing(&current_loc, &target_WP);
+
+	}else if(yaw_tracking == MAV_ROI_WPNEXT){
+		nav_yaw = target_bearing;
+	}
+}

ArduCopterMega/Attitude.pde

@@ -132,12 +132,12 @@ void calc_nav_throttle()
 
 	if(error < 0){
 		// try and prevent rapid fall
-		scaler = (altitude_sensor == BARO) ? .5 : .9;
+		//scaler = (altitude_sensor == BARO) ? 1 : 1;
 	}
 
 	if(altitude_sensor == BARO){
 		nav_throttle = g.pid_baro_throttle.get_pid(error, dTnav2, scaler);	// .25
-		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -50, 100);
+		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -70, 140);
 	}else{
 		nav_throttle = g.pid_sonar_throttle.get_pid(error, dTnav2, scaler);	// .5
 		nav_throttle = g.throttle_cruise + constrain(nav_throttle, -70, 150);
@@ -260,7 +260,7 @@ output_yaw_with_hold(boolean hold)
 		}else{
 			// RATE control
 																// Hein, 5/21/11
-			long error			= ((long)g.rc_4.control_in * 6) - (rate * 3);					// control is += 6000 * 6 = 36000
+			long error			= ((long)g.rc_4.control_in * 6) - (rate * 2);					// 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;  // 2
 		}

ArduCopterMega/Log.pde

@@ -52,26 +52,21 @@ print_log_menu(void)
 	byte last_log_num = get_num_logs();
 
 	Serial.printf_P(PSTR("logs enabled: "));
+
 	if (0 == g.log_bitmask) {
 		Serial.printf_P(PSTR("none"));
 	}else{
-		// Macro to make the following code a bit easier on the eye.
+		if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)	Serial.printf_P(PSTR(" ATTITUDE_FAST"));
-		// Pass it the capitalised name of the log option, as defined
+		if (g.log_bitmask & MASK_LOG_ATTITUDE_MED)	Serial.printf_P(PSTR(" ATTITUDE_MED"));
-		// in defines.h but without the LOG_ prefix.  It will check for
+		if (g.log_bitmask & MASK_LOG_GPS)			Serial.printf_P(PSTR(" GPS"));
-		// the bit being set and print the name of the log option to suit.
+		if (g.log_bitmask & MASK_LOG_PM)			Serial.printf_P(PSTR(" PM"));
-		#define PLOG(_s)	if (g.log_bitmask & MASK_LOG_ ## _s) Serial.printf_P(PSTR(" %S"), PSTR(#_s))
+		if (g.log_bitmask & MASK_LOG_CTUN)			Serial.printf_P(PSTR(" CTUN"));
-		PLOG(ATTITUDE_FAST);
+		if (g.log_bitmask & MASK_LOG_NTUN)			Serial.printf_P(PSTR(" NTUN"));
-		PLOG(ATTITUDE_MED);
+		if (g.log_bitmask & MASK_LOG_RAW)			Serial.printf_P(PSTR(" RAW"));
-		PLOG(GPS);
+		if (g.log_bitmask & MASK_LOG_CMD)			Serial.printf_P(PSTR(" CMD"));
-		PLOG(PM);
+		if (g.log_bitmask & MASK_LOG_CURRENT)		Serial.printf_P(PSTR(" CURRENT"));
-		PLOG(CTUN);
-		PLOG(NTUN);
-		PLOG(MODE);
-		PLOG(RAW);
-		PLOG(CMD);
-		PLOG(CURRENT);
-		#undef PLOG
 	}
+
 	Serial.println();
 
 	if (last_log_num == 0) {
@@ -108,13 +103,13 @@ dump_log(uint8_t argc, const Menu::arg *argv)
 	}
 
 	get_log_boundaries(last_log_num, dump_log, dump_log_start, dump_log_end);
-	Serial.printf_P(PSTR("Dumping Log number %d,    start %d,   end %d\n"),
+	/*Serial.printf_P(PSTR("Dumping Log number %d,    start %d,   end %d\n"),
 				  dump_log,
 				  dump_log_start,
 				  dump_log_end);
-
+	*/
 	Log_Read(dump_log_start, dump_log_end);
-	Serial.printf_P(PSTR("Done\n"));
+	//Serial.printf_P(PSTR("Done\n"));
 }
 
 static int8_t
@@ -354,74 +349,6 @@ int find_last_log_page(int bottom_page)
 	return top_page;
 }
 
-void Log_Write_Nav_Tuning()
-{
-	Matrix3f tempmat = dcm.get_dcm_matrix();
-
-	DataFlash.WriteByte(HEAD_BYTE1);
-	DataFlash.WriteByte(HEAD_BYTE2);
-	DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
-
-	DataFlash.WriteInt((int)(dcm.yaw_sensor/100));		// 1
-	DataFlash.WriteInt((int)wp_distance);				// 2
-	DataFlash.WriteByte(wp_verify_byte);				// 3
-	DataFlash.WriteInt((int)(target_bearing/100));		// 4
-	DataFlash.WriteInt((int)(nav_bearing/100));			// 5
-
-	DataFlash.WriteInt((int)(g.rc_3.servo_out));		// 6
-	DataFlash.WriteByte(altitude_sensor);				// 7
-	DataFlash.WriteInt((int)sonar_alt);					// 8
-	DataFlash.WriteInt((int)baro_alt);					// 9
-
-	DataFlash.WriteInt((int)home.alt);					// 10
-	DataFlash.WriteInt((int)next_WP.alt);				// 11
-	DataFlash.WriteInt((int)altitude_error);			// 12
-
-	DataFlash.WriteInt((int)(wrap_360(ToDeg(compass.heading)*100)/100)); // Just a temp hack
-	DataFlash.WriteLong(compass.last_update); // Just a temp hack
-	DataFlash.WriteInt((int)(tempmat.b.x*1000)); // Just a temp hack
-	DataFlash.WriteInt((int)(compass.heading_x*1000)); // Just a temp hack
-	DataFlash.WriteInt((int)(tempmat.a.x*1000)); // Just a temp hack
-	DataFlash.WriteInt((int)(compass.heading_y*1000)); // Just a temp hack
-
-	DataFlash.WriteByte(END_BYTE);
-}
-    //	1	2	3	4  5  6	 7	 8	   9	  10   11
-//NTUN, 236, 0, 132, 10, 0, 0, 29, 2963, 16545, 16682, 108
-
-void Log_Read_Nav_Tuning()
-{
-							 //	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, %d, "
-								"%d, %ld, %4.4f, %4.4f, %4.4f, %4.4f\n"),
-				DataFlash.ReadInt(),	//yaw sensor
-				DataFlash.ReadInt(),	//distance
-				DataFlash.ReadByte(),	//bitmask
-
-				DataFlash.ReadInt(),	//target bearing
-				DataFlash.ReadInt(),	//nav bearing
-
-				DataFlash.ReadInt(),	//throttle
-				DataFlash.ReadByte(),	//Alt sensor
-				DataFlash.ReadInt(),	//Sonar
-				DataFlash.ReadInt(),	//Baro
-
-				DataFlash.ReadInt(),	//home.alt
-				DataFlash.ReadInt(),	//Next_alt
-				DataFlash.ReadInt(),	//Alt Error
-
-				DataFlash.ReadInt(),
-				DataFlash.ReadLong(),
-				(float)DataFlash.ReadInt()/1000,
-				(float)DataFlash.ReadInt()/1000,
-				(float)DataFlash.ReadInt()/1000,
-				(float)DataFlash.ReadInt()/1000);
-}
-
-
 // Write an GPS packet. Total length : 30 bytes
 void Log_Write_GPS()
 {
@@ -516,6 +443,31 @@ void Log_Read_Current()
 			DataFlash.ReadInt());
 }
 
+void Log_Write_Nav_Tuning()
+{
+	Matrix3f tempmat = dcm.get_dcm_matrix();
+
+	DataFlash.WriteByte(HEAD_BYTE1);
+	DataFlash.WriteByte(HEAD_BYTE2);
+	DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
+
+	DataFlash.WriteInt((int)wp_distance);				// 1
+	DataFlash.WriteByte(wp_verify_byte);				// 2
+	DataFlash.WriteInt((int)(target_bearing/100));		// 3
+	DataFlash.WriteInt((int)(nav_bearing/100));			// 4
+
+	DataFlash.WriteByte(END_BYTE);
+}
+
+void Log_Read_Nav_Tuning()
+{
+							 //	 1	 2	 3	 4
+	Serial.printf_P(PSTR( "NTUN, %d, %d, %d, %d\n"),
+				DataFlash.ReadInt(),	//distance
+				DataFlash.ReadByte(),	//bitmask
+				DataFlash.ReadInt(),	//target bearing
+				DataFlash.ReadInt());	//nav bearing
+}
 
 // Write a control tuning packet. Total length : 22 bytes
 #if HIL_MODE != HIL_MODE_ATTITUDE
@@ -526,29 +478,24 @@ void Log_Write_Control_Tuning()
 	DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
 
 	// Control
-	DataFlash.WriteInt((int)(g.rc_3.control_in));
-	DataFlash.WriteInt((int)(g.rc_3.servo_out));
 	DataFlash.WriteInt((int)(g.rc_4.control_in));
 	DataFlash.WriteInt((int)(g.rc_4.servo_out));
 
 	// yaw
 	DataFlash.WriteByte(yaw_debug);
-	DataFlash.WriteInt((int)yaw_error);
 	DataFlash.WriteInt((int)(dcm.yaw_sensor/100));
+	DataFlash.WriteInt((int)(nav_yaw/100));
+	DataFlash.WriteInt((int)yaw_error);
 	DataFlash.WriteInt((int)(omega.z * 1000));
 
-	// Pitch/roll
+	// Alt hold
-	DataFlash.WriteInt((int)(dcm.pitch_sensor/100));
+	DataFlash.WriteInt((int)(g.rc_3.servo_out));
-	DataFlash.WriteInt((int)(dcm.roll_sensor/100));
+	DataFlash.WriteInt((int)sonar_alt);					//
+	DataFlash.WriteInt((int)baro_alt);					//
 
-	DataFlash.WriteInt((int)g.throttle_cruise);
+	DataFlash.WriteInt((int)next_WP.alt);				//
+	DataFlash.WriteInt((int)altitude_error);			//
 	DataFlash.WriteInt((int)g.pid_baro_throttle.get_integrator());
-	DataFlash.WriteInt((int)g.pid_sonar_throttle.get_integrator());
-
-	// Position
-	//DataFlash.WriteInt((int)dcm.pitch_sensor);
-	//DataFlash.WriteInt((int)dcm.roll_sensor);
-	//DataFlash.WriteInt((int)(dcm.yaw_sensor/10));
 
 	DataFlash.WriteByte(END_BYTE);
 }
@@ -557,28 +504,26 @@ void Log_Write_Control_Tuning()
 // Read an control tuning packet
 void Log_Read_Control_Tuning()
 {
-	Serial.printf_P(PSTR(   "CTUN, %d, %d, %d, %d, "
+	Serial.printf_P(PSTR(   "CTUN, %d, %d, "
-				"%d, %d, %d, %1.4f, "
+				"%d, %d, %d, %d, %1.4f, "
-				"%d, %d, "
+				"%d, %d, %d, %d, %d, %d\n"),
-				"%d, %d, %d\n"),
 
 				// Control
 				DataFlash.ReadInt(),
 				DataFlash.ReadInt(),
-				DataFlash.ReadInt(),
-				DataFlash.ReadInt(),
 
 				// yaw
 				(int)DataFlash.ReadByte(),
 				DataFlash.ReadInt(),
 				DataFlash.ReadInt(),
+				DataFlash.ReadInt(),
 				(float)DataFlash.ReadInt() / 1000.0,// Gyro Rate
 
-				// Pitch/roll
+				// Alt Hold
+				DataFlash.ReadInt(),
 				DataFlash.ReadInt(),
 				DataFlash.ReadInt(),
 
-				// Alt Hold
 				DataFlash.ReadInt(),
 				DataFlash.ReadInt(),
 				DataFlash.ReadInt());
@@ -700,6 +645,7 @@ void Log_Write_Mode(byte mode)
 	DataFlash.WriteByte(HEAD_BYTE2);
 	DataFlash.WriteByte(LOG_MODE_MSG);
 	DataFlash.WriteByte(mode);
+	DataFlash.WriteInt((int)g.throttle_cruise);
 	DataFlash.WriteByte(END_BYTE);
 }
 
@@ -707,7 +653,8 @@ void Log_Write_Mode(byte mode)
 void Log_Read_Mode()
 {
 	Serial.printf_P(PSTR("MOD:"));
-	Serial.println(flight_mode_strings[DataFlash.ReadByte()]);
+	Serial.print(flight_mode_strings[DataFlash.ReadByte()]);
+	Serial.printf_P(PSTR(", %d\n"),DataFlash.ReadInt());
 }
 
 // Read a raw accel/gyro packet

ArduCopterMega/commands.pde

@@ -227,6 +227,9 @@ void init_home()
 	home.alt 	= max(g_gps->altitude, 0);		// we sometimes get negatives from GPS, not valid
 	home_is_set = true;
 
+	// to point yaw towards home until we set it with Mavlink
+	target_WP 	= home;
+
 	//Serial.printf_P(PSTR("gps alt: %ld\n"), home.alt);
 
 	// Save Home to EEPROM

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.8 Beta", main_menu_commands);
+MENU(main_menu, "AC 2.0.9 Beta", main_menu_commands);
 
 void init_ardupilot()
 {

ArduCopterMega/test.pde

@@ -518,6 +518,7 @@ test_gps(uint8_t argc, const Menu::arg *argv)
 					g_gps->longitude,
 					g_gps->altitude/100,
 					g_gps->num_sats);
+			g_gps->new_data = false;
 		}else{
 			Serial.print(".");
 		}