2.0.46 -Not flight tested!

restored 42 alt hold code
removed throttle timer, replaced with safer constant
increased alt control range
reformatted nav_rate calls
removed unused simple mode defines
set alt hold home to 10m
tuned down rateP to .13 from .14 for broader application.

ArduCopter/ArduCopter.pde

@@ -1,6 +1,6 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "ArduCopter V2.0.45 Beta"
+#define THISFIRMWARE "ArduCopter V2.0.46 Beta"
 /*
 ArduCopter Version 2.0 Beta
 Authors:	Jason Short
@@ -129,6 +129,9 @@ static AP_Int8                *flight_modes = &g.flight_mode1;
 	APM_BMP085_Class        barometer;
     AP_Compass_HMC5843      compass(Parameters::k_param_compass);
 
+  #ifdef OPTFLOW_ENABLED
+    AP_OpticalFlow_ADNS3080 optflow;
+  #endif
 
 	// real GPS selection
 	#if   GPS_PROTOCOL == GPS_PROTOCOL_AUTO
@@ -200,10 +203,6 @@ static AP_Int8                *flight_modes = &g.flight_mode1;
 	#endif
 	// normal dcm
 	AP_DCM  dcm(&imu, g_gps);
-
- 	#ifdef OPTFLOW_ENABLED
-	AP_OpticalFlow_ADNS3080 optflow;
- 	#endif
 #endif
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -483,7 +482,6 @@ static byte				gps_watchdog;
 // --------------
 static unsigned long 	fast_loopTimer;				// Time in miliseconds of main control loop
 static byte 			medium_loopCounter;			// Counters for branching from main control loop to slower loops
-static unsigned long	throttle_timer;
 
 static unsigned long	fiftyhz_loopTimer;
 
@@ -585,6 +583,9 @@ static void fast_loop()
 	// write out the servo PWM values
 	// ------------------------------
 	set_servos_4();
+
+	//if(motor_armed)
+	//	Log_Write_Attitude();
 }
 
 static void medium_loop()
@@ -950,19 +951,10 @@ static void update_GPS(void)
 			// so that the altitude is more accurate
 			// -------------------------------------
 			if (current_loc.lat == 0) {
-				//SendDebugln("!! bad loc");
 				ground_start_count = 5;
 
 			}else{
-				//Serial.printf("init Home!");
-
-				// reset our nav loop timer
-				//nav_loopTimer = millis();
 				init_home();
-
-				// init altitude
-				// commented out because we aren't using absolute altitude
-				// current_loc.alt = home.alt;
 				ground_start_count = 0;
 			}
 		}
@@ -1048,10 +1040,7 @@ void update_roll_pitch_mode(void)
 
 	switch(roll_pitch_mode){
 		case ROLL_PITCH_ACRO:
-			// Roll control
 			g.rc_1.servo_out = get_rate_roll(g.rc_1.control_in);
-
-			// Pitch control
 			g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in);
 			break;
 
@@ -1070,7 +1059,6 @@ void update_roll_pitch_mode(void)
 	}
 }
 
-
 // 50 hz update rate, not 250
 void update_throttle_mode(void)
 {
@@ -1084,8 +1072,6 @@ void update_throttle_mode(void)
 				g.pi_rate_pitch.reset_I();
 				g.rc_3.servo_out = 0;
 			}
-			// reset the timer to throttle so that we never get fast I term run-ups
-			throttle_timer = 0;
 			break;
 
 		case THROTTLE_HOLD:
@@ -1276,11 +1262,11 @@ adjust_altitude()
 {
 	if(g.rc_3.control_in <= 200){
 		next_WP.alt -= 1;												// 1 meter per second
-		next_WP.alt = max(next_WP.alt, (current_loc.alt - 400));		// don't go less than 4 meters below current location
+		next_WP.alt = max(next_WP.alt, (current_loc.alt - 500));		// don't go less than 4 meters below current location
 		next_WP.alt = max(next_WP.alt, 100);							// don't go less than 1 meter
 	}else if (g.rc_3.control_in > 700){
 		next_WP.alt += 1;												// 1 meter per second
-		next_WP.alt = min(next_WP.alt, (current_loc.alt + 400));		// don't go more than 4 meters below current location
+		next_WP.alt = min(next_WP.alt, (current_loc.alt + 500));		// don't go more than 4 meters below current location
 	}
 }
 
@@ -1360,10 +1346,10 @@ static void update_nav_wp()
 		calc_location_error(&next_WP);
 
 		// use error as the desired rate towards the target
-		calc_nav_rate(long_error, lat_error, g.waypoint_speed_max, 0);
+		calc_loiter(long_error, lat_error);
 
 		// rotate pitch and roll to the copter frame of reference
-		calc_nav_pitch_roll();
+		calc_loiter_pitch_roll();
 
 	}else if(wp_control == CIRCLE_MODE){
 
@@ -1393,18 +1379,18 @@ static void update_nav_wp()
 
 		// use error as the desired rate towards the target
 		// nav_lon, nav_lat is calculated
-		calc_nav_rate(long_error, lat_error, 200, 0);
+		calc_loiter(long_error, lat_error);
 
 		// rotate pitch and roll to the copter frame of reference
-		calc_nav_pitch_roll();
+		calc_loiter_pitch_roll();
 
 	} else {
 		// for long journey's reset the wind resopnse
 		// it assumes we are standing still.
 		// use error as the desired rate towards the target
-		calc_nav_rate2(g.waypoint_speed_max);
+		calc_nav_rate(g.waypoint_speed_max);
 		// rotate pitch and roll to the copter frame of reference
-		calc_nav_pitch_roll2();
+		calc_nav_pitch_roll();
 	}
 }
 

ArduCopter/Attitude.pde

@@ -85,7 +85,7 @@ get_stabilize_yaw(long target_angle)
 	return (int)constrain(rate, -2500, 2500);
 }
 
-#define ALT_ERROR_MAX 350
+#define ALT_ERROR_MAX 300
 static int
 get_nav_throttle(long z_error, int target_speed)
 {
@@ -95,9 +95,8 @@ get_nav_throttle(long z_error, int target_speed)
 	// limit error to prevent I term run up
 	z_error 		= constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX);
 	target_speed 	= z_error * scaler;
-
 	rate_error 		= target_speed - altitude_rate;
-	rate_error 		= constrain(rate_error, -120, 140);
+	rate_error 		= constrain(rate_error, -110, 110);
 
 	return (int)g.pi_throttle.get_pi(rate_error, .1);
 }
@@ -106,10 +105,9 @@ static int
 get_rate_roll(long target_rate)
 {
 	long error;
-	target_rate 		= constrain(target_rate, -2500, 2500);
-
-	error		= (target_rate * 4.5) - (long)(degrees(omega.x) * 100.0);
-	target_rate = g.pi_rate_roll.get_pi(error, G_Dt);
+	target_rate 	= constrain(target_rate, -2500, 2500);
+	error			= (target_rate * 4.5) - (long)(degrees(omega.x) * 100.0);
+	target_rate 	= g.pi_rate_roll.get_pi(error, G_Dt);
 
 	// output control:
 	return (int)constrain(target_rate, -2500, 2500);
@@ -119,10 +117,9 @@ static int
 get_rate_pitch(long target_rate)
 {
 	long error;
-	target_rate 		= constrain(target_rate, -2500, 2500);
-
-	error		= (target_rate * 4.5) - (long)(degrees(omega.y) * 100.0);
-	target_rate = g.pi_rate_pitch.get_pi(error, G_Dt);
+	target_rate 	= constrain(target_rate, -2500, 2500);
+	error			= (target_rate * 4.5) - (long)(degrees(omega.y) * 100.0);
+	target_rate 	= g.pi_rate_pitch.get_pi(error, G_Dt);
 
 	// output control:
 	return (int)constrain(target_rate, -2500, 2500);
@@ -132,7 +129,6 @@ static int
 get_rate_yaw(long target_rate)
 {
 	long error;
-
 	error		= (target_rate * 4.5) - (long)(degrees(omega.z) * 100.0);
 	target_rate = g.pi_rate_yaw.get_pi(error, G_Dt);
 

ArduCopter/config.h

@@ -221,8 +221,6 @@
   //#define OPTFLOW_ENABLED
 #endif
 
-//#define OPTFLOW_ENABLED
-
 #ifndef OPTFLOW					// sets global enabled/disabled flag for optflow (as seen in CLI)
 # define OPTFLOW				DISABLED
 #endif
@@ -317,19 +315,6 @@
 // Attitude Control
 //
 
-// SIMPLE Mode
-#ifndef SIMPLE_YAW
-# define SIMPLE_YAW 		YAW_HOLD
-#endif
-
-#ifndef SIMPLE_RP
-# define SIMPLE_RP 			ROLL_PITCH_STABLE
-#endif
-
-#ifndef SIMPLE_THR
-# define SIMPLE_THR 		THROTTLE_MANUAL
-#endif
-
 // Alt Hold Mode
 #ifndef ALT_HOLD_YAW
 # define ALT_HOLD_YAW 		YAW_HOLD
@@ -403,40 +388,40 @@
 // Attitude Control
 //
 #ifndef STABILIZE_ROLL_P
-# define STABILIZE_ROLL_P 		4.2
+# define STABILIZE_ROLL_P 		4.0
 #endif
 #ifndef STABILIZE_ROLL_I
 # define STABILIZE_ROLL_I 		0.001
 #endif
 #ifndef STABILIZE_ROLL_IMAX
-# define STABILIZE_ROLL_IMAX 	0		// degrees
+# define STABILIZE_ROLL_IMAX 	1.5		// degrees
 #endif
 
 #ifndef STABILIZE_PITCH_P
-# define STABILIZE_PITCH_P		4.2
+# define STABILIZE_PITCH_P		4.0
 #endif
 #ifndef STABILIZE_PITCH_I
 # define STABILIZE_PITCH_I		0.001
 #endif
 #ifndef STABILIZE_PITCH_IMAX
-# define STABILIZE_PITCH_IMAX	0		// degrees
+# define STABILIZE_PITCH_IMAX	1.5		// degrees
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
 // Rate Control
 //
 #ifndef RATE_ROLL_P
-# define RATE_ROLL_P         0.14
+# define RATE_ROLL_P         .13
 #endif
 #ifndef RATE_ROLL_I
-# define RATE_ROLL_I         0 //0.18
+# define RATE_ROLL_I         0.0
 #endif
 #ifndef RATE_ROLL_IMAX
 # define RATE_ROLL_IMAX	 	15			// degrees
 #endif
 
 #ifndef RATE_PITCH_P
-# define RATE_PITCH_P       0.14
+# define RATE_PITCH_P       0.13
 #endif
 #ifndef RATE_PITCH_I
 # define RATE_PITCH_I		0 //0.18
@@ -519,7 +504,7 @@
 # define THROTTLE_P		0.6			//
 #endif
 #ifndef THROTTLE_I
-# define THROTTLE_I		0.02		// with 4m error, 8 PWM gain/s
+# define THROTTLE_I		0.10		// with 4m error, 12.5s windup
 #endif
 #ifndef THROTTLE_IMAX
 # define THROTTLE_IMAX		300
@@ -671,7 +656,7 @@
 #endif
 
 #ifndef ALT_HOLD_HOME
-# define ALT_HOLD_HOME -1
+# define ALT_HOLD_HOME 10
 #endif
 
 #ifndef USE_CURRENT_ALT

ArduCopter/control_modes.pde

@@ -18,10 +18,7 @@ static void read_control_switch()
 				// setup Simple mode
 				// do we enable simple mode?
 	            do_simple = (g.simple_modes & (1 << switchPosition));
-				//Serial.printf("do simple: %d \n",  (int)do_simple);
 			#endif
-
-
 		}else{
 			switch_debouncer 	= true;
 		}

ArduCopter/navigation.pde

@@ -59,35 +59,16 @@ static void calc_location_error(struct Location *next_loc)
 // 	nav_roll	= g.pid_of_roll.get_pid(-optflow.x_cm * 10, dTnav, 1.0);
 
 #define NAV_ERR_MAX 400
-static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed)
+static void calc_loiter(int x_error, int y_error)
 {
 	// moved to globals for logging
-	//int x_actual_speed, y_actual_speed;
-	//int x_rate_error, y_rate_error;
+
 	x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);
 	y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX);
 
-	float scaler = (float)max_speed/(float)NAV_ERR_MAX;
-	g.pi_loiter_lat.kP(scaler);
-	g.pi_loiter_lon.kP(scaler);
-
 	int x_target_speed = g.pi_loiter_lon.get_pi(x_error, dTnav);
 	int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav);
 
-	//Serial.printf("scaler: %1.3f, y_target_speed %d",scaler,y_target_speed);
-
-	if(x_target_speed > 0){
-		x_target_speed	= max(x_target_speed, min_speed);
-	}else{
-		x_target_speed	= min(x_target_speed, -min_speed);
-	}
-
-	if(y_target_speed > 0){
-		y_target_speed	= max(y_target_speed, min_speed);
-	}else{
-		y_target_speed	= min(y_target_speed, -min_speed);
-	}
-
 	// find the rates:
 	float temp		= radians((float)g_gps->ground_course/100.0);
 
@@ -116,7 +97,7 @@ static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed
 	nav_lon		 	= constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500);
 }
 
-static void calc_nav_rate2(int max_speed)
+static void calc_nav_rate(int max_speed)
 {
 	/*
 	0  1   2   3   4   5   6   7   8
@@ -151,7 +132,7 @@ static void calc_nav_rate2(int max_speed)
 }
 
 // nav_roll, nav_pitch
-static void calc_nav_pitch_roll2()
+static void calc_nav_pitch_roll()
 {
 	float temp  	 = radians((float)(9000 - (dcm.yaw_sensor - original_target_bearing))/100.0);
 	float _cos_yaw_x = cos(temp);
@@ -173,7 +154,7 @@ static void calc_nav_pitch_roll2()
 
 
 // nav_roll, nav_pitch
-static void calc_nav_pitch_roll()
+static void calc_loiter_pitch_roll()
 {
 	// rotate the vector
 	nav_roll 	=  (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;