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

ArduCopter/ArduCopter.pde

@@ -1,6 +1,6 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "ArduCopter V2.2 b3"
+#define THISFIRMWARE "ArduCopter V2.2 b4"
 /*
 ArduCopter Version 2.2
 Authors:	Jason Short
@@ -323,6 +323,9 @@ static int16_t y_actual_speed;
 static int16_t x_rate_error;
 static int16_t y_rate_error;
 
+//static int16_t my_max_speed; // used for debugging logs
+//static int16_t target_x_rate;
+
 ////////////////////////////////////////////////////////////////////////////////
 // Radio
 ////////////////////////////////////////////////////////////////////////////////
@@ -360,8 +363,6 @@ static bool rc_override_active = false;
 // Status flag that tracks whether we are under GCS control
 static uint32_t rc_override_fs_timer = 0;
 
-
-
 ////////////////////////////////////////////////////////////////////////////////
 // Heli
 ////////////////////////////////////////////////////////////////////////////////
@@ -604,9 +605,9 @@ static int16_t 	landing_boost;
 ////////////////////////////////////////////////////////////////////////////////
 // The location of the copter in relation to home, updated every GPS read
 static int32_t	home_to_copter_bearing;
-// distance between plane and home in meters (not cm!!!)
+// distance between plane and home in cm
 static int32_t	home_distance;
-// distance between plane and next_WP in meters (not cm!!!)
+// distance between plane and next_WP in cm
 static int32_t	wp_distance;
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -1532,7 +1533,7 @@ void update_throttle_mode(void)
 				takeoff_complete = false;
 
 				// reset baro data if we are near home
-				if(home_distance < 4  || GPS_enabled == false){
+				if(home_distance < 400  || GPS_enabled == false){ // 4m from home
 					// causes Baro to do a quick recalibration
 					// XXX commented until further testing
 					// reset_baro();
@@ -1602,7 +1603,7 @@ void update_throttle_mode(void)
 					// get the AP throttle, if landing boost > 0 we are actually Landing
 					// This allows us to grab just the compensation.
 					if(landing_boost > 0)
-						nav_throttle = get_nav_throttle(-100);
+						nav_throttle = get_nav_throttle(-200);
 					else
 						nav_throttle = get_nav_throttle(altitude_error);
 
@@ -1743,7 +1744,7 @@ static void update_navigation()
 	// are we in SIMPLE mode?
 	if(do_simple && g.super_simple){
 		// get distance to home
-		if(home_distance > 10){ // 10m from home
+		if(home_distance > 1000){ // 10m from home
 			// we reset the angular offset to be a vector from home to the quad
 			initial_simple_bearing = home_to_copter_bearing;
 			//Serial.printf("ISB: %d\n", initial_simple_bearing);

ArduCopter/Attitude.pde

@@ -165,7 +165,8 @@ get_nav_throttle(int32_t z_error)
 	//output -= constrain(rate_d, -25, 25);
 
 	// light filter of output
-	output = (old_output * 3 + output) / 4;
+	//output = (old_output * 3 + output) / 4;
+	output = (old_output + output) / 2;
 
 	// save our output
 	old_output  = output;

ArduCopter/Camera.pde

@@ -4,6 +4,8 @@
 
 static void init_camera()
 {
+  APM_RC.enable_out(CH_CAM_PITCH);
+  APM_RC.enable_out(CH_CAM_ROLL);
 	// ch 6 high(right) is down.
 	g.rc_camera_pitch.set_angle(4500);
 	g.rc_camera_roll.set_angle(4500);

ArduCopter/GCS_Mavlink copy.txt

@@ -270,7 +270,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
 		nav_pitch / 1.0e2,
 		target_bearing / 1.0e2,
 		dcm.yaw_sensor / 1.0e2, // was target_bearing
-		wp_distance,
+		wp_distance / 1.0e2,
 		altitude_error / 1.0e2,
 		0,
 		crosstrack_error);

ArduCopter/GCS_Mavlink.pde

@@ -118,7 +118,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
         nav_pitch / 1.0e2,
         nav_bearing / 1.0e2,
         target_bearing / 1.0e2,
-        wp_distance,
+        wp_distance / 1.0e2,
         altitude_error / 1.0e2,
         0,
         crosstrack_error);	// was 0
@@ -1518,15 +1518,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             // set dcm hil sensor
             dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
             packet.pitchspeed,packet.yawspeed);
-        
+
-                    // rad/sec
+            // rad/sec
+            /*
             Vector3f gyros;
             gyros.x = (float)packet.rollspeed;
             gyros.y = (float)packet.pitchspeed;
             gyros.z = (float)packet.yawspeed;
-        
-            imu.set_gyro(gyros);
 
+            imu.set_gyro(gyros);
+			*/
             //imu.set_accel(accels);
             break;
         }

ArduCopter/Log.pde

@@ -528,16 +528,28 @@ static void Log_Write_Nav_Tuning()
 	DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
 
 	DataFlash.WriteInt(wp_distance);						// 1
-	DataFlash.WriteInt(target_bearing/100);					// 2
+	DataFlash.WriteInt(nav_bearing/100);					// 2
 	DataFlash.WriteInt(long_error);							// 3
 	DataFlash.WriteInt(lat_error);							// 4
 	DataFlash.WriteInt(nav_lon);							// 5
 	DataFlash.WriteInt(nav_lat);							// 6
-	DataFlash.WriteInt(g.pi_nav_lon.get_integrator());		// 7
+	DataFlash.WriteInt(x_actual_speed);						// 7
-	DataFlash.WriteInt(g.pi_nav_lat.get_integrator());	    // 8
+	DataFlash.WriteInt(y_actual_speed);					    // 8
 	DataFlash.WriteInt(g.pi_loiter_lon.get_integrator());	// 9
 	DataFlash.WriteInt(g.pi_loiter_lat.get_integrator());	// 10
 
+	/*DataFlash.WriteInt(wp_distance);						// 1
+	DataFlash.WriteInt(nav_bearing/100);					// 2
+	DataFlash.WriteInt(my_max_speed);						// 3
+	DataFlash.WriteInt(long_error);							// 4
+	DataFlash.WriteInt(x_actual_speed);						// 5
+	DataFlash.WriteInt(target_x_rate);						// 6
+	DataFlash.WriteInt(x_rate_error);						// 7
+	DataFlash.WriteInt(nav_lon_p);							// 8
+	DataFlash.WriteInt(g.pi_loiter_lon.get_integrator());	// 9
+	DataFlash.WriteInt(nav_lon);							// 10
+	*/
+
 	DataFlash.WriteByte(END_BYTE);
 }
 

ArduCopter/Parameters.h

@@ -107,7 +107,7 @@ public:
 	k_param_optflow_enabled,
 	k_param_low_voltage,
 	k_param_ch7_option,
-	k_param_sonar_type,  
+	k_param_sonar_type,
 	k_param_super_simple, //155
 
 	//
@@ -167,7 +167,7 @@ public:
 	//
 	// 235: PI/D Controllers
 	//
-	k_param_stablize_d = 234,
+	k_param_stabilize_d = 234,
 	k_param_pi_rate_roll = 235,
 	k_param_pi_rate_pitch,
 	k_param_pi_rate_yaw,
@@ -184,7 +184,7 @@ public:
 	k_param_pi_acro_pitch,
 	k_param_pi_optflow_roll,
 	k_param_pi_optflow_pitch,  // 250
-
+	k_param_loiter_d,
 
     // 254,255: reserved
 	};
@@ -286,6 +286,7 @@ public:
 	AP_Float	camera_pitch_gain;
 	AP_Float	camera_roll_gain;
 	AP_Float	stablize_d;
+	AP_Float	loiter_d;
 
 	// PI/D controllers
 	APM_PI		pi_rate_roll;
@@ -341,10 +342,10 @@ public:
 	command_total			(0,							k_param_command_total,					PSTR("WP_TOTAL")),
 	command_index			(0,							k_param_command_index,					PSTR("WP_INDEX")),
 	command_nav_index		(0,							k_param_command_nav_index,				PSTR("WP_MUST_INDEX")),
-	waypoint_radius			(WP_RADIUS_DEFAULT,			k_param_waypoint_radius,				PSTR("WP_RADIUS")),
+	waypoint_radius			(WP_RADIUS_DEFAULT * 100,	k_param_waypoint_radius,				PSTR("WP_RADIUS")),
 	loiter_radius			(LOITER_RADIUS,	    		k_param_loiter_radius,					PSTR("WP_LOITER_RAD")),
 	waypoint_speed_max		(WAYPOINT_SPEED_MAX,		k_param_waypoint_speed_max,				PSTR("WP_SPEED_MAX")),
-	crosstrack_gain			(CROSSTRACK_GAIN * 100,		k_param_crosstrack_gain,				PSTR("XTRK_GAIN_SC")),
+	crosstrack_gain			(CROSSTRACK_GAIN,			k_param_crosstrack_gain,				PSTR("XTRK_GAIN_SC")),
 	auto_land_timeout		(AUTO_LAND_TIME * 1000,		k_param_auto_land_timeout,				PSTR("AUTO_LAND")),
 
 	throttle_min			(0,							k_param_throttle_min,					PSTR("THR_MIN")),
@@ -409,7 +410,8 @@ public:
 	camera_pitch_gain 		(CAM_PITCH_GAIN, 			k_param_camera_pitch_gain, 				PSTR("CAM_P_G")),
 	camera_roll_gain 		(CAM_ROLL_GAIN, 			k_param_camera_roll_gain,	 			PSTR("CAM_R_G")),
 
-	stablize_d 				(STABILIZE_D, 				k_param_stablize_d,	 					PSTR("STAB_D")),
+	stablize_d 				(STABILIZE_D, 				k_param_stabilize_d,	 				PSTR("STAB_D")),
+	loiter_d 				(LOITER_D, 					k_param_loiter_d,	 					PSTR("LOITER_D")),
 
 	// PI controller	group key						name				initial P			initial I			initial imax
 	//--------------------------------------------------------------------------------------------------------------------------------------------------------------------

ArduCopter/commands.pde

@@ -178,7 +178,7 @@ static void set_next_WP(struct Location *wp)
 
 	// reset speed governer
 	// --------------------
-	waypoint_speed_gov = 0;
+	waypoint_speed_gov = WAYPOINT_SPEED_MIN;
 }
 
 

ArduCopter/commands_logic.pde

@@ -365,7 +365,7 @@ static bool verify_land()
 		//landing_boost 	= min(landing_boost, 15);
 		float tmp = (1.75 * icount * icount) - (7.2 * icount);
 		landing_boost = tmp;
-		landing_boost = constrain(landing_boost, 0, 200);
+		landing_boost = constrain(landing_boost, 1, 200);
 		icount += 0.4;
 
 		//Serial.printf("lb:%d, %1.4f, ic:%1.4f\n",landing_boost, tmp, icount);
@@ -512,7 +512,7 @@ static void do_change_alt()
 
 static void do_within_distance()
 {
-	condition_value	 = command_cond_queue.lat;
+	condition_value	 = command_cond_queue.lat * 100;
 }
 
 static void do_yaw()

ArduCopter/config.h

@@ -622,20 +622,23 @@
 # define LOITER_P			1.5		// was .25 in previous
 #endif
 #ifndef LOITER_I
-# define LOITER_I			0.04	// Wind control
+# define LOITER_I			0.09	// Wind control
 #endif
 #ifndef LOITER_IMAX
 # define LOITER_IMAX		30		// degrees°
 #endif
+#ifndef LOITER_D
+# define LOITER_D			3		// rate control
+#endif
 
 //////////////////////////////////////////////////////////////////////////////
 // WP Navigation control gains
 //
 #ifndef NAV_P
-# define NAV_P				2.2			// 3 was causing rapid oscillations in Loiter
+# define NAV_P				3.0			// 3 was causing rapid oscillations in Loiter
 #endif
 #ifndef NAV_I
-# define NAV_I				0.15		// used in traverals
+# define NAV_I				0			//
 #endif
 #ifndef NAV_IMAX
 # define NAV_IMAX			30			// degrees
@@ -645,6 +648,10 @@
 # define WAYPOINT_SPEED_MAX			600			// for 6m/s error = 13mph
 #endif
 
+#ifndef WAYPOINT_SPEED_MIN
+# define WAYPOINT_SPEED_MIN			100			// for 6m/s error = 13mph
+#endif
+
 
 //////////////////////////////////////////////////////////////////////////////
 // Throttle control gains

ArduCopter/motors_hexa.pde

@@ -10,6 +10,16 @@ static void init_motors_out()
 	#endif
 }
 
+static void motors_output_enable()
+{
+  APM_RC.enable_out(MOT_1);
+  APM_RC.enable_out(MOT_2);
+  APM_RC.enable_out(MOT_3);
+  APM_RC.enable_out(MOT_4);
+  APM_RC.enable_out(MOT_5);
+  APM_RC.enable_out(MOT_6);
+}
+
 static void output_motors_armed()
 {
 	int roll_out, pitch_out;

ArduCopter/motors_octa.pde

@@ -10,6 +10,18 @@ static void init_motors_out()
 	#endif
 }
 
+static void motors_output_enable()
+{
+  APM_RC.enable_out(MOT_1);
+  APM_RC.enable_out(MOT_2);
+  APM_RC.enable_out(MOT_3);
+  APM_RC.enable_out(MOT_4);
+  APM_RC.enable_out(MOT_5);
+  APM_RC.enable_out(MOT_6);
+  APM_RC.enable_out(MOT_7);
+  APM_RC.enable_out(MOT_8);
+}
+
 static void output_motors_armed()
 {
 	int roll_out, pitch_out;

ArduCopter/motors_octa_quad.pde

@@ -10,6 +10,18 @@ static void init_motors_out()
 	#endif
 }
 
+static void motors_output_enable()
+{
+  APM_RC.enable_out(MOT_1);
+  APM_RC.enable_out(MOT_2);
+  APM_RC.enable_out(MOT_3);
+  APM_RC.enable_out(MOT_4);
+  APM_RC.enable_out(MOT_5);
+  APM_RC.enable_out(MOT_6);
+  APM_RC.enable_out(MOT_7);
+  APM_RC.enable_out(MOT_8);
+}
+
 static void output_motors_armed()
 {
 	int roll_out, pitch_out;

ArduCopter/motors_quad.pde

@@ -9,6 +9,14 @@ static void init_motors_out()
 	#endif
 }
 
+static void motors_output_enable()
+{
+  APM_RC.enable_out(MOT_1);
+  APM_RC.enable_out(MOT_2);
+  APM_RC.enable_out(MOT_3);
+  APM_RC.enable_out(MOT_4);
+}
+
 static void output_motors_armed()
 {
 	int roll_out, pitch_out;

ArduCopter/motors_tri.pde

@@ -8,6 +8,13 @@ static void init_motors_out()
 	#endif
 }
 
+static void motors_output_enable()
+{
+  APM_RC.enable_out(MOT_1);
+  APM_RC.enable_out(MOT_2);
+  APM_RC.enable_out(MOT_4);
+}
+
 
 static void output_motors_armed()
 {

ArduCopter/motors_y6.pde

@@ -12,6 +12,16 @@ static void init_motors_out()
 	#endif
 }
 
+static void motors_output_enable()
+{
+  APM_RC.enable_out(MOT_1);
+  APM_RC.enable_out(MOT_2);
+  APM_RC.enable_out(MOT_3);
+  APM_RC.enable_out(MOT_4);
+  APM_RC.enable_out(MOT_5);
+  APM_RC.enable_out(MOT_6);
+}
+
 static void output_motors_armed()
 {
 	int out_min = g.rc_3.radio_min;

ArduCopter/navigation.pde

@@ -143,13 +143,13 @@ static void calc_location_error(struct Location *next_loc)
 */
 
 #define NAV_ERR_MAX 800
-static void calc_loiter1(int x_error, int y_error)
+static void calc_loiter(int x_error, int y_error)
 {
 	int16_t lon_PI 	= g.pi_loiter_lon.get_pi(x_error, dTnav);
-	int16_t lon_D 	= 3 * x_actual_speed ; // this controls the small bumps
+	int16_t lon_D 	= g.loiter_d * x_actual_speed ; // this controls the small bumps
 
 	int16_t lat_PI 	= g.pi_loiter_lat.get_pi(y_error, dTnav);
-	int16_t lat_D 	= 3 * y_actual_speed ;
+	int16_t lat_D 	= g.loiter_d * y_actual_speed ;
 
 	//limit of terms
 	lon_D = constrain(lon_D,-500,500);
@@ -160,14 +160,15 @@ static void calc_loiter1(int x_error, int y_error)
 }
 
 
-static void calc_loiter(int x_error, int y_error)
+static void calc_loiter1(int x_error, int y_error)
 {
 	// East/West
 	x_error 				= constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX);	//800
 	int16_t x_target_speed 	= g.pi_loiter_lon.get_p(x_error);
 	int16_t x_iterm 		= g.pi_loiter_lon.get_i(x_error, dTnav);
 	x_rate_error 			= x_target_speed - x_actual_speed;
-	nav_lon_p		 		= g.pi_nav_lon.get_p(x_rate_error);
+	nav_lon_p		 		= x_rate_error * g.loiter_d;
+
 	nav_lon_p				= constrain(nav_lon_p, -1200, 1200);
 	nav_lon					= nav_lon_p + x_iterm;
 	nav_lon					= constrain(nav_lon, -2500, 2500);
@@ -177,7 +178,8 @@ static void calc_loiter(int x_error, int y_error)
 	int16_t y_target_speed 	= g.pi_loiter_lat.get_p(y_error);
 	int16_t y_iterm 		= g.pi_loiter_lat.get_i(y_error, dTnav);
 	y_rate_error 			= y_target_speed - y_actual_speed;
-	nav_lat_p	 			= g.pi_nav_lat.get_p(y_rate_error);
+	nav_lat_p		 		= y_rate_error * g.loiter_d;
+
 	nav_lat_p				= constrain(nav_lat_p, -1200, 1200);
 	nav_lat					= nav_lat_p + y_iterm;
 	nav_lat					= constrain(nav_lat, -2500, 2500);
@@ -270,21 +272,20 @@ static int16_t calc_desired_speed(int16_t max_speed)
 	*/
 
 	// max_speed is default 600 or 6m/s
-	// (wp_distance * 50) = 1/2 of the distance converted to speed
+	// (wp_distance * .5) = 1/2 of the distance converted to speed
 	// wp_distance is always in m/s and not cm/s - I know it's stupid that way
 	// for example 4m from target = 200cm/s speed
 	// we choose the lowest speed based on disance
-	max_speed 		= min(max_speed, (wp_distance * 100));
+	max_speed 		= min(max_speed, wp_distance);
+
+	// 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
-	if(waypoint_speed_gov < max_speed){
+	if(max_speed > waypoint_speed_gov){
 		waypoint_speed_gov += (int)(100.0 * dTnav); // increase at .5/ms
-
+		max_speed = waypoint_speed_gov;
-		// go at least 100cm/s
-		max_speed 		= max(100, waypoint_speed_gov);
-		// limit with governer
-		max_speed 		= min(max_speed, waypoint_speed_gov);
 	}
 
 	return max_speed;
@@ -296,7 +297,7 @@ static void calc_nav_rate(int max_speed)
 	update_crosstrack();
 
 	// nav_bearing includes crosstrack
-	float temp 		= (9000 - nav_bearing) * RADX100;
+	float temp 			= (9000l - nav_bearing) * RADX100;
 
 	x_rate_error 		= (cos(temp) * max_speed) - x_actual_speed; // 413
 	x_rate_error 		= constrain(x_rate_error, -1000, 1000);
@@ -398,7 +399,6 @@ static void set_new_altitude(int32_t _new_alt)
 		alt_change_flag = REACHED_ALT;
 		//Serial.printf("reached alt\n");
 	}
-
 	//Serial.printf("new alt: %d Org alt: %d\n", target_altitude, original_altitude);
 }
 
@@ -490,16 +490,14 @@ static int32_t wrap_180(int32_t error)
 	return current_loc.alt - home.alt;
 }
 */
+
 // distance is returned in meters
 static int32_t get_distance(struct Location *loc1, struct Location *loc2)
 {
-	//if(loc1->lat == 0 || loc1->lng == 0)
-	//	return -1;
-	//if(loc2->lat == 0 || loc2->lng == 0)
-	//	return -1;
 	float dlat 		= (float)(loc2->lat - loc1->lat);
 	float dlong		= ((float)(loc2->lng - loc1->lng)) * scaleLongDown;
-	return sqrt(sq(dlat) + sq(dlong)) * .01113195;
+	dlong			= sqrt(sq(dlat) + sq(dlong)) * 1.113195;
+	return			dlong;
 }
 /*
 //static int32_t get_alt_distance(struct Location *loc1, struct Location *loc2)

ArduCopter/radio.pde

@@ -100,6 +100,7 @@ static void init_rc_out()
 
 void output_min()
 {
+  motors_output_enable();
     #if FRAME_CONFIG ==	HELI_FRAME
         heli_move_servos_to_mid();
 	#else
@@ -158,7 +159,7 @@ static void throttle_failsafe(uint16_t pwm)
 			// Don't enter Failsafe if we are not armed
 			// home distance is in meters
 			// This is to prevent accidental RTL
-			if((motor_armed == true) && (home_distance > 10) && (current_loc.alt > 400)){
+			if((motor_armed == true) && (home_distance > 1000) && (current_loc.alt > 400)){
 				SendDebug("MSG FS ON ");
 				SendDebugln(pwm, DEC);
 				set_failsafe(true);

ArduCopter/setup.pde

@@ -1096,6 +1096,7 @@ static void print_enabled(boolean b)
 static void
 init_esc()
 {
+  motors_output_enable();
 	while(1){
 		read_radio();
 		delay(100);

ArduPlane/radio.pde

@@ -33,18 +33,17 @@ static void init_rc_in()
 
 static void init_rc_out()
 {
-	APM_RC.OutputCh(CH_1, 	g.channel_roll.radio_trim);					// Initialization of servo outputs
-	APM_RC.OutputCh(CH_2, 	g.channel_pitch.radio_trim);
-	APM_RC.OutputCh(CH_3, 	g.channel_throttle.radio_min);
-	APM_RC.OutputCh(CH_4, 	g.channel_rudder.radio_trim);
-
-	APM_RC.OutputCh(CH_5, 	g.rc_5.radio_trim);
-	APM_RC.OutputCh(CH_6, 	g.rc_6.radio_trim);
-	APM_RC.OutputCh(CH_7,   g.rc_7.radio_trim);
-	APM_RC.OutputCh(CH_8,   g.rc_8.radio_trim);
-
 	APM_RC.Init( &isr_registry );		// APM Radio initialization
 
+  APM_RC.enable_out(CH_1);
+  APM_RC.enable_out(CH_2);
+  APM_RC.enable_out(CH_3);
+  APM_RC.enable_out(CH_4);
+  APM_RC.enable_out(CH_5);
+  APM_RC.enable_out(CH_6);
+  APM_RC.enable_out(CH_7);
+  APM_RC.enable_out(CH_8);
+
 	APM_RC.OutputCh(CH_1, 	g.channel_roll.radio_trim);					// Initialization of servo outputs
 	APM_RC.OutputCh(CH_2, 	g.channel_pitch.radio_trim);
 	APM_RC.OutputCh(CH_3, 	g.channel_throttle.radio_min);
@@ -53,7 +52,7 @@ static void init_rc_out()
 	APM_RC.OutputCh(CH_5, 	g.rc_5.radio_trim);
 	APM_RC.OutputCh(CH_6, 	g.rc_6.radio_trim);
 	APM_RC.OutputCh(CH_7,   g.rc_7.radio_trim);
-    APM_RC.OutputCh(CH_8,   g.rc_8.radio_trim);
+  APM_RC.OutputCh(CH_8,   g.rc_8.radio_trim);
 }
 
 static void read_radio()

libraries/APM_RC/APM_RC.h

@@ -41,6 +41,9 @@ class APM_RC_Class
 	virtual void clearOverride(void) = 0;
     virtual void Force_Out() = 0;
     virtual void SetFastOutputChannels( uint32_t channelmask ) = 0;
+  virtual void enable_out(uint8_t) = 0;
+  virtual void disable_out(uint8_t) = 0;
+
 };
 
 

libraries/APM_RC/APM_RC_APM1.cpp

@@ -84,7 +84,7 @@ void APM_RC_APM1::Init( Arduino_Mega_ISR_Registry * isr_reg )
   pinMode(13,OUTPUT); //OUT3 (PB7/OC1C)
 
   //Remember the registers not declared here remains zero by default...
-  TCCR1A =((1<<WGM11)|(1<<COM1A1)|(1<<COM1B1)|(1<<COM1C1)); //Please read page 131 of DataSheet, we are changing the registers settings of WGM11,COM1B1,COM1A1 to 1 thats all...
+  TCCR1A =((1<<WGM11)); //Please read page 131 of DataSheet, we are changing the registers settings of WGM11,COM1B1,COM1A1 to 1 thats all...
   TCCR1B = (1<<WGM13)|(1<<WGM12)|(1<<CS11); //Prescaler set to 8, that give us a resolution of 0.5us, read page 134 of data sheet
   OCR1A = 0xFFFF; // Init ODR registers to nil output signal
   OCR1B = 0xFFFF;
@@ -95,7 +95,7 @@ void APM_RC_APM1::Init( Arduino_Mega_ISR_Registry * isr_reg )
   pinMode(2,OUTPUT); //OUT7 (PE4/OC3B)
   pinMode(3,OUTPUT); //OUT6 (PE5/OC3C)
   pinMode(5,OUTPUT); //OUT10(PE3/OC3A)
-  TCCR3A =((1<<WGM31)|(1<<COM3A1)|(1<<COM3B1)|(1<<COM3C1));
+  TCCR3A =((1<<WGM31));
   TCCR3B = (1<<WGM33)|(1<<WGM32)|(1<<CS31);
   OCR3A = 0xFFFF; // Init ODR registers to nil output signal
   OCR3B = 0xFFFF;
@@ -107,7 +107,7 @@ void APM_RC_APM1::Init( Arduino_Mega_ISR_Registry * isr_reg )
   pinMode(45,OUTPUT);  //OUT0 (PL4/OC5B)
   pinMode(46,OUTPUT);  //OUT8 (PL3/OC5A)
 
-  TCCR5A =((1<<WGM51)|(1<<COM5A1)|(1<<COM5B1)|(1<<COM5C1));
+  TCCR5A =((1<<WGM51));
   TCCR5B = (1<<WGM53)|(1<<WGM52)|(1<<CS51);
   OCR5A = 0xFFFF; // Init ODR registers to nil output signal
   OCR5B = 0xFFFF;
@@ -119,7 +119,7 @@ void APM_RC_APM1::Init( Arduino_Mega_ISR_Registry * isr_reg )
   pinMode(7,OUTPUT);   //OUT5 (PH4/OC4B)
   pinMode(8,OUTPUT);   //OUT4 (PH5/OC4C)
 
-  TCCR4A =((1<<WGM40)|(1<<WGM41)|(1<<COM4C1)|(1<<COM4B1)|(1<<COM4A1));
+  TCCR4A =((1<<WGM40)|(1<<WGM41));
   //Prescaler set to 8, that give us a resolution of 0.5us
   // Input Capture rising edge
   TCCR4B = ((1<<WGM43)|(1<<WGM42)|(1<<CS41)|(1<<ICES4));
@@ -153,6 +153,40 @@ void APM_RC_APM1::OutputCh(uint8_t ch, uint16_t pwm)
   }
 }
 
+void APM_RC_APM1::enable_out(uint8_t ch)
+{
+ switch(ch){
+    case 0:  TCCR5A |= (1<<COM5B1); break;  // CH_1 : OC5B
+    case 1:  TCCR5A |= (1<<COM5C1); break;  // CH_2 : OC5C
+    case 2:  TCCR1A |= (1<<COM1B1); break;  // CH_3 : OC1B
+    case 3:  TCCR1A |= (1<<COM1C1); break;  // CH_4 : OC1C
+    case 4:  TCCR4A |= (1<<COM4C1); break;  // CH_5 : OC4C
+    case 5:  TCCR4A |= (1<<COM4B1); break;  // CH_6 : OC4B
+    case 6:  TCCR3A |= (1<<COM3C1); break;  // CH_7 : OC3C
+    case 7:  TCCR3A |= (1<<COM3B1); break;  // CH_8 : OC3B
+    case 8:  TCCR5A |= (1<<COM5A1); break;  // CH_9 : OC5A
+    case 9:  TCCR1A |= (1<<COM1A1); break;  // CH_10: OC1A
+    case 10: TCCR3A |= (1<<COM3A1); break;  // CH_11: OC3A
+  }
+}
+
+void APM_RC_APM1::disable_out(uint8_t ch)
+{
+ switch(ch){
+    case 0:  TCCR5A &= ~(1<<COM5B1); break;  // CH_1 : OC5B
+    case 1:  TCCR5A &= ~(1<<COM5C1); break;  // CH_2 : OC5C
+    case 2:  TCCR1A &= ~(1<<COM1B1); break;  // CH_3 : OC1B
+    case 3:  TCCR1A &= ~(1<<COM1C1); break;  // CH_4 : OC1C
+    case 4:  TCCR4A &= ~(1<<COM4C1); break;  // CH_5 : OC4C
+    case 5:  TCCR4A &= ~(1<<COM4B1); break;  // CH_6 : OC4B
+    case 6:  TCCR3A &= ~(1<<COM3C1); break;  // CH_7 : OC3C
+    case 7:  TCCR3A &= ~(1<<COM3B1); break;  // CH_8 : OC3B
+    case 8:  TCCR5A &= ~(1<<COM5A1); break;  // CH_9 : OC5A
+    case 9:  TCCR1A &= ~(1<<COM1A1); break;  // CH_10: OC1A
+    case 10: TCCR3A &= ~(1<<COM3A1); break;  // CH_11: OC3A
+  }
+}
+
 uint16_t APM_RC_APM1::InputCh(uint8_t ch)
 {
   uint16_t result;

libraries/APM_RC/APM_RC_APM1.h

@@ -20,6 +20,9 @@ class APM_RC_APM1 : public APM_RC_Class
 	void Force_Out(void);
 	void SetFastOutputChannels(uint32_t chmask);
 
+  void enable_out(uint8_t);
+  void disable_out(uint8_t);
+
 	void Force_Out0_Out1(void);
 	void Force_Out2_Out3(void);
 	void Force_Out6_Out7(void);

libraries/APM_RC/APM_RC_APM2.cpp

@@ -80,9 +80,8 @@ void APM_RC_APM2::Init( Arduino_Mega_ISR_Registry * isr_reg )
   pinMode(11,OUTPUT); // OUT2 (PB5/OC1A)
 
   // WGM: 1 1 1 0. Clear Timer on Compare, TOP is ICR1.
-  // COM1A and COM1B enabled, set to low level on match.
   // CS11: prescale by 8 => 0.5us tick
-  TCCR1A =((1<<WGM11)|(1<<COM1A1)|(1<<COM1B1));
+  TCCR1A =((1<<WGM11));
   TCCR1B = (1<<WGM13)|(1<<WGM12)|(1<<CS11);
   ICR1 = 40000; // 0.5us tick => 50hz freq
   OCR1A = 0xFFFF; // Init OCR registers to nil output signal
@@ -94,9 +93,8 @@ void APM_RC_APM2::Init( Arduino_Mega_ISR_Registry * isr_reg )
   pinMode(6,OUTPUT); // OUT5 (PH3/OC4A)
 
   // WGM: 1 1 1 0. Clear Timer on Compare, TOP is ICR4.
-  // COM4A, 4B, 4C enabled, set to low level on match.
   // CS41: prescale by 8 => 0.5us tick
-  TCCR4A =((1<<WGM41)|(1<<COM4A1)|(1<<COM4B1)|(1<<COM4C1));
+  TCCR4A =((1<<WGM41));
   TCCR4B = (1<<WGM43)|(1<<WGM42)|(1<<CS41);
   OCR4A = 0xFFFF; // Init OCR registers to nil output signal
   OCR4B = 0xFFFF;
@@ -109,9 +107,8 @@ void APM_RC_APM2::Init( Arduino_Mega_ISR_Registry * isr_reg )
   pinMode(5,OUTPUT); // OUT8 (PE3/OC3A)
 
   // WGM: 1 1 1 0. Clear timer on Compare, TOP is ICR3
-  // COM3A, 3B, 3C enabled, set to low level on match
   // CS31: prescale by 8 => 0.5us tick
-  TCCR3A =((1<<WGM31)|(1<<COM3A1)|(1<<COM3B1)|(1<<COM3C1));
+  TCCR3A =((1<<WGM31));
   TCCR3B = (1<<WGM33)|(1<<WGM32)|(1<<CS31);
   OCR3A = 0xFFFF; // Init OCR registers to nil output signal
   OCR3B = 0xFFFF;
@@ -155,6 +152,34 @@ void APM_RC_APM2::OutputCh(unsigned char ch, uint16_t pwm)
   }
 }
 
+void APM_RC_APM2::enable_out(uint8_t ch)
+{
+  switch(ch) {
+    case 0: TCCR1A |= (1<<COM1B1); break; // CH_1 : OC1B
+    case 1: TCCR1A |= (1<<COM1A1); break; // CH_2 : OC1A
+    case 2: TCCR4A |= (1<<COM4C1); break; // CH_3 : OC4C
+    case 3: TCCR4A |= (1<<COM4B1); break; // CH_4 : OC4B
+    case 4: TCCR4A |= (1<<COM4A1); break; // CH_5 : OC4A
+    case 5: TCCR3A |= (1<<COM3C1); break; // CH_6 : OC3C
+    case 6: TCCR3A |= (1<<COM3B1); break; // CH_7 : OC3B
+    case 7: TCCR3A |= (1<<COM3A1); break; // CH_8 : OC3A
+  }
+}
+
+void APM_RC_APM2::disable_out(uint8_t ch)
+{
+  switch(ch) {
+    case 0: TCCR1A &= ~(1<<COM1B1); break; // CH_1 : OC1B
+    case 1: TCCR1A &= ~(1<<COM1A1); break; // CH_2 : OC1A
+    case 2: TCCR4A &= ~(1<<COM4C1); break; // CH_3 : OC4C
+    case 3: TCCR4A &= ~(1<<COM4B1); break; // CH_4 : OC4B
+    case 4: TCCR4A &= ~(1<<COM4A1); break; // CH_5 : OC4A
+    case 5: TCCR3A &= ~(1<<COM3C1); break; // CH_6 : OC3C
+    case 6: TCCR3A &= ~(1<<COM3B1); break; // CH_7 : OC3B
+    case 7: TCCR3A &= ~(1<<COM3A1); break; // CH_8 : OC3A
+  }
+}
+
 uint16_t APM_RC_APM2::InputCh(unsigned char ch)
 {
   uint16_t result;

libraries/APM_RC/APM_RC_APM2.h

@@ -22,6 +22,9 @@ class APM_RC_APM2 : public APM_RC_Class
     void Force_Out(void);
     void SetFastOutputChannels(uint32_t chmask);
 
+  void enable_out(uint8_t);
+  void disable_out(uint8_t);
+
 	void Force_Out0_Out1(void);
 	void Force_Out2_Out3(void);
 	void Force_Out6_Out7(void);

libraries/PID/examples/pid/Makefile

@@ -1,2 +1 @@
-BOARD	=	mega
 include ../../../AP_Common/Arduino.mk

libraries/PID/examples/pid/pid.pde

@@ -4,20 +4,30 @@
 */
 
 #include <FastSerial.h>
+#include <avr/pgmspace.h>
 #include <AP_Common.h>
 #include <APM_RC.h> // ArduPilot RC Library
 #include <PID.h> // ArduPilot Mega RC Library
+#include <Arduino_Mega_ISR_Registry.h>
 
 long radio_in;
 long radio_trim;
 
-PID pid(10, "TEST1_");
+Arduino_Mega_ISR_Registry isr_registry;
+
+#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
+    APM_RC_APM2 APM_RC;
+#else
+    APM_RC_APM1 APM_RC;
+#endif
+
+PID pid(AP_Var::k_key_none, NULL);
 
 void setup()
 {
 	Serial.begin(38400);
 	Serial.println("ArduPilot Mega PID library test");
-	APM_RC.Init();		// APM Radio initialization
+	APM_RC.Init(&isr_registry);		// APM Radio initialization
 
 	delay(1000);
 	//rc.trim();