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

Conflicts:
	ArduCopter/APM_Config.h

ArduCopter/APM_Config.h

@@ -7,8 +7,6 @@
 
 // GPS is auto-selected
 
-#define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
-
 //#define MAG_ORIENTATION		AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
 //#define HIL_MODE				HIL_MODE_ATTITUDE
 
@@ -66,8 +64,4 @@
 #define USERHOOK_VARIABLES "UserVariables.h"
 
 
-// enable this for the new 'APM2' hardware
+//# define LOGGING_ENABLED		DISABLED
-// #define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
-// #define APM2_BETA_HARDWARE  // for developers who received an early beta board with the older BMP085
-
-//# define LOGGING_ENABLED		DISABLED

ArduCopter/ArduCopter.pde

@@ -1,8 +1,8 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "ArduCopter V2.1.1r10 alpha"
+#define THISFIRMWARE "ArduCopter V2.2"
 /*
-ArduCopter Version 2.0 Beta
+ArduCopter Version 2.2
 Authors:	Jason Short
 Based on code and ideas from the Arducopter team: Jose Julio, Randy Mackay, Jani Hirvinen
 Thanks to:	Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier
@@ -598,11 +598,12 @@ static boolean	takeoff_complete;
 static int32_t	takeoff_timer;
 // Used to see if we have landed and if we should shut our engines - not fully implemented
 static boolean	land_complete = true;
-
 // used to manually override throttle in interactive Alt hold modes
 static int16_t 	manual_boost;
 // An additional throttle added to keep the copter at the same altitude when banking
 static int16_t 	angle_boost;
+// Push copter down for clean landing
+static uint8_t 	landing_boost;
 
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -900,6 +901,10 @@ static void fast_loop()
 	// IMU DCM Algorithm
 	read_AHRS();
 
+	if(GPS_enabled){
+		update_GPS();
+	}
+
 	// custom code/exceptions for flight modes
 	// ---------------------------------------
 	update_yaw_mode();
@@ -930,10 +935,6 @@ static void medium_loop()
 		case 0:
 			medium_loopCounter++;
 
-			if(GPS_enabled){
-				update_GPS();
-			}
-
 			#if HIL_MODE != HIL_MODE_ATTITUDE					// don't execute in HIL mode
 				if(g.compass_enabled){
 					if (compass.read()) {
@@ -961,10 +962,6 @@ static void medium_loop()
 				// clear nav flag
 				nav_ok = false;
 
-				// invalidate GPS data
-				// -------------------
-				g_gps->new_data 	= false;
-
 				// calculate the copter's desired bearing and WP distance
 				// ------------------------------------------------------
 				if(navigate()){
@@ -1295,11 +1292,15 @@ static void update_GPS(void)
 	}else{
 		// after 12 reads we guess we may have lost GPS signal, stop navigating
 		// we have lost GPS signal for a moment. Reduce our error to avoid flyaways
-		nav_roll  >>= 1;
+		nav_roll  = 0;
-		nav_pitch >>= 1;
+		nav_pitch = 0;
 	}
 
     if (g_gps->new_data && g_gps->fix) {
+
+		// clear new data flag
+    	g_gps->new_data = false;
+
 		gps_watchdog = 0;
 
 		// OK to run the nav routines
@@ -1418,16 +1419,9 @@ void update_roll_pitch_mode(void)
 				update_simple_mode();
 			}
 
-			#if WIND_COMP_STAB == 1
+			// in this mode, nav_roll and nav_pitch = the iterm
-				// in this mode, nav_roll and nav_pitch = the iterm
+			g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
-				g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in  + nav_roll);
+			g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
-				g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in + nav_pitch);
-			#else
-				// in this mode, nav_roll and nav_pitch = the iterm
-				g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
-				g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
-			#endif
-
 			break;
 
 		case ROLL_PITCH_AUTO:
@@ -1616,9 +1610,9 @@ void update_throttle_mode(void)
 				}
 
 				#if FRAME_CONFIG == HELI_FRAME
-					throttle_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle + get_z_damping());
+					throttle_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle + get_z_damping() - landing_boost);
 				#else
-					throttle_out = g.throttle_cruise + nav_throttle + angle_boost + get_z_damping();
+					throttle_out = g.throttle_cruise + nav_throttle + angle_boost + get_z_damping() - landing_boost;
 				#endif
 			}
 
@@ -1702,6 +1696,7 @@ static void update_navigation()
 				wp_control = LOITER_MODE;
 			}
 
+			// Kick us out of loiter and begin landing if the auto_land_timer is set
 			if(auto_land_timer != 0 && (millis() - auto_land_timer) > 20000){
 				// just to make sure we clear the timer
 				auto_land_timer = 0;
@@ -1713,10 +1708,6 @@ static void update_navigation()
 			break;
 
 		case LAND:
-			wp_control 		= LOITER_MODE;
-
-			// verify land will override WP_control if we are below
-			// a certain altitude
 			verify_land();
 
 			// calculates the desired Roll and Pitch
@@ -1835,6 +1826,7 @@ static void update_altitude()
 		#else
 			// This is real life
 			// calc the vertical accel rate
+			// positive = going up.
 			sonar_rate 		= (sonar_alt - old_sonar_alt) * 10;
 			old_sonar_alt 	= sonar_alt;
 		#endif
@@ -2006,6 +1998,7 @@ static void tuning(){
 	}
 }
 
+// Outputs Nav_Pitch and Nav_Roll
 static void update_nav_wp()
 {
 	if(wp_control == LOITER_MODE){
@@ -2071,24 +2064,17 @@ static void update_nav_wp()
 		// use error as the desired rate towards the target
 		calc_nav_rate(speed);
 		// rotate pitch and roll to the copter frame of reference
-		//calc_nav_pitch_roll();
 		calc_loiter_pitch_roll();
 
 	}else if(wp_control == NO_NAV_MODE){
-		// calc the Iterms for Loiter based on velocity
+		// clear out our nav so we can do things like land straight down
-		#if WIND_COMP_STAB == 1
-			if (g_gps->ground_speed < 50)
-				calc_wind_compensation();
-			else
-				reduce_wind_compensation();
 
-			// rotate nav_lat, nav_lon to roll and pitch
+		// We bring in our iterms for wind control, but we don't navigate
-			calc_loiter_pitch_roll();
+		nav_lon	= g.pi_loiter_lon.get_integrator();
-		#else
+		nav_lat = g.pi_loiter_lat.get_integrator();
-			// clear out our nav so we can do things like land straight
+
-			nav_pitch 	= 0;
+		// rotate pitch and roll to the copter frame of reference
-			nav_roll 	= 0;
+		calc_loiter_pitch_roll();
-		#endif
 	}
 }
 

ArduCopter/Attitude.pde

@@ -166,6 +166,7 @@ static int16_t
 get_nav_throttle(int32_t z_error)
 {
 	static int16_t old_output = 0;
+	static int16_t rate_d = 0;
 
 	int16_t rate_error;
 	int16_t output;
@@ -185,6 +186,12 @@ get_nav_throttle(int32_t z_error)
 	// limit the rate - iterm is not used
 	output =  constrain((int)g.pi_throttle.get_p(rate_error), -160, 180);
 
+	// a positive climb rate means we're going up
+	rate_d =  ((rate_d + climb_rate)>>1) * 1; // replace with gain
+
+	// slight adjustment to alt hold output
+	output -= constrain(rate_d, -25, 25);
+
 	// light filter of output
 	output = (old_output * 3 + output) / 4;
 
@@ -222,15 +229,6 @@ get_rate_yaw(int32_t target_rate)
 	return (int)constrain(target_rate, -2500, 2500);
 }
 
-
-// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
-// Keeps outdated data out of our calculations
-//static void reset_hold_I(void)
-//{
-//	g.pi_loiter_lat.reset_I();
-//	g.pi_loiter_lon.reset_I();
-//}
-
 // Keeps old data out of our calculation / logs
 static void reset_nav(void)
 {

ArduCopter/commands_logic.pde

@@ -359,16 +359,20 @@ static bool verify_land()
 	// remenber altitude for climb_rate
 	old_alt = current_loc.alt;
 
-	if ((current_loc.alt - home.alt) < 250){
+	if ((current_loc.alt - home.alt) < 200){
 		// don't bank to hold position
 		wp_control = NO_NAV_MODE;
 		// try and come down faster
-		// by setting next_WP really low.
+		landing_boost++;
-		set_new_altitude(-1000);
+		landing_boost 	= min(landing_boost, 20);
+	}else{
+		landing_boost 	= 0;
+		wp_control 		= LOITER_MODE;
 	}
 
 	if((current_loc.alt - home.alt)  < 100 && velocity_land <= 50){
 		land_complete = true;
+		landing_boost = 0;
 
 		// reset old_alt
 		old_alt = 0;

ArduCopter/config.h

@@ -487,13 +487,6 @@
 #endif
 
 
-// experimental feature for
-#ifndef WIND_COMP_STAB
-# define WIND_COMP_STAB		0
-#endif
-
-
-
 //////////////////////////////////////////////////////////////////////////////
 // Attitude Control
 //
@@ -618,10 +611,10 @@
 // Loiter control gains
 //
 #ifndef LOITER_P
-# define LOITER_P			2.4		// was .25 in previous
+# define LOITER_P			2.0		// was .25 in previous
 #endif
 #ifndef LOITER_I
-# define LOITER_I			0.1	// Wind control
+# define LOITER_I			0.05	// Wind control
 #endif
 #ifndef LOITER_IMAX
 # define LOITER_IMAX		30		// degrees°

ArduCopter/navigation.pde

@@ -46,38 +46,37 @@ static void calc_XY_velocity(){
 	static int32_t last_longitude = 0;
 	static int32_t last_latitude  = 0;
 
+	static int16_t x_speed_old = 0;
+	static int16_t y_speed_old = 0;
+
 	// y_GPS_speed positve = Up
 	// x_GPS_speed positve = Right
 
 	// this speed is ~ in cm because we are using 10^7 numbers from GPS
 	float tmp = 1.0/dTnav;
-	//int8_t tmp = 5;
+	//float tmp = 5;
 
-	int16_t	x_diff	= (g_gps->longitude - last_longitude) * tmp;
+	// straightforward approach:
-	int16_t	y_diff	= (g_gps->latitude  - last_latitude) * tmp;
+	/*
+	int16_t	x_estimate	= (float)(g_gps->longitude - last_longitude) * tmp;
+	int16_t	y_estimate	= (float)(g_gps->latitude  - last_latitude)  * tmp;
+	// slight averaging filter
+	x_GPS_speed = (x_GPS_speed  + x_estimate) >> 1;
+	y_GPS_speed = (y_GPS_speed  + y_estimate) >> 1;
+	*/
 
-	// filter
+	// Ryan Beall's forward estimator:
-	x_GPS_speed = (x_GPS_speed  + x_diff) >> 1;
+	int16_t	x_speed_new = (float)(g_gps->longitude - last_longitude) * tmp;
-	y_GPS_speed = (y_GPS_speed  + y_diff) >> 1;
+	int16_t	y_speed_new = (float)(g_gps->latitude  - last_latitude)  * tmp;
 
-	//x_GPS_speed = x_diff;
+	x_GPS_speed 	= x_speed_new + (x_speed_new - x_speed_old);
-	//y_GPS_speed = y_diff;
+	y_GPS_speed 	= y_speed_new + (y_speed_new - y_speed_old);
 
-	// Above simply works better than GPS groundspeed
+	x_speed_old 	= x_speed_new;
-	// which is proving to be problematic
+	y_speed_old 	= y_speed_new;
-
-	/*if(g_gps->ground_speed > 120){
-		// Derive X/Y speed from GPS
-		// this is far more accurate when traveling about 1.5m/s
-		float temp		= g_gps->ground_course * RADX100;
-		x_GPS_speed 	= sin(temp) * (float)g_gps->ground_speed;
-		y_GPS_speed 	= cos(temp) * (float)g_gps->ground_speed;
-	}*/
 
 	last_longitude 	= g_gps->longitude;
 	last_latitude 	= g_gps->latitude;
-
-	//Serial.printf("GS: %d  \tx:%d \ty:%d\n", g_gps->ground_speed, x_GPS_speed, y_GPS_speed);
 }
 
 static void calc_location_error(struct Location *next_loc)
@@ -257,81 +256,6 @@ static void calc_loiter_pitch_roll()
 	nav_pitch = -nav_pitch;
 }
 
-#if WIND_COMP_STAB == 1
-static void calc_wind_compensation()
-{
-	// this idea is a function that converts user input into I term for position hold
-	// the concept is simple. The iterms always act upon flight no matter what mode were in.
-	// when our velocity is 0, we call this function to update our iterms
-	// otherwise we slowly reduce out iterms to 0
-
-	// take the pitch and roll of the copter and,
-	float roll 	=  dcm.roll_sensor;
-	float pitch = -dcm.pitch_sensor; // flip pitch to make positive pitch forward
-
-	// rotate it to eliminate yaw of Copter
-	int32_t roll_tmp 	= roll *  sin_yaw_y  - pitch * -cos_yaw_x;
-	int32_t pitch_tmp 	= roll * -cos_yaw_x  + pitch *  sin_yaw_y;
-
-	roll_tmp  = constrain(roll_tmp, -2000, 2000);
-	pitch_tmp = constrain(pitch_tmp, -2000, 2000);
-
-	// filter the input and apply it to out integrator value
-	// nav_lon and nav_lat will be applied to normal flight
-
-	// This filter is far too fast!!!
-	//nav_lon = ((int32_t)g.pi_loiter_lon.get_integrator() * 15 + roll_tmp) / 16;
-	//nav_lat = ((int32_t)g.pi_loiter_lat.get_integrator() * 15 + pitch_tmp) / 16;
-
-	nav_lon = g.pi_loiter_lon.get_integrator();
-	nav_lat = g.pi_loiter_lat.get_integrator();
-
-	// Maybe a slower filter would work?
-	if(g.pi_loiter_lon.get_integrator() > roll_tmp){
-		g.pi_loiter_lon.set_integrator(nav_lon - 5);
-	}else if(g.pi_loiter_lon.get_integrator() < roll_tmp){
-		g.pi_loiter_lon.set_integrator(nav_lon + 5);
-	}
-	if(g.pi_loiter_lat.get_integrator() > pitch_tmp){
-		g.pi_loiter_lat.set_integrator(nav_lat - 5);
-	}else if(g.pi_loiter_lat.get_integrator() < pitch_tmp){
-		g.pi_loiter_lat.set_integrator(nav_lat + 5);
-	}
-
-	// save smoothed input to integrator
-	g.pi_loiter_lon.set_integrator(nav_lon);		// X
-	g.pi_loiter_lat.set_integrator(nav_lat);		// Y
-
-	//Serial.printf("build wind iterm X:%d  Y:%d, r:%d, p:%d\n",
-	//				nav_lon,
-	//				nav_lat,
-	//				nav_roll,
-	//				nav_pitch);
-}
-
-static void reduce_wind_compensation()
-{
-	//slow degradation of iterms
-	float tmp;
-
-	tmp = g.pi_loiter_lon.get_integrator();
-	tmp *= .98;
-	g.pi_loiter_lon.set_integrator(tmp);		// X
-
-	tmp = g.pi_loiter_lat.get_integrator();
-	tmp *= .98;
-	g.pi_loiter_lat.set_integrator(tmp);		// Y
-
-	// debug
-	//int16_t t1 = g.pi_loiter_lon.get_integrator();
-	//int16_t t2 = g.pi_loiter_lon.get_integrator();
-
-	//Serial.printf("reduce wind iterm X:%d  Y:%d \n",
-	//				t1,
-	//				t2);
-}
-#endif
-
 static int16_t calc_desired_speed(int16_t max_speed)
 {
 	/*
@@ -508,29 +432,22 @@ static int32_t get_new_altitude()
 	}
 
 	int32_t diff 	= abs(next_WP.alt - target_altitude);
-	int8_t			_scale 	= 4;
+	int8_t			_scale 	= 3;
 
 	if (next_WP.alt < target_altitude){
 		// we are below the target alt
-
 		if(diff < 200){
-			// we are going up
 			_scale = 5;
 		} else {
 			_scale = 4;
 		}
-
 	}else {
-		// we are above the target
+		// we are above the target, going down
-		// stay at 16 for speed
+		if(diff < 600){
-		//_scale = 16;
+			_scale = 4;
-
+		}
-		if(diff < 400){
+		if(diff < 300){
-			// we are going down
 			_scale = 5;
-
-		}else if(diff < 100){
-			_scale = 6;
 		}
 	}
 

ArduCopter/sensors.pde

@@ -81,6 +81,7 @@ static void init_compass()
 	dcm.set_compass(&compass);
 	compass.init();
 	compass.get_offsets();					// load offsets to account for airframe magnetic interference
+	compass.null_offsets_enable();
 }
 
 static void init_optflow()

ArduCopter/system.pde

@@ -428,12 +428,6 @@ static void startup_ground(void)
 
 static void set_mode(byte mode)
 {
-	//if(control_mode == mode){
-		// don't switch modes if we are already in the correct mode.
-	//	Serial.print("Double mode\n");
-		//return;
-	//}
-
 	// if we don't have GPS lock
 	if(home_is_set == false){
 		// our max mode should be
@@ -458,6 +452,9 @@ static void set_mode(byte mode)
 	// clearing value used in interactive alt hold
 	manual_boost = 0;
 
+	// clearing value used to force the copter down in landing mode
+	landing_boost = 0;
+
 	// do not auto_land if we are leaving RTL
 	auto_land_timer = 0;
 
@@ -574,13 +571,6 @@ static void set_mode(byte mode)
 		// We are under manual attitude control
 		// removes the navigation from roll and pitch commands, but leaves the wind compensation
 		reset_nav();
-
-		#if WIND_COMP_STAB == 1
-		if(GPS_enabled){
-			wp_control = NO_NAV_MODE;
-			update_nav_wp();
-		}
-		#endif
 	}
 
 	Log_Write_Mode(control_mode);

ArduCopter/test.pde

@@ -22,7 +22,7 @@ static int8_t	test_battery(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);
 static int8_t	test_current(uint8_t argc, 		const Menu::arg *argv);
-//static int8_t	test_relay(uint8_t argc,	 	const Menu::arg *argv);
+static int8_t	test_relay(uint8_t argc,	 	const Menu::arg *argv);
 static int8_t	test_wp(uint8_t argc, 			const Menu::arg *argv);
 #if HIL_MODE != HIL_MODE_ATTITUDE
 static int8_t	test_baro(uint8_t argc, 		const Menu::arg *argv);
@@ -70,7 +70,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
 	{"tune",		test_tuning},
 	//{"tri",			test_tri},
 	{"current",		test_current},
-//	{"relay",		test_relay},
+	{"relay",		test_relay},
 	{"wp",			test_wp},
 	//{"nav",			test_nav},
 #if HIL_MODE != HIL_MODE_ATTITUDE
@@ -777,9 +777,8 @@ test_current(uint8_t argc, const Menu::arg *argv)
 	return (0);
 }
 
-/*
+
-//static int8_t
+static int8_t test_relay(uint8_t argc, const Menu::arg *argv)
-//test_relay(uint8_t argc, const Menu::arg *argv)
 {
 	print_hit_enter();
 	delay(1000);
@@ -800,7 +799,7 @@ test_current(uint8_t argc, const Menu::arg *argv)
 		}
 	}
 }
-*/
+
 static int8_t
 test_wp(uint8_t argc, const Menu::arg *argv)
 {

ArduPlane/system.pde

@@ -202,6 +202,7 @@ static void init_ardupilot()
         } else {
             dcm.set_compass(&compass);
             compass.get_offsets();						// load offsets to account for airframe magnetic interference
+            compass.null_offsets_enable();
         }
 	}
 #endif

ArduPlane/test.pde

@@ -586,6 +586,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
         Serial.println_P(PSTR("Compass initialisation failed!"));
         return 0;
     }
+    compass.null_offsets_enable();
     dcm.set_compass(&compass);
     report_compass();
 

Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml

@@ -55,7 +55,7 @@
     <url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.hex</url>
     <url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.hex</url2560>
     <url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560-2.hex</url2560-2>
-    <name>ArduCopter V2.1.0 Alpha Quad</name>
+    <name>ArduCopter V2.2 Quad</name>
     <desc>
 #define FRAME_CONFIG QUAD_FRAME
 #define FRAME_ORIENTATION X_FRAME
@@ -67,7 +67,7 @@
     <url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.hex</url>
     <url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.hex</url2560>
     <url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560-2.hex</url2560-2>
-    <name>ArduCopter V2.1.0 Alpha Tri</name>
+    <name>ArduCopter V2.2 Tri</name>
     <desc>
 #define FRAME_CONFIG TRI_FRAME
 #define FRAME_ORIENTATION X_FRAME
@@ -79,7 +79,7 @@
     <url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.hex</url>
     <url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.hex</url2560>
     <url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560-2.hex</url2560-2>
-    <name>ArduCopter V2.1.0 Alpha Hexa</name>
+    <name>ArduCopter V2.2 Hexa</name>
     <desc>
 #define FRAME_CONFIG HEXA_FRAME
 #define FRAME_ORIENTATION X_FRAME
@@ -91,7 +91,7 @@
     <url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.hex</url>
     <url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.hex</url2560>
     <url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560-2.hex</url2560-2>
-    <name>ArduCopter V2.1.0 Alpha Y6</name>
+    <name>ArduCopter V2.2 Y6</name>
     <desc>
 #define FRAME_CONFIG Y6_FRAME
 #define FRAME_ORIENTATION X_FRAME
@@ -103,7 +103,7 @@
     <url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octav-1280.hex</url>
     <url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octav-2560.hex</url2560>
     <url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octav-2560-2.hex</url2560-2>
-    <name>ArduCopter V2.1.0 Alpha Octav</name>
+    <name>ArduCopter V2.2 Octav</name>
     <desc>
 #define FRAME_CONFIG OCTA_FRAME
 #define FRAME_ORIENTATION V_FRAME
@@ -115,7 +115,7 @@
     <url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octa-1280.hex</url>
     <url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octa-2560.hex</url2560>
     <url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octa-2560-2.hex</url2560-2>
-    <name>ArduCopter V2.1.0 Alpha Octa</name>
+    <name>ArduCopter V2.2 Octa</name>
     <desc>
 #define FRAME_CONFIG OCTA_FRAME
 #define FRAME_ORIENTATION X_FRAME
@@ -175,7 +175,7 @@
     <url>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.hex</url>
     <url2560>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.hex</url2560>
     <url2560-2>http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560-2.hex</url2560-2>
-    <name>ArduCopter V2.1.0 Alpha Quad Hil</name>
+    <name>ArduCopter V2.2 Quad Hil</name>
     <desc>
 #define FRAME_CONFIG QUAD_FRAME
 #define FRAME_ORIENTATION PLUS_FRAME

libraries/AP_Compass/Compass.cpp

@@ -11,6 +11,7 @@ Compass::Compass(AP_Var::Key key) :
     _offset				(&_group, 1),
     _declination		(&_group, 2, 0.0, PSTR("DEC")),
     _null_init_done(false),
+    _null_enable(false),
 	product_id(AP_COMPASS_TYPE_UNKNOWN)
 {
     // Default the orientation matrix to none - will be overridden at group load time
@@ -146,6 +147,8 @@ Compass::null_offsets(const Matrix3f &dcm_matrix)
     float       weight;
 
     Vector3f mag_body_new = Vector3f(mag_x,mag_y,mag_z);
+    
+    if(_null_enable == false) return;
 
     if(_null_init_done) {
         dcm_new_from_last = dcm_matrix.transposed() * _last_dcm_matrix;      // Note 11/20/2010: transpose() is not working, transposed() is.
@@ -166,3 +169,18 @@ Compass::null_offsets(const Matrix3f &dcm_matrix)
     _last_dcm_matrix = dcm_matrix;
 
 }
+
+
+void
+Compass::null_offsets_enable(void)
+{
+	_null_init_done = false;
+	_null_enable = true;
+}
+
+void
+Compass::null_offsets_disable(void)
+{
+	_null_init_done = false;
+	_null_enable = false;
+}

libraries/AP_Compass/Compass.h

@@ -41,7 +41,7 @@ public:
 	float           heading_x;      ///< compass vector X magnitude
 	float           heading_y;      ///< compass vector Y magnitude
 	unsigned long   last_update;    ///< millis() time of last update
-	bool		healthy;        ///< true if last read OK
+	bool			healthy;        ///< true if last read OK
 
 	/// Constructor
 	///
@@ -117,6 +117,16 @@ public:
 	void null_offsets(const Matrix3f &dcm_matrix);
 
 
+	/// Enable/Start automatic offset updates 
+	///
+	void null_offsets_enable(void);
+
+
+	/// Disable/Stop automatic offset updates
+	///
+	void null_offsets_disable(void);
+
+
 	/// Sets the local magnetic field declination.
 	///
 	/// @param  radians             Local field declination.
@@ -130,6 +140,7 @@ protected:
 	AP_VarS<Vector3f>   _offset;
 	AP_Float            _declination;
 
+	bool                _null_enable;        	///< enabled flag for offset nulling
 	bool                _null_init_done;        ///< first-time-around flag used by offset nulling
 	Matrix3f            _last_dcm_matrix;       ///< previous DCM matrix used by offset nulling
 	Vector3f            _mag_body_last;         ///< ?? used by offset nulling

libraries/AP_DCM/AP_DCM.cpp

@@ -195,6 +195,9 @@ AP_DCM::accel_adjust(void)
 void
 AP_DCM::matrix_reset(void)
 {
+	if (_compass != NULL) {
+		_compass->null_offsets_disable();		
+	}
 	_dcm_matrix.a.x = 1.0f;
 	_dcm_matrix.a.y = 0.0f;
 	_dcm_matrix.a.z = 0.0f;
@@ -207,6 +210,11 @@ AP_DCM::matrix_reset(void)
 	_omega_I.x = 0.0f;
 	_omega_I.y = 0.0f;
 	_omega_I.z = 0.0f;
+	if (_compass != NULL) {
+		_compass->null_offsets_enable();	// This call is needed to restart the nulling
+		// Otherwise the reset in the DCM matrix can mess up
+		// the nulling
+	}
 }
 
 /*************************************************