SITL: use new HIL compass API

libraries/AP_HAL_AVR_SITL/SITL_State.h

@@ -54,7 +54,7 @@ private:
     // these methods are static as they are called
     // from the timer
     static void _update_barometer(float height);
-    static void _update_compass(float roll, float pitch, float yaw);
+    static void _update_compass(float rollDeg, float pitchDeg, float yawDeg);
 
     struct gps_data {
 	    double latitude;
@@ -89,7 +89,6 @@ private:
     static float _gyro_drift(void);
     static float _rand_float(void);
     static Vector3f _rand_vec3f(void);
-    static Vector3f _heading_to_mag(float roll, float pitch, float yaw);
 
     // signal handlers
     static void _sig_fpe(int signum);

libraries/AP_HAL_AVR_SITL/sitl_compass.cpp

@@ -21,60 +21,21 @@
 
 using namespace AVR_SITL;
 
-#define MAG_OFS_X 5.0
-#define MAG_OFS_Y 13.0
-#define MAG_OFS_Z -18.0
-
-// 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
- */
-Vector3f SITL_State::_heading_to_mag(float roll, float pitch, float yaw)
-{
-	Vector3f Bearth, m;
-	Matrix3f R;
-	float declination = AP_Declination::get_declination(_sitl->state.latitude, _sitl->state.longitude);
-
-	// 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(declination));
-	Bearth = R * Bearth;
-
-	// create a rotation matrix for the given attitude
-	R.from_euler(roll, pitch, yaw);
-
-	// 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 + (_rand_vec3f() * _sitl->mag_noise);
-}
-
-
-
 /*
   setup the compass with new input
   all inputs are in degrees
  */
-void SITL_State::_update_compass(float roll, float pitch, float yaw)
+void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg)
 {
 	if (_compass == NULL) {
 		// no compass in this sketch
 		return;
 	}
-	Vector3f m = _heading_to_mag(ToRad(roll),
-				     ToRad(pitch),
-				     ToRad(yaw));
-	_compass->setHIL(m.x, m.y, m.z);
+	_compass->setHIL(radians(rollDeg), radians(pitchDeg), radians(yawDeg));
+	Vector3f noise = _rand_vec3f() * _sitl->mag_noise;
+	_compass->mag_x += noise.x;
+	_compass->mag_y += noise.y;
+	_compass->mag_z += noise.z;
 }
 
 #endif