alt hold updates

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

ArduCopterMega/ArduCopterMega.pde

@@ -298,6 +298,7 @@ long 	nav_yaw;							// deg * 100 : target yaw angle
 long 	nav_lat;							// for error calcs
 long 	nav_lon;							// for error calcs
 int 	nav_throttle;						// 0-1000 for throttle control
+int 	nav_throttle_old;						// 0-1000 for throttle control
 
 long 	command_yaw_start;					// what angle were we to begin with
 long 	command_yaw_start_time;				// when did we start turning
@@ -708,6 +709,7 @@ void update_GPS(void)
 	GPS.update();
 	update_GPS_light();
 	
+	// !!! comment out after testing
 	//fake_out_gps();
 	
 	if (GPS.new_data && GPS.fix) {
@@ -768,11 +770,6 @@ void update_current_flight_mode(void)
 			//	break;
 				
 			default:
-				// Intput Pitch, Roll, Yaw and Throttle
-				// ------------------------------------
-				//calc_nav_pid();
-				//calc_nav_roll();
-				//calc_nav_pitch();
 				
 				// based on altitude error
 				// -----------------------
@@ -798,8 +795,6 @@ void update_current_flight_mode(void)
 	
 		switch(control_mode){
 			case ACRO:
-				// Intput Pitch, Roll, Yaw and Throttle
-				// ------------------------------------
 				// clear any AP naviagtion values
 				nav_pitch 		= 0;
 				nav_roll 		= 0;
@@ -835,8 +830,6 @@ void update_current_flight_mode(void)
 				break;
 			
 			case STABILIZE:
-				// Intput Pitch, Roll, Yaw and Throttle
-				// ------------------------------------
 				// clear any AP naviagtion values
 				nav_pitch 		= 0;
 				nav_roll 		= 0;
@@ -903,8 +896,6 @@ void update_current_flight_mode(void)
 				break;
 
 			case ALT_HOLD:
-				// Intput Pitch, Roll, Yaw and Throttle
-				// ------------------------------------
 				// clear any AP naviagtion values
 				nav_pitch 		= 0;
 				nav_roll 		= 0;
@@ -913,6 +904,9 @@ void update_current_flight_mode(void)
 				// get desired height from the throttle
 				next_WP.alt 	= home.alt + (rc_3.control_in * 4); // 0 - 1000 (40 meters)
 				
+				// !!! testing
+				//next_WP.alt 	-= 500;
+				
 				// Yaw control
 				// -----------
 				output_manual_yaw();
@@ -934,13 +928,7 @@ void update_current_flight_mode(void)
 				output_stabilize_pitch();
 				break;
 				
-			case RTL:
+			case RTL:				
-				// Intput Pitch, Roll, Yaw and Throttle
-				// ------------------------------------
-				//calc_nav_pid();
-				//calc_nav_roll();
-				//calc_nav_pitch();
-				
 				// based on altitude error
 				// -----------------------
 				calc_nav_throttle();
@@ -961,12 +949,7 @@ void update_current_flight_mode(void)
 				break;
 				
 			case POSITION_HOLD:
-				// Intput Pitch, Roll, Yaw and Throttle
+
-				// ------------------------------------
-				//calc_nav_pid();
-				//calc_nav_roll();
-				//calc_nav_pitch();
-								
 				// Yaw control
 				// -----------
 				output_manual_yaw();

ArduCopterMega/commands.pde

@@ -133,7 +133,9 @@ void
 set_current_loc_here()
 {
 	//struct Location temp;
-	set_next_WP(&current_loc);
+	Location l = current_loc;
+	l.alt = get_altitude_above_home();
+	set_next_WP(&l);
 }
 
 /*
@@ -160,8 +162,8 @@ set_next_WP(struct Location *wp)
 
 	// used to control FBW and limit the rate of climb
 	// -----------------------------------------------
-	target_altitude = current_loc.alt;
+	target_altitude = current_loc.alt;						// PH: target_altitude = 200
-	offset_altitude = next_WP.alt - current_loc.alt;
+	offset_altitude = next_WP.alt - current_loc.alt;		// PH: offset_altitude = 0
 	
 	// zero out our loiter vals to watch for missed waypoints
 	loiter_delta 	= 0;

ArduCopterMega/config.h

@@ -386,10 +386,10 @@
 # define THROTTLE_BARO_I		0.05
 #endif
 #ifndef THROTTLE_BARO_D
-# define THROTTLE_BARO_D		0.06
+# define THROTTLE_BARO_D		0.2
 #endif
 #ifndef THROTTLE_BARO_IMAX
-# define THROTTLE_BARO_IMAX		100
+# define THROTTLE_BARO_IMAX		50
 #endif
 
 #ifndef THROTTLE_SONAR_P

ArduCopterMega/flight_control.pde

@@ -28,13 +28,15 @@ void calc_nav_throttle()
 		float t = pid_baro_throttle.kP();
 		
 		if(error > 0){
-			//pid_baro_throttle.kP(.25);
+			pid_baro_throttle.kP(t);
 		}else{
 			pid_baro_throttle.kP(t/4.0);
 		}
+		
 		// limit output of throttle control
 		nav_throttle = pid_baro_throttle.get_pid(error, delta_ms_fast_loop, 1.0);
-		nav_throttle = throttle_cruise + constrain(nav_throttle, -20, 70);
+		nav_throttle = throttle_cruise + constrain(nav_throttle, -30, 80);
+		
 		pid_baro_throttle.kP(t);
 		
 	} else {
@@ -44,6 +46,9 @@ void calc_nav_throttle()
 		// limit output of throttle control
 		nav_throttle = throttle_cruise + constrain(nav_throttle, -60, 100);
 	}
+	
+	nav_throttle = (nav_throttle + nav_throttle_old) >> 1;
+	nav_throttle_old = nav_throttle;
 }
 
 float angle_boost()

ArduCopterMega/motors.pde

@@ -182,6 +182,7 @@ set_servos_4()
 			write_int(motor_out[CH_3]);
 			write_int(motor_out[CH_4]);
 			
+			write_int(rc_3.servo_out);
 			write_int((int)(cos_yaw_x * 100));
 			write_int((int)(sin_yaw_y * 100));
 			write_int((int)(dcm.yaw_sensor / 100));
@@ -192,11 +193,12 @@ set_servos_4()
 
 			write_int((int)nav_roll);
 			write_int((int)nav_pitch);
+			
 			//24
-			write_long(home.lat);	//28
+			write_long(current_loc.lat);	//28
-			write_long(home.lng);	//32
+			write_long(current_loc.lng);	//32
-			write_int((int)home.alt);	//34
+			write_int((int)current_loc.alt);	//34
-
+			
 			write_long(next_WP.lat);
 			write_long(next_WP.lng);
 			write_int((int)next_WP.alt);		//44

ArduCopterMega/navigation.pde

@@ -149,6 +149,10 @@ void calc_altitude_error()
 	//Serial.printf("s: %d %d t_alt %d\n", (int)current_loc.alt, (int)altitude_error, (int)target_altitude);
 }
 
+//	target_altitude = current_loc.alt;						// PH: target_altitude = -200
+//	offset_altitude = next_WP.alt - current_loc.alt;		// PH: offset_altitude = 0
+
+
 long wrap_360(long error)
 {
 	if (error > 36000)	error -= 36000;

ArduCopterMega/system.pde

@@ -258,9 +258,7 @@ void set_mode(byte mode)
 		
 	control_mode = mode;
 	control_mode = constrain(control_mode, 0, NUM_MODES - 1);
-	
+		
-	save_EEPROM_PID();
-	
 	// used to stop fly_aways
 	if(rc_1.control_in == 0){
 		// we are on the ground is this is true

ArduCopterMega/test.pde

@@ -868,7 +868,8 @@ void fake_out_gps()
 	
 	GPS.latitude	= 377696000;	// Y
 	GPS.longitude	= -1224319000;	// X
-
+	GPS.altitude	= 9000;			// meters * 100
+	
 	//next_WP.lng	 	= home.lng - length * sin(rads);   // X
 	//next_WP.lat 	= home.lat + length * cos(rads);   // Y
 }