Merge branch 'master' of https://code.google.com/p/ardupilot-mega

ArduCopter/ArduCopter.pde

@@ -489,6 +489,12 @@ static float sin_pitch_y, sin_yaw_y, sin_roll_y;
 // or in SuperSimple mode when the copter leaves a 20m radius from home.
 static int32_t initial_simple_bearing;
 
+////////////////////////////////////////////////////////////////////////////////
+// ACRO Mode
+////////////////////////////////////////////////////////////////////////////////
+// Used to control Axis lock
+int32_t roll_axis;
+int32_t pitch_axis;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Circle Mode / Loiter control
@@ -660,9 +666,10 @@ static int16_t	nav_lat;
 // The desired bank towards East (Positive) or West (Negative)
 static int16_t	nav_lon;
 // The Commanded ROll from the autopilot based on optical flow sensor.
-static int32_t	of_roll = 0;
+static int32_t	of_roll;
 // The Commanded pitch from the autopilot based on optical flow sensor. negative Pitch means go forward.
-static int32_t	of_pitch = 0;
+static int32_t	of_pitch;
+static bool	slow_wp = false;
 
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -1333,14 +1340,7 @@ static void update_GPS(void)
 				ground_start_count = 5;
 
 			}else{
-				// block until we get a good fix
+				// save home to eeprom (we must have a good fix to have reached this point)
-				// -----------------------------
-				while (!g_gps->new_data || !g_gps->fix) {
-					g_gps->update();
-					// we need GCS update while waiting for GPS, to ensure
-					// we react to HIL mavlink
-					gcs_update();
-				}
 				init_home();
 				ground_start_count = 0;
 			}
@@ -1383,6 +1383,7 @@ void update_yaw_mode(void)
 
 		case YAW_AUTO:
 			nav_yaw += constrain(wrap_180(auto_yaw - nav_yaw), -20, 20); // 40 deg a second
+			//Serial.printf("nav_yaw %d ", nav_yaw);
 			nav_yaw  = wrap_360(nav_yaw);
 			break;
 	}
@@ -1407,9 +1408,27 @@ void update_roll_pitch_mode(void)
 
 	switch(roll_pitch_mode){
 		case ROLL_PITCH_ACRO:
+			if(g.axis_enabled){
+				roll_axis 	+= (float)g.rc_1.control_in * g.axis_lock_p;
+				pitch_axis 	+= (float)g.rc_2.control_in * g.axis_lock_p;
+
+				roll_axis = wrap_360(roll_axis);
+				pitch_axis = wrap_360(pitch_axis);
+
+				// in this mode, nav_roll and nav_pitch = the iterm
+				g.rc_1.servo_out = get_stabilize_roll(roll_axis);
+				g.rc_2.servo_out = get_stabilize_pitch(pitch_axis);
+
+				if (g.rc_3.control_in == 0){
+					roll_axis = 0;
+					pitch_axis = 0;
+				}
+
+			}else{
 				// ACRO does not get SIMPLE mode ability
 				g.rc_1.servo_out = get_acro_roll(g.rc_1.control_in);
 				g.rc_2.servo_out = get_acro_pitch(g.rc_2.control_in);
+			}
 			break;
 
 		case ROLL_PITCH_STABLE:
@@ -1585,9 +1604,12 @@ void update_throttle_mode(void)
 									manual_boost,
 									iterm);
 				//*/
+				// this lets us know we need to update the altitude after manual throttle control
 				reset_throttle_flag = true;
 
 			}else{
+				// we are under automatic throttle control
+				// ---------------------------------------
 				if(reset_throttle_flag)	{
 					set_new_altitude(max(current_loc.alt, 100));
 					reset_throttle_flag = false;
@@ -1635,8 +1657,8 @@ void update_throttle_mode(void)
 // called after a GPS read
 static void update_navigation()
 {
-	// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
+	// wp_distance is in CM
-	// ------------------------------------------------------------------------
+	// --------------------
 	switch(control_mode){
 		case AUTO:
 			// note: wp_control is handled by commands_logic
@@ -1676,6 +1698,7 @@ static void update_navigation()
 			}
 
 			wp_control = WP_MODE;
+			slow_wp = true;
 
 			// calculates desired Yaw
 			#if FRAME_CONFIG ==	HELI_FRAME
@@ -1903,15 +1926,16 @@ static void update_altitude()
 static void
 adjust_altitude()
 {
-	if(g.rc_3.control_in <= 180){
+	if(g.rc_3.control_in <= (MINIMUM_THROTTLE + 100)){
 		// we remove 0 to 100 PWM from hover
-		manual_boost = g.rc_3.control_in - 180;
+		manual_boost = (g.rc_3.control_in - MINIMUM_THROTTLE) -100;
-		manual_boost = max(-120, manual_boost);
+		manual_boost = max(-100, manual_boost);
 		update_throttle_cruise();
 
-	}else if  (g.rc_3.control_in >= 650){
+	}else if  (g.rc_3.control_in >= (MAXIMUM_THROTTLE - 100)){
 		// we add 0 to 100 PWM to hover
-		manual_boost = g.rc_3.control_in - 650;
+		manual_boost = g.rc_3.control_in - (MAXIMUM_THROTTLE - 100);
+		manual_boost = min(100, manual_boost);
 		update_throttle_cruise();
 	}else {
 		manual_boost = 0;
@@ -1935,7 +1959,6 @@ static void tuning(){
 			break;
 
 		case CH6_STABILIZE_KP:
-			//g.rc_6.set_range(0,8000); 		// 0 to 8
 			g.pi_stabilize_roll.kP(tuning_value);
 			g.pi_stabilize_pitch.kP(tuning_value);
 			break;
@@ -1960,7 +1983,6 @@ static void tuning(){
 			break;
 
 		case CH6_YAW_RATE_KP:
-			//g.rc_6.set_range(0,1000);
 			g.pid_rate_yaw.kP(tuning_value);
 			break;
 
@@ -2088,7 +2110,7 @@ static void update_nav_wp()
 		// calc error to target
 		calc_location_error(&next_WP);
 
-		int16_t speed = calc_desired_speed(g.waypoint_speed_max);
+		int16_t speed = calc_desired_speed(g.waypoint_speed_max, slow_wp);
 		// use error as the desired rate towards the target
 		calc_nav_rate(speed);
 		// rotate pitch and roll to the copter frame of reference
@@ -2121,5 +2143,6 @@ static void update_auto_yaw()
 		// Point towards next WP
 		auto_yaw = target_bearing;
 	}
+	//Serial.printf("auto_yaw %d ", auto_yaw);
 	// MAV_ROI_NONE = basic Yaw hold
 }

ArduCopter/Attitude.pde

@@ -245,6 +245,9 @@ static void reset_nav_params(void)
 
 	// Will be set by new command, used by loiter
 	next_WP.alt				= 0;
+
+	// We want to by default pass WPs
+	slow_wp = false;
 }
 
 /*

ArduCopter/GCS.h

@@ -127,7 +127,7 @@ private:
 	///
 	AP_Param   *_queued_parameter;                  ///< next parameter to be sent in queue
     enum ap_var_type _queued_parameter_type;        ///< type of the next parameter
-    uint16_t	_queued_parameter_token;            ///AP_Param token for next() call
+    AP_Param::ParamToken	_queued_parameter_token;///AP_Param token for next() call
 	uint16_t    _queued_parameter_index;            ///< next queued parameter's index
     uint16_t    _queued_parameter_count;            ///< saved count of parameters for queued send
 

ArduCopter/GCS_Mavlink.pde

@@ -1617,7 +1617,7 @@ GCS_MAVLINK::_count_parameters()
 	// if we haven't cached the parameter count yet...
 	if (0 == _parameter_count) {
         AP_Param  *vp;
-        uint16_t token;
+        AP_Param::ParamToken token;
 
         vp = AP_Param::first(&token, NULL);
 		do {

ArduCopter/Parameters.h

@@ -110,6 +110,7 @@ public:
 	k_param_sonar_type,
 	k_param_super_simple, //155
 	k_param_rtl_land_enabled,
+	k_param_axis_enabled,
 
 	//
 	// 160: Navigation parameters
@@ -172,6 +173,7 @@ public:
 	//
 	k_param_stabilize_d = 220,
 	k_param_acro_p,
+	k_param_axis_lock_p,
 	k_param_pid_rate_roll,
 	k_param_pid_rate_pitch,
 	k_param_pid_rate_yaw,
@@ -215,6 +217,8 @@ public:
 	AP_Float	low_voltage;
 	AP_Int8		super_simple;
 	AP_Int8		rtl_land_enabled;
+	AP_Int8		axis_enabled;
+
 
 
 	// Waypoints
@@ -295,6 +299,7 @@ public:
 
 	// PI/D controllers
     AP_Float	acro_p;
+	AP_Float	axis_lock_p;
 
 	AC_PID		pid_rate_roll;
 	AC_PID		pid_rate_pitch;
@@ -339,6 +344,7 @@ public:
 	low_voltage				(LOW_VOLTAGE),
 	super_simple			(SUPER_SIMPLE),
 	rtl_land_enabled		(RTL_AUTO_LAND),
+	axis_enabled			(AXIS_LOCK_ENABLED),
 
 	waypoint_mode			(0),
 	command_total			(0),
@@ -397,6 +403,7 @@ public:
 	camera_roll_gain 		(CAM_ROLL_GAIN),
 	stabilize_d 			(STABILIZE_D),
 	acro_p					(ACRO_P),
+	axis_lock_p				(AXIS_LOCK_P),
 
 	// PID controller	initial P	   	    initial I		 	initial D       	initial imax
 	//-----------------------------------------------------------------------------------------------------

ArduCopter/Parameters.pde

@@ -111,6 +111,8 @@ static const AP_Param::Info var_info[] PROGMEM = {
 	GSCALAR(camera_roll_gain, 	"CAM_R_G"),
 	GSCALAR(stabilize_d, 		"STAB_D"),
 	GSCALAR(acro_p, 			"ACRO_P"),
+	GSCALAR(axis_lock_p, 		"AXIS_P"),
+	GSCALAR(axis_enabled, 		"AXIS_ENABLE"),
 
 	// PID controller
 	//---------------

ArduCopter/commands_logic.pde

@@ -208,6 +208,10 @@ static void do_RTL(void)
 	// --------------------
 	set_next_WP(&temp);
 
+
+	// We want to come home and stop on a dime
+	slow_wp = true;
+
 	// output control mode to the ground station
 	// -----------------------------------------
 	gcs_send_message(MSG_HEARTBEAT);

ArduCopter/config.h

@@ -398,7 +398,7 @@
 # define MINIMUM_THROTTLE	130
 #endif
 #ifndef MAXIMUM_THROTTLE
-# define MAXIMUM_THROTTLE	1000
+# define MAXIMUM_THROTTLE	850
 #endif
 
 #ifndef AUTO_LAND_TIME
@@ -554,20 +554,23 @@
 #endif
 
 
-#ifndef STABILIZE_D
-# define STABILIZE_D 		.2
-#endif
-
-
-
 #ifndef ACRO_P
 # define ACRO_P 		4.5
 #endif
 
 
+#ifndef AXIS_LOCK_ENABLED
+# define AXIS_LOCK_ENABLED 	DISABLED
+#endif
+
+#ifndef AXIS_LOCK_P
+# define AXIS_LOCK_P 		.02
+#endif
+
+
 // Good for smaller payload motors.
 #ifndef STABILIZE_ROLL_P
-# define STABILIZE_ROLL_P 		4
+# define STABILIZE_ROLL_P 		4.5
 #endif
 #ifndef STABILIZE_ROLL_I
 # define STABILIZE_ROLL_I 		0.0
@@ -577,7 +580,7 @@
 #endif
 
 #ifndef STABILIZE_PITCH_P
-# define STABILIZE_PITCH_P		4
+# define STABILIZE_PITCH_P		4.5
 #endif
 #ifndef STABILIZE_PITCH_I
 # define STABILIZE_PITCH_I		0.0
@@ -601,26 +604,26 @@
 // Stabilize Rate Control
 //
 #ifndef RATE_ROLL_P
-# define RATE_ROLL_P        0.12
+# define RATE_ROLL_P        0.14
 #endif
 #ifndef RATE_ROLL_I
-# define RATE_ROLL_I        0.02
+# define RATE_ROLL_I        0.0
 #endif
 #ifndef RATE_ROLL_D
-# define RATE_ROLL_D        0.008
+# define RATE_ROLL_D        0.002
 #endif
 #ifndef RATE_ROLL_IMAX
 # define RATE_ROLL_IMAX	 	5			// degrees
 #endif
 
 #ifndef RATE_PITCH_P
-# define RATE_PITCH_P       0.12
+# define RATE_PITCH_P       0.14
 #endif
 #ifndef RATE_PITCH_I
-# define RATE_PITCH_I		0.02
+# define RATE_PITCH_I		0.0
 #endif
 #ifndef RATE_PITCH_D
-# define RATE_PITCH_D       0.008
+# define RATE_PITCH_D       0.002
 #endif
 #ifndef RATE_PITCH_IMAX
 # define RATE_PITCH_IMAX   	5			// degrees
@@ -633,13 +636,17 @@
 # define RATE_YAW_I    		 0.0
 #endif
 #ifndef RATE_YAW_D
-# define RATE_YAW_D    		 .002
+# define RATE_YAW_D    		 .000
 #endif
 #ifndef RATE_YAW_IMAX
 # define RATE_YAW_IMAX 		  50		// degrees
 #endif
 
 
+#ifndef STABILIZE_D
+# define STABILIZE_D 		0.05
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 // Loiter control gains
 //
@@ -690,8 +697,6 @@
 #endif
 
 
-
-
 #ifndef WAYPOINT_SPEED_MAX
 # define WAYPOINT_SPEED_MAX		600			// 6m/s error = 13mph
 #endif

ArduCopter/motors_octa.pde

@@ -29,7 +29,7 @@ static void output_motors_armed()
 	int out_max = g.rc_3.radio_max;
 
 	// Throttle is 0 to 1000 only
-	g.rc_3.servo_out 	= constrain(g.rc_3.servo_out, 0, 800);
+	g.rc_3.servo_out 	= constrain(g.rc_3.servo_out, 0, MAXIMUM_THROTTLE);
 
 	if(g.rc_3.servo_out > 0)
 		out_min = g.rc_3.radio_min + MINIMUM_THROTTLE;

ArduCopter/motors_octa_quad.pde

@@ -29,7 +29,7 @@ static void output_motors_armed()
 	int out_max = g.rc_3.radio_max;
 
 	// Throttle is 0 to 1000 only
-	g.rc_3.servo_out 	= constrain(g.rc_3.servo_out, 0, 800);
+	g.rc_3.servo_out 	= constrain(g.rc_3.servo_out, 0, MAXIMUM_THROTTLE);
 
 	if(g.rc_3.servo_out > 0)
 		out_min = g.rc_3.radio_min + MINIMUM_THROTTLE;

ArduCopter/motors_quad.pde

@@ -24,7 +24,7 @@ static void output_motors_armed()
 	int out_max = g.rc_3.radio_max;
 
 	// Throttle is 0 to 1000 only
-	g.rc_3.servo_out 	= constrain(g.rc_3.servo_out, 0, 800);
+	g.rc_3.servo_out 	= constrain(g.rc_3.servo_out, 0, MAXIMUM_THROTTLE);
 
 	if(g.rc_3.servo_out > 0)
 		out_min = g.rc_3.radio_min + MINIMUM_THROTTLE;

ArduCopter/motors_tri.pde

@@ -23,7 +23,7 @@ static void output_motors_armed()
 	int out_max = g.rc_3.radio_max;
 
 	// Throttle is 0 to 1000 only
-	g.rc_3.servo_out 	= constrain(g.rc_3.servo_out, 0, 800);
+	g.rc_3.servo_out 	= constrain(g.rc_3.servo_out, 0, MAXIMUM_THROTTLE);
 
 	if(g.rc_3.servo_out > 0)
 		out_min = g.rc_3.radio_min + MINIMUM_THROTTLE;

ArduCopter/motors_y6.pde

@@ -28,7 +28,7 @@ static void output_motors_armed()
 	int out_max = g.rc_3.radio_max;
 
 	// Throttle is 0 to 1000 only
-	g.rc_3.servo_out 	= constrain(g.rc_3.servo_out, 0, 800);
+	g.rc_3.servo_out 	= constrain(g.rc_3.servo_out, 0, MAXIMUM_THROTTLE);
 
 	if(g.rc_3.servo_out > 0)
 		out_min = g.rc_3.radio_min + MINIMUM_THROTTLE;

ArduCopter/navigation.pde

@@ -265,7 +265,7 @@ static void calc_loiter_pitch_roll()
 	auto_pitch = -auto_pitch;
 }
 
-static int16_t calc_desired_speed(int16_t max_speed)
+static int16_t calc_desired_speed(int16_t max_speed, bool _slow)
 {
 	/*
 	|< WP Radius
@@ -277,14 +277,13 @@ static int16_t calc_desired_speed(int16_t max_speed)
 	*/
 
 	// max_speed is default 600 or 6m/s
-	// (wp_distance * .5) = 1/2 of the distance converted to speed
+	if(_slow){
-	// wp_distance is always in m/s and not cm/s - I know it's stupid that way
+		max_speed 		= min(max_speed, wp_distance / 2);
-	// for example 4m from target = 200cm/s speed
+		max_speed 		= max(max_speed, 0);
-	// we choose the lowest speed based on disance
+	}else{
 		max_speed 		= min(max_speed, wp_distance);
-
+		max_speed 		= max(max_speed, WAYPOINT_SPEED_MIN);	// go at least 100cm/s
-	// go at least 100cm/s
+	}
-	max_speed 		= max(max_speed, WAYPOINT_SPEED_MIN);
 
 	// limit the ramp up of the speed
 	// waypoint_speed_gov is reset to 0 at each new WP command
@@ -326,6 +325,13 @@ static void clear_new_altitude()
 
 static void set_new_altitude(int32_t _new_alt)
 {
+	if(_new_alt == current_loc.alt){
+		next_WP.alt 	= _new_alt;
+		target_altitude = _new_alt;
+		alt_change_flag = REACHED_ALT;
+		return;
+	}
+
 	// just to be clear
 	next_WP.alt = current_loc.alt;
 

ArduCopter/radio.pde

@@ -22,9 +22,11 @@ static void init_rc_in()
 	// set rc channel ranges
 	g.rc_1.set_angle(4500);
 	g.rc_2.set_angle(4500);
-	g.rc_3.set_range(0, MAXIMUM_THROTTLE);
+	#if FRAME_CONFIG == HELI_FRAME
-    #if FRAME_CONFIG !=	HELI_FRAME
+		// we do not want to limit the movment of the heli's swash plate
-	g.rc_3.scale_output = .9;
+		g.rc_3.set_range(0, 1000);
+	#else
+		g.rc_3.set_range(MINIMUM_THROTTLE, MAXIMUM_THROTTLE);
 	#endif
 	g.rc_4.set_angle(4500);
 
@@ -35,13 +37,6 @@ static void init_rc_in()
 	g.rc_2.set_type(RC_CHANNEL_ANGLE_RAW);
 	g.rc_4.set_type(RC_CHANNEL_ANGLE_RAW);
 
-	// set rc dead zones
-	/*g.rc_1.dead_zone = 60;
-	g.rc_2.dead_zone = 60;
-	g.rc_3.dead_zone = 60;
-	g.rc_4.dead_zone = 300;
-	*/
-
 	//set auxiliary ranges
 	g.rc_5.set_range(0,1000);
 	g.rc_6.set_range(0,1000);
@@ -69,7 +64,7 @@ static void init_rc_out()
     }
 
 	// we are full throttle
-	if(g.rc_3.control_in == 800){
+	if(g.rc_3.control_in >= (MAXIMUM_THROTTLE - 50)){
 		if(g.esc_calibrate == 0){
 			// we will enter esc_calibrate mode on next reboot
 			g.esc_calibrate.set_and_save(1);
@@ -138,7 +133,7 @@ static void read_radio()
 
 		#if FRAME_CONFIG != HELI_FRAME
 			// limit our input to 800 so we can still pitch and roll
-			g.rc_3.control_in = min(g.rc_3.control_in, 800);
+			g.rc_3.control_in = min(g.rc_3.control_in, MAXIMUM_THROTTLE);
 		#endif
 
 		throttle_failsafe(g.rc_3.radio_in);

ArduCopter/setup.pde

@@ -457,7 +457,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
 	int value = 0;
 	int temp;
 	int state = 0;   // 0 = set rev+pos, 1 = capture min/max
-	int max_roll, max_pitch, min_collective, max_collective, min_tail, max_tail;
+	int max_roll=0, max_pitch=0, min_collective=0, max_collective=0, min_tail=0, max_tail=0;
 
 	// initialise swash plate
 	heli_init_swash();

ArduCopter/system.pde

@@ -419,6 +419,9 @@ static void set_mode(byte mode)
 	// clearing value used to force the copter down in landing mode
 	landing_boost = 0;
 
+	// do we want to come to a stop or pass a WP?
+	slow_wp = false;
+
 	// do not auto_land if we are leaving RTL
 	auto_land_timer = 0;
 

ArduCopter/test.pde

@@ -17,7 +17,7 @@ static int8_t	test_imu(uint8_t argc, 			const Menu::arg *argv);
 //static int8_t	test_dcm(uint8_t argc, 			const Menu::arg *argv);
 //static int8_t	test_omega(uint8_t argc, 		const Menu::arg *argv);
 static int8_t	test_battery(uint8_t argc, 		const Menu::arg *argv);
-//static int8_t	test_nav(uint8_t argc, 			const Menu::arg *argv);
+//static int8_t	test_boost(uint8_t argc, 		const Menu::arg *argv);
 //static int8_t	test_wp_nav(uint8_t argc, 		const Menu::arg *argv);
 //static int8_t	test_reverse(uint8_t argc, 		const Menu::arg *argv);
 static int8_t	test_tuning(uint8_t argc, 		const Menu::arg *argv);
@@ -70,7 +70,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
 	//{"tri",			test_tri},
 	{"relay",		test_relay},
 	{"wp",			test_wp},
-	//{"nav",			test_nav},
+//	{"boost",		test_boost},
 #if HIL_MODE != HIL_MODE_ATTITUDE
 	{"altitude",	test_baro},
 	{"sonar",		test_sonar},
@@ -179,37 +179,26 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
 }*/
 
 /*
-//static int8_t
+static int8_t
-//test_nav(uint8_t argc, const Menu::arg *argv)
+//test_boost(uint8_t argc, const Menu::arg *argv)
 {
 	print_hit_enter();
 	delay(1000);
+	int16_t temp = MINIMUM_THROTTLE;
 
 	while(1){
-		delay(1000);
+		delay(20);
-		g_gps->ground_course = 19500;
+		g.rc_3.control_in = temp;
-		calc_nav_rate2(g.waypoint_speed_max);
+		adjust_altitude();
-		calc_nav_pitch_roll2();
+		Serial.printf("tmp:%d, boost: %d\n", temp, manual_boost);
+		temp++;
 
-		g_gps->ground_course = 28500;
+		if(temp > MAXIMUM_THROTTLE){
-		calc_nav_rate2(g.waypoint_speed_max);
-		calc_nav_pitch_roll2();
-
-		g_gps->ground_course = 1500;
-		calc_nav_rate2(g.waypoint_speed_max);
-		calc_nav_pitch_roll2();
-
-		g_gps->ground_course = 10500;
-		calc_nav_rate2(g.waypoint_speed_max);
-		calc_nav_pitch_roll2();
-
-
-		//if(Serial.available() > 0){
 			return (0);
-		//}
+		}
 	}
 }
-*/
+//*/
 
 static int8_t
 test_radio(uint8_t argc, const Menu::arg *argv)

ArduPlane/GCS.h

@@ -127,7 +127,7 @@ private:
 	///
 	AP_Param   *_queued_parameter;                  ///< next parameter to be sent in queue
     enum ap_var_type _queued_parameter_type;        ///< type of the next parameter
-    uint16_t	_queued_parameter_token;            ///AP_Param token for next() call
+    AP_Param::ParamToken	_queued_parameter_token;///AP_Param token for next() call
 	uint16_t    _queued_parameter_index;            ///< next queued parameter's index
     uint16_t    _queued_parameter_count;            ///< saved count of parameters for queued send
 

ArduPlane/GCS_Mavlink.pde

@@ -1970,7 +1970,7 @@ GCS_MAVLINK::_count_parameters()
     // if we haven't cached the parameter count yet...
     if (0 == _parameter_count) {
         AP_Param  *vp;
-        uint16_t token;
+        AP_Param::ParamToken token;
 
         vp = AP_Param::first(&token, NULL);
         do {

Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.zh-TW.resx

@@ -151,6 +151,15 @@
   <data name="Param4.HeaderText" xml:space="preserve">
     <value>參數4</value>
   </data>
+  <data name="Lat.HeaderText" xml:space="preserve">
+    <value>緯度</value>
+  </data>
+  <data name="Lon.HeaderText" xml:space="preserve">
+    <value>經度</value>
+  </data>
+  <data name="Alt.HeaderText" xml:space="preserve">
+    <value>高度</value>
+  </data>
   <data name="Delete.HeaderText" xml:space="preserve">
     <value>删除</value>
   </data>
@@ -161,13 +170,13 @@
     <value>上移</value>
   </data>
   <data name="Up.ToolTipText" xml:space="preserve">
-    <value>向上移動本行</value>
+    <value>向上移动本行</value>
   </data>
   <data name="Down.HeaderText" xml:space="preserve">
     <value>下移</value>
   </data>
   <data name="Down.ToolTipText" xml:space="preserve">
-    <value>向下移动本行</value>
+    <value>向下移動本行</value>
   </data>
   <data name="Commands.ToolTip" xml:space="preserve">
     <value />
@@ -185,7 +194,7 @@
     <value>Microsoft Sans Serif, 8.25pt</value>
   </data>
   <data name="TXT_WPRad.Location" type="System.Drawing.Point, System.Drawing">
-    <value>66, 31</value>
+    <value>24, 40</value>
   </data>
   <data name="TXT_WPRad.ToolTip" xml:space="preserve">
     <value />
@@ -194,7 +203,7 @@
     <value>Microsoft Sans Serif, 8.25pt</value>
   </data>
   <data name="TXT_DefaultAlt.Location" type="System.Drawing.Point, System.Drawing">
-    <value>278, 31</value>
+    <value>162, 40</value>
   </data>
   <data name="TXT_DefaultAlt.ToolTip" xml:space="preserve">
     <value />
@@ -227,7 +236,7 @@
     <value>Microsoft Sans Serif, 8.25pt</value>
   </data>
   <data name="TXT_loiterrad.Location" type="System.Drawing.Point, System.Drawing">
-    <value>170, 31</value>
+    <value>90, 40</value>
   </data>
   <data name="TXT_loiterrad.ToolTip" xml:space="preserve">
     <value />
@@ -239,7 +248,7 @@
     <value>55, 13</value>
   </data>
   <data name="label5.Text" xml:space="preserve">
-    <value>盤旋半径</value>
+    <value>盤旋半徑</value>
   </data>
   <data name="label5.ToolTip" xml:space="preserve">
     <value />
@@ -431,7 +440,7 @@
     <value />
   </data>
   <data name="comboBoxMapType.ToolTip" xml:space="preserve">
-    <value>更改目前地圖類型</value>
+    <value>更改當前地圖類型</value>
   </data>
   <data name="lbl_status.Font" type="System.Drawing.Font, System.Drawing">
     <value>Microsoft Sans Serif, 8.25pt</value>
@@ -449,7 +458,7 @@
     <value>1. 連接
 2. 讀取航路（如果需要）
 3. 確保起始位置和高度都已設置
-4. 在地图上點擊，增加航點</value>
+4. 在地圖上點擊，增加航點</value>
   </data>
   <data name="textBox1.ToolTip" xml:space="preserve">
     <value />
@@ -457,6 +466,24 @@
   <data name="splitter1.ToolTip" xml:space="preserve">
     <value />
   </data>
+  <data name="BUT_loadkml.Text" xml:space="preserve">
+    <value>KML疊加</value>
+  </data>
+  <data name="BUT_loadkml.ToolTip" xml:space="preserve">
+    <value />
+  </data>
+  <data name="BUT_zoomto.Text" xml:space="preserve">
+    <value>縮放至</value>
+  </data>
+  <data name="BUT_zoomto.ToolTip" xml:space="preserve">
+    <value />
+  </data>
+  <data name="BUT_Camera.Text" xml:space="preserve">
+    <value>相機</value>
+  </data>
+  <data name="BUT_Camera.ToolTip" xml:space="preserve">
+    <value />
+  </data>
   <data name="BUT_grid.Text" xml:space="preserve">
     <value>網格</value>
   </data>
@@ -467,7 +494,7 @@
     <value>預取</value>
   </data>
   <data name="BUT_Prefetch.ToolTip" xml:space="preserve">
-    <value>預先缓存選中區域的地圖</value>
+    <value>預先緩存選中區域的地圖</value>
   </data>
   <data name="button1.Text" xml:space="preserve">
     <value>海拔圖</value>
@@ -518,19 +545,19 @@
     <value>删除航點</value>
   </data>
   <data name="loiterForeverToolStripMenuItem.Size" type="System.Drawing.Size, System.Drawing">
-    <value>152, 22</value>
+    <value>98, 22</value>
   </data>
   <data name="loiterForeverToolStripMenuItem.Text" xml:space="preserve">
     <value>永久</value>
   </data>
   <data name="loitertimeToolStripMenuItem.Size" type="System.Drawing.Size, System.Drawing">
-    <value>152, 22</value>
+    <value>98, 22</value>
   </data>
   <data name="loitertimeToolStripMenuItem.Text" xml:space="preserve">
     <value>時間</value>
   </data>
   <data name="loitercirclesToolStripMenuItem.Size" type="System.Drawing.Size, System.Drawing">
-    <value>152, 22</value>
+    <value>98, 22</value>
   </data>
   <data name="loitercirclesToolStripMenuItem.Text" xml:space="preserve">
     <value>圈數</value>
@@ -541,12 +568,6 @@
   <data name="loiterToolStripMenuItem.Text" xml:space="preserve">
     <value>盤旋</value>
   </data>
-  <data name="jumpstartToolStripMenuItem.Size" type="System.Drawing.Size, System.Drawing">
-    <value>152, 22</value>
-  </data>
-  <data name="jumpwPToolStripMenuItem.Size" type="System.Drawing.Size, System.Drawing">
-    <value>152, 22</value>
-  </data>
   <data name="jumpToolStripMenuItem.Size" type="System.Drawing.Size, System.Drawing">
     <value>122, 22</value>
   </data>
@@ -569,20 +590,17 @@
     <value>旋轉地圖</value>
   </data>
   <data name="addPolygonPointToolStripMenuItem.Size" type="System.Drawing.Size, System.Drawing">
-    <value>152, 22</value>
+    <value>122, 22</value>
   </data>
   <data name="addPolygonPointToolStripMenuItem.Text" xml:space="preserve">
     <value>增加頂點</value>
   </data>
   <data name="clearPolygonToolStripMenuItem.Size" type="System.Drawing.Size, System.Drawing">
-    <value>152, 22</value>
+    <value>122, 22</value>
   </data>
   <data name="clearPolygonToolStripMenuItem.Text" xml:space="preserve">
     <value>清除區域</value>
   </data>
-  <data name="gridToolStripMenuItem.Size" type="System.Drawing.Size, System.Drawing">
-    <value>122, 22</value>
-  </data>
   <data name="clearMissionToolStripMenuItem.Size" type="System.Drawing.Size, System.Drawing">
     <value>122, 22</value>
   </data>
@@ -750,7 +768,7 @@
     <value>31, 13</value>
   </data>
   <data name="label11.Text" xml:space="preserve">
-    <value>缩放</value>
+    <value>縮放</value>
   </data>
   <data name="label11.ToolTip" xml:space="preserve">
     <value />
@@ -764,35 +782,4 @@
   <data name="$this.ToolTip" xml:space="preserve">
     <value />
   </data>
-  <assembly alias="System.Windows.Forms" name="System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089" />
-  <data name="MAVCmd" type="System.Resources.ResXFileRef, System.Windows.Forms">
-    <value>..\Resources\MAVCmd.zh-Hans.txt;System.String, mscorlib, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089;utf-8</value>
-  </data>
-  <data name="Alt.HeaderText" xml:space="preserve">
-    <value>高度</value>
-  </data>
-  <data name="BUT_Camera.Text" xml:space="preserve">
-    <value>相機</value>
-  </data>
-  <data name="BUT_Camera.ToolTip" xml:space="preserve">
-    <value />
-  </data>
-  <data name="BUT_loadkml.Text" xml:space="preserve">
-    <value>KML疊加</value>
-  </data>
-  <data name="BUT_loadkml.ToolTip" xml:space="preserve">
-    <value />
-  </data>
-  <data name="BUT_zoomto.Text" xml:space="preserve">
-    <value>缩放至</value>
-  </data>
-  <data name="BUT_zoomto.ToolTip" xml:space="preserve">
-    <value />
-  </data>
-  <data name="Lat.HeaderText" xml:space="preserve">
-    <value>緯度</value>
-  </data>
-  <data name="Lon.HeaderText" xml:space="preserve">
-    <value>經度</value>
-  </data>
 </root>

Tools/VARTest/VARTest.pde

@@ -106,7 +106,7 @@ void setup() {
 	compass.save_offsets();
 
 	// full testing of all variables
-	uint16_t token;
+	AP_Param::ParamToken token;
 	for (AP_Param *ap = AP_Param::first(&token, &type);
 		 ap;
 		 ap=AP_Param::next(&token, &type)) {

Tools/autotest/arducopter.py

@@ -14,6 +14,10 @@ HOME=location(-35.362938,149.165085,584,270)
 homeloc = None
 num_wp = 0
 
+def hover(mavproxy, mav):
+    mavproxy.send('rc 3 1395\n')
+    return True
+
 def calibrate_level(mavproxy, mav):
     '''init the accelerometers'''
     print("Initialising accelerometers")
@@ -54,7 +58,7 @@ def takeoff(mavproxy, mav, alt_min = 30):
     m = mav.recv_match(type='VFR_HUD', blocking=True)
     if (m.alt < alt_min):
         wait_altitude(mav, alt_min, (alt_min + 5))
-    mavproxy.send('rc 3 1430\n')
+    hover(mavproxy, mav)
     print("TAKEOFF COMPLETE")
     return True
 
@@ -86,13 +90,13 @@ def change_alt(mavproxy, mav, alt_min):
     m = mav.recv_match(type='VFR_HUD', blocking=True)
     if(m.alt < alt_min):
         print("Rise to alt:%u from %u" % (alt_min, m.alt))
-        mavproxy.send('rc 3 1800\n')
+        mavproxy.send('rc 3 1920\n')
         wait_altitude(mav, alt_min, (alt_min + 5))
     else:
         print("Lower to alt:%u from %u" % (alt_min, m.alt))
-        mavproxy.send('rc 3 1100\n')
+        mavproxy.send('rc 3 1120\n')
         wait_altitude(mav, (alt_min -5), alt_min)
-    mavproxy.send('rc 3 1430\n')
+    hover(mavproxy, mav)
     return True
 
 
@@ -107,7 +111,7 @@ def fly_square(mavproxy, mav, side=50, timeout=120):
     save_wp(mavproxy, mav)
 
     print("turn")
-    mavproxy.send('rc 3 1430\n')
+    hover(mavproxy, mav)
     mavproxy.send('rc 4 1610\n')
     if not wait_heading(mav, 0):
         return False
@@ -156,7 +160,7 @@ def fly_RTL(mavproxy, mav, side=60):
     '''Fly, return, land'''
     mavproxy.send('switch 6\n')
     wait_mode(mav, 'STABILIZE')
-    mavproxy.send('rc 3 1430\n')
+    hover(mavproxy, mav)
     failed = False
 
     print("# Going forward %u meters" % side)
@@ -181,7 +185,7 @@ def fly_failsafe(mavproxy, mav, side=60):
     '''Fly, Failsafe, return, land'''
     mavproxy.send('switch 6\n')
     wait_mode(mav, 'STABILIZE')
-    mavproxy.send('rc 3 1430\n')
+    hover(mavproxy, mav)
     failed = False
 
     print("# Going forward %u meters" % side)
@@ -193,22 +197,25 @@ def fly_failsafe(mavproxy, mav, side=60):
     print("# Enter Failsafe")
     mavproxy.send('rc 3 900\n')
     tstart = time.time()
-    while time.time() < tstart + 120:
+    while time.time() < tstart + 250:
         m = mav.recv_match(type='VFR_HUD', blocking=True)
         pos = current_location(mav)
-        #delta = get_distance(start, pos)
+        home_distance = get_distance(HOME, pos)
-        print("Alt: %u" % m.alt)
+        print("Alt: %u  HomeDistance: %.0f" % (m.alt, home_distance))
-        if(m.alt <= 1):
+        if m.alt <= 1 and home_distance < 10:
+            print("Reached failsafe home OK")
             mavproxy.send('rc 3 1100\n')
             return True
-    return True
+    print("Failed to land on failsafe RTL - timed out after 120 seconds")
+    return False
 
 
 def fly_simple(mavproxy, mav, side=60, timeout=120):
     '''fly Simple, flying N then E'''
     mavproxy.send('switch 6\n')
     wait_mode(mav, 'STABILIZE')
-    mavproxy.send('rc 3 1440\n')
+    mavproxy.send('rc 3 1400\n')
+
     tstart = time.time()
     failed = False
 
@@ -234,7 +241,7 @@ def fly_simple(mavproxy, mav, side=60, timeout=120):
     mavproxy.send('rc 2 1500\n')
     #restore to default
     mavproxy.send('param set SIMPLE 0\n')
-    mavproxy.send('rc 3 1430\n')
+    hover(mavproxy, mav)
     return not failed
 
 

Tools/autotest/autotest.py

@@ -67,6 +67,16 @@ def dump_logs(atype):
     print("Saved log for %s to %s" % (atype, logfile))
     return True
 
+
+def build_all():
+    '''run the build_all.sh script'''
+    print("Running build_all.sh")
+    if util.run_cmd(util.reltopdir('Tools/scripts/build_all.sh'), dir=util.reltopdir('.')) != 0:
+        print("Failed build_all.sh")
+        return False
+    return True
+
+
 def convert_gpx():
     '''convert any mavlog files to GPX and KML'''
     import glob
@@ -108,6 +118,7 @@ steps = [
     'prerequesites',
     'build1280.ArduPlane',
     'build2560.ArduPlane',
+    'build.All',
     'build.ArduPlane',
     'defaults.ArduPlane',
     'fly.ArduPlane',
@@ -171,6 +182,9 @@ def run_step(step):
     if step == 'fly.ArduPlane':
         return arduplane.fly_ArduPlane(viewerip=opts.viewerip)
 
+    if step == 'build.All':
+        return build_all()
+
     if step == 'convertgpx':
         return convert_gpx()
 

Tools/scripts/build_all.sh

@@ -6,6 +6,7 @@
 set -e
 set -x
 
+echo "Testing ArduPlane build"
 pushd ArduPlane
 for b in all apm2 apm2beta hil hilsensors mavlink10 sitl; do
     pwd
@@ -14,10 +15,23 @@ for b in all apm2 apm2beta hil hilsensors mavlink10 sitl; do
 done
 popd
 
+echo "Testing ArduCopter build"
 pushd ArduCopter
-for b in all apm2 apm2beta hil sitl; do
+for b in all apm2 apm2beta hil sitl heli; do
     pwd
     make clean
     make $b
 done
 popd
+
+echo "Testing build of examples"
+
+examples="Tools/VARTest Tools/CPUInfo"
+for d in $examples; do
+    pushd $d
+    make clean
+    make
+    popd
+done
+
+exit 0

libraries/AP_Common/AP_Param.cpp

@@ -575,9 +575,10 @@ bool AP_Param::load_all(void)
 
 
 // return the first variable in _var_info
-AP_Param *AP_Param::first(uint16_t *token, enum ap_var_type *ptype)
+AP_Param *AP_Param::first(ParamToken *token, enum ap_var_type *ptype)
 {
-    *token = 0;
+    token->key = 0;
+    token->group_element = 0;
     if (_num_vars == 0) {
         return NULL;
     }
@@ -593,7 +594,7 @@ AP_Param *AP_Param::next_group(uint8_t vindex, const struct GroupInfo *group_inf
                                bool *found_current,
                                uint8_t group_base,
                                uint8_t group_shift,
-                               uint16_t *token,
+                               ParamToken *token,
                                enum ap_var_type *ptype)
 {
     uint8_t type;
@@ -612,13 +613,14 @@ AP_Param *AP_Param::next_group(uint8_t vindex, const struct GroupInfo *group_inf
         } else {
             if (*found_current) {
                 // got a new one
-                (*token) = ((uint16_t)GROUP_ID(group_info, group_base, i, group_shift)<<8) | vindex;
+                token->key = vindex;
+                token->group_element = GROUP_ID(group_info, group_base, i, group_shift);
                 if (ptype != NULL) {
                     *ptype = (enum ap_var_type)type;
                 }
                 return (AP_Param*)(PGM_POINTER(&_var_info[vindex].ptr) + PGM_UINT16(&group_info[i].offset));
             }
-            if (GROUP_ID(group_info, group_base, i, group_shift) == (uint8_t)((*token)>>8)) {
+            if (GROUP_ID(group_info, group_base, i, group_shift) == token->group_element) {
                 *found_current = true;
             }
         }
@@ -628,9 +630,9 @@ AP_Param *AP_Param::next_group(uint8_t vindex, const struct GroupInfo *group_inf
 
 /// Returns the next variable in _var_info, recursing into groups
 /// as needed
-AP_Param *AP_Param::next(uint16_t *token, enum ap_var_type *ptype)
+AP_Param *AP_Param::next(ParamToken *token, enum ap_var_type *ptype)
 {
-    uint8_t i = (*token)&0xFF;
+    uint8_t i = token->key;
     bool found_current = false;
     if (i >= _num_vars) {
         // illegal token
@@ -651,7 +653,8 @@ AP_Param *AP_Param::next(uint16_t *token, enum ap_var_type *ptype)
             }
         } else {
             // found the next one
-            (*token) = i;
+            token->key = i;
+            token->group_element = 0;
             if (ptype != NULL) {
                 *ptype = (enum ap_var_type)type;
             }
@@ -663,7 +666,7 @@ AP_Param *AP_Param::next(uint16_t *token, enum ap_var_type *ptype)
 
 /// Returns the next scalar in _var_info, recursing into groups
 /// as needed
-AP_Param *AP_Param::next_scalar(uint16_t *token, enum ap_var_type *ptype)
+AP_Param *AP_Param::next_scalar(ParamToken *token, enum ap_var_type *ptype)
 {
     AP_Param *ap;
     enum ap_var_type type;
@@ -696,7 +699,7 @@ float AP_Param::cast_to_float(enum ap_var_type type)
 // print the value of all variables
 void AP_Param::show_all(void)
 {
-    uint16_t token;
+    ParamToken token;
     AP_Param *ap;
     enum ap_var_type type;
 

libraries/AP_Common/AP_Param.h

@@ -74,6 +74,12 @@ public:
         const struct GroupInfo *group_info;
     };
 
+    // a token used for first()/next() state
+    typedef struct {
+        uint8_t key;
+        uint8_t group_element;
+    } ParamToken;
+
     // called once at startup to setup the _var_info[] table. This
     // will also check the EEPROM header and re-initialise it if the
     // wrong version is found
@@ -138,15 +144,15 @@ public:
     /// @return             The first variable in _var_info, or NULL if
     ///                     there are none.
     ///
-    static AP_Param *first(uint16_t *token, enum ap_var_type *ptype);
+    static AP_Param *first(ParamToken *token, enum ap_var_type *ptype);
 
     /// Returns the next variable in _var_info, recursing into groups
     /// as needed
-    static AP_Param *next(uint16_t *token, enum ap_var_type *ptype);
+    static AP_Param *next(ParamToken *token, enum ap_var_type *ptype);
 
     /// Returns the next scalar variable in _var_info, recursing into groups
     /// as needed
-    static AP_Param *next_scalar(uint16_t *token, enum ap_var_type *ptype);
+    static AP_Param *next_scalar(ParamToken *token, enum ap_var_type *ptype);
 
     /// cast a variable to a float given its type
     float cast_to_float(enum ap_var_type type);
@@ -203,7 +209,7 @@ private:
                                 bool *found_current,
                                 uint8_t group_base,
                                 uint8_t group_shift,
-                                uint16_t *token,
+                                ParamToken *token,
                                 enum ap_var_type *ptype);
 
     static uint16_t _eeprom_size;

libraries/RC_Channel/RC_Channel.cpp

@@ -108,6 +108,8 @@ RC_Channel::set_pwm(int pwm)
 	if(_type == RC_CHANNEL_RANGE){
 		//Serial.print("range ");
 		control_in = pwm_to_range();
+		//control_in = constrain(control_in, _low, _high);
+		control_in = min(control_in, _high);
 		control_in = (control_in < _dead_zone) ? 0 : control_in;
 
 		if (fabs(scale_output) != 1){