SITL: add magnetic field noise to the simulated compass

libraries/Desktop/support/sitl_adc.h

@@ -8,6 +8,7 @@
 
 #include <stdlib.h>
 #include <math.h>
+#include "util.h"
 
 static const float vibration_level = 0.2;
 static const float drift_speed = 0.2; // degrees/second/minute
@@ -18,13 +19,6 @@ static const float noise_offset[8]= {     0,     0,     0,    0,   0,    0,    0
 static const float drift_rate[8]  = {     1.0, 1.0,   1.0,    0,   0,    0,    0,    0 };
 static const float base_noise = 2;
 
-// generate a random float between -1 and 1
-static double rand_float(void)
-{
-	float ret = ((unsigned)random()) % 2000000;
-	return (ret - 1.0e6) / 1.0e6;
-}
-
 static inline float gyro_drift(uint8_t chan)
 {
 	if (drift_rate[chan] * drift_speed == 0.0) {

libraries/Desktop/support/sitl_compass.cpp

@@ -30,6 +30,8 @@
 // using an APM1 with 5883L compass
 #define MAG_FIELD_STRENGTH 818
 
+#define MAG_NOISE 5
+
 /*
   given a magnetic heading, and roll, pitch, yaw values,
   calculate consistent magnetometer components
@@ -53,7 +55,7 @@ static Vector3f heading_to_mag(float roll, float pitch, float 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;
+	return m + (rand_vec3f() * MAG_NOISE);
 }
 
 

libraries/Desktop/support/util.cpp

@@ -13,6 +13,7 @@
 #include <math.h>
 #include <stdint.h>
 #include <fcntl.h>
+#include <AP_Math.h>
 #include "desktop.h"
 #include "util.h"
 
@@ -58,3 +59,15 @@ void convert_body_frame(double rollDeg, double pitchDeg,
 	*q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta);
 	*r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot;
 }
+
+// generate a random Vector3f of size 1
+Vector3f rand_vec3f(void)
+{
+	Vector3f v = Vector3f(rand_float(),
+			      rand_float(),
+			      rand_float());
+	if (v.length() != 0.0) {
+		v.normalize();
+	}
+	return v;
+}

libraries/Desktop/support/util.h

@@ -12,3 +12,10 @@ void runInterrupt(uint8_t inum);
 void convert_body_frame(double rollDeg, double pitchDeg,
 			double rollRate, double pitchRate, double yawRate,
 			double *p, double *q, double *r);
+
+// generate a random float between -1 and 1
+#define rand_float() (((((unsigned)random()) % 2000000) - 1.0e6) / 1.0e6)
+
+#ifdef VECTOR3_H
+Vector3f rand_vec3f(void);
+#endif