SITL: improve the simulated compass

this implements a much more accurate model of a compass using matrix
rotations, instead of trying to calculate components directly

libraries/Desktop/support/desktop.h

@@ -20,7 +20,7 @@ void sitl_input(void);
 void sitl_setup(void);
 int sitl_gps_pipe(void);
 ssize_t sitl_gps_read(int fd, void *buf, size_t count);
-void sitl_update_compass(float heading, float roll, float pitch, float yaw);
+void sitl_update_compass(float roll, float pitch, float yaw);
 void sitl_update_gps(double latitude, double longitude, float altitude,
 		     double speedN, double speedE, bool have_lock);
 void sitl_update_adc(float roll, float pitch, float yaw,

libraries/Desktop/support/sitl.cpp

@@ -278,7 +278,7 @@ static void timer_handler(int signum)
 			sim_state.xAccel, sim_state.yAccel, sim_state.zAccel,
 			sim_state.airspeed);
 	sitl_update_barometer(sim_state.altitude);
-	sitl_update_compass(sim_state.heading, sim_state.rollDeg, sim_state.pitchDeg, sim_state.heading);
+	sitl_update_compass(sim_state.rollDeg, sim_state.pitchDeg, sim_state.heading);
 	sei();
 }
 
@@ -326,7 +326,7 @@ void sitl_setup(void)
 	// setup some initial values
 	sitl_update_barometer(desktop_state.initial_height);
 	sitl_update_adc(0, 0, 0, 0, 0, 0, 0, 0, -9.8, 0);
-	sitl_update_compass(0, 0, 0, 0);
+	sitl_update_compass(0, 0, 0);
 	sitl_update_gps(0, 0, 0, 0, 0, false);
 }
 

libraries/Desktop/support/sitl_compass.cpp

@@ -20,40 +20,40 @@
 #define MAG_OFS_Y 13.0
 #define MAG_OFS_Z -18.0
 
+// declination in Canberra (degrees)
+#define MAG_DECLINATION 12.1
+
+// inclination in Canberra (degrees)
+#define MAG_INCLINATION -66
+
+// magnetic field strength in Canberra as observed
+// using an APM1 with 5883L compass
+#define MAG_FIELD_STRENGTH 818
+
 /*
   given a magnetic heading, and roll, pitch, yaw values,
   calculate consistent magnetometer components
 
   All angles are in radians
  */
-static Vector3f heading_to_mag(float heading, float roll, float pitch, float yaw)
+static Vector3f heading_to_mag(float roll, float pitch, float yaw)
 {
-	Vector3f v;
-	double headX, headY, cos_roll, sin_roll, cos_pitch, sin_pitch, scale;
-	const double magnitude = 665;
+	Vector3f Bearth, m;
+	Matrix3f R;
 
-	cos_roll = cos(roll);
-	sin_roll = sin(roll);
-	cos_pitch = cos(pitch);
-	sin_pitch = sin(pitch);
+	// Bearth is the magnetic field in Canberra. We need to adjust
+	// it for inclination and declination
+	Bearth(MAG_FIELD_STRENGTH, 0, 0);
+	R.from_euler(0, -ToRad(MAG_INCLINATION), ToRad(MAG_DECLINATION));
+	Bearth = R * Bearth;
 
+	// create a rotation matrix for the given attitude
+	R.from_euler(roll, pitch, yaw);
 
-	headY = -sin(heading);
-	headX = cos(heading);
-
-	if (fabs(cos_roll) < 1.0e-20) {
-		cos_roll = 1.0e-10;
-	}
-	if (fabs(cos_pitch) < 1.0e-20) {
-		cos_pitch = 1.0e-10;
-	}
-
-	v.z = -0.6*cos(roll)*cos(pitch);
-	v.y = (headY + v.z*sin_roll) / cos_roll;
-	v.x = (headX - (v.y*sin_roll*sin_pitch + v.z*cos_roll*sin_pitch)) / cos_pitch;
-	scale = magnitude / sqrt((v.x*v.x) + (v.y*v.y) + (v.z*v.z));
-	v *= scale;
-	return v;
+	// convert the earth frame magnetic vector to body frame, and
+	// apply the offsets
+	m = R.transposed() * Bearth - Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z);
+	return m;
 }
 
 
@@ -62,12 +62,11 @@ static Vector3f heading_to_mag(float heading, float roll, float pitch, float yaw
   setup the compass with new input
   all inputs are in degrees
  */
-void sitl_update_compass(float heading, float roll, float pitch, float yaw)
+void sitl_update_compass(float roll, float pitch, float yaw)
 {
 	extern AP_Compass_HIL compass;
-	Vector3f m = heading_to_mag(ToRad(heading),
-				    ToRad(roll),
+	Vector3f m = heading_to_mag(ToRad(roll),
 				    ToRad(pitch),
 				    ToRad(yaw));
-	compass.setHIL(m.x - MAG_OFS_X, m.y - MAG_OFS_Y, m.z - MAG_OFS_Z);
+	compass.setHIL(m.x, m.y, m.z);
 }