Try a different mag nulling algorithm.

flight/Modules/Sensors/sensors.c

@@ -48,6 +48,7 @@
 
 #include "pios.h"
 #include "attitude.h"
+#include "homelocation.h"
 #include "magnetometer.h"
 #include "magbias.h"
 #include "accels.h"
@@ -424,6 +425,7 @@ static void SensorsTask(void *parameters)
  */
 static void magOffsetEstimation(MagnetometerData *mag)
 {
+#if 0
 	// Constants, to possibly go into a UAVO
 	static const float MIN_NORM_DIFFERENCE = 50;
 
@@ -462,6 +464,61 @@ static void magOffsetEstimation(MagnetometerData *mag)
 		// Store this value to compare against next update
 		B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2];
 	}
+#else
+	static uint32_t call_count = 0;
+
+	MagBiasData magBias;
+	MagBiasGet(&magBias);
+	
+	// Remove the current estimate of the bias
+	mag->x -= magBias.x;
+	mag->y -= magBias.y;
+	mag->z -= magBias.z;
+		
+	if((call_count++ % 5) == 0) {
+		HomeLocationData homeLocation;
+		HomeLocationGet(&homeLocation);
+		
+		AttitudeActualData attitude;
+		AttitudeActualGet(&attitude);
+		
+		const float Rxy = sqrtf(homeLocation.Be[0]*homeLocation.Be[0] + homeLocation.Be[1]*homeLocation.Be[1]);
+		const float Rz = homeLocation.Be[2];
+		
+		const float rate = cal.MagBiasNullingRate;
+		float R[3][3];
+		float B_e[3];
+		float xy[2];
+		float delta[3];
+		
+		// Get the rotation matrix
+		Quaternion2R(&attitude.q1, R);
+		
+		// Rotate the mag into the NED frame
+		B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z;
+		B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z;
+		B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z;
+		
+		float cy = cosf(attitude.Yaw * M_PI / 180.0f);
+		float sy = sinf(attitude.Yaw * M_PI / 180.0f);
+		
+		xy[0] =  cy * B_e[0] + sy * B_e[1];
+		xy[1] = -sy * B_e[0] + cy * B_e[1];
+		
+		float xy_norm = sqrtf(xy[0]*xy[0] + xy[1]*xy[1]);
+		
+		delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]);
+		delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
+		delta[2] = -rate * (Rz - B_e[2]);
+		
+		if (delta[0] == delta[0] && delta[1] == delta[1] && delta[2] == delta[2]) {		
+			magBias.x += delta[0];
+			magBias.y += delta[1];
+			magBias.z += delta[2];
+			MagBiasSet(&magBias);
+		}
+	}
+#endif
 }
 
 /**

flight/Modules/Sensors/simulated/sensors.c

@@ -834,6 +834,7 @@ static float rand_gauss (void) {
  */
 static void magOffsetEstimation(MagnetometerData *mag)
 {
+#if 0
 	RevoCalibrationData cal;
 	RevoCalibrationGet(&cal);
 
@@ -875,6 +876,56 @@ static void magOffsetEstimation(MagnetometerData *mag)
 		// Store this value to compare against next update
 		B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2];
 	}
+#else
+	HomeLocationData homeLocation;
+	HomeLocationGet(&homeLocation);
+	
+	AttitudeActualData attitude;
+	AttitudeActualGet(&attitude);
+	
+	MagBiasData magBias;
+	MagBiasGet(&magBias);
+	
+	// Remove the current estimate of the bias
+	mag->x -= magBias.x;
+	mag->y -= magBias.y;
+	mag->z -= magBias.z;
+	
+	const float Rxy = sqrtf(homeLocation.Be[0]*homeLocation.Be[0] + homeLocation.Be[1]*homeLocation.Be[1]);
+	const float Rz = homeLocation.Be[2];
+	
+	const float rate = 0.01;
+	float R[3][3];
+	float B_e[3];
+	float xy[2];
+	float delta[3];
+	
+	// Get the rotation matrix
+	Quaternion2R(&attitude.q1, R);
+	
+	// Rotate the mag into the NED frame
+	B_e[0] = R[0][0] * mag->x + R[1][0] * mag->y + R[2][0] * mag->z;
+	B_e[1] = R[0][1] * mag->x + R[1][1] * mag->y + R[2][1] * mag->z;
+	B_e[2] = R[0][2] * mag->x + R[1][2] * mag->y + R[2][2] * mag->z;
+	
+	float cy = cosf(attitude.Yaw * M_PI / 180.0f);
+	float sy = sinf(attitude.Yaw * M_PI / 180.0f);
+
+	xy[0] =  cy * B_e[0] + sy * B_e[1];
+	xy[1] = -sy * B_e[0] + cy * B_e[1];
+	
+	float xy_norm = sqrtf(xy[0]*xy[0] + xy[1]*xy[1]);
+	
+	delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]);
+	delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
+	delta[2] = -rate * (Rz - B_e[2]);
+	
+	magBias.x += delta[0];
+	magBias.y += delta[1];
+	magBias.z += delta[2];
+	MagBiasSet(&magBias);
+#endif
+
 }
 
 /**