From 972bdcfa393ce23994ef4d185caf36dd17a1db58 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 28 Mar 2012 20:47:33 +1100
Subject: [PATCH] SITL: add magnetic field noise to the simulated compass

---
 libraries/Desktop/support/sitl_adc.h       |  8 +-------
 libraries/Desktop/support/sitl_compass.cpp |  4 +++-
 libraries/Desktop/support/util.cpp         | 13 +++++++++++++
 libraries/Desktop/support/util.h           |  7 +++++++
 4 files changed, 24 insertions(+), 8 deletions(-)

diff --git a/libraries/Desktop/support/sitl_adc.h b/libraries/Desktop/support/sitl_adc.h
index 6a21d6f0f9..348d433ff9 100644
--- a/libraries/Desktop/support/sitl_adc.h
+++ b/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) {
diff --git a/libraries/Desktop/support/sitl_compass.cpp b/libraries/Desktop/support/sitl_compass.cpp
index 8d1d319a37..39912e8959 100644
--- a/libraries/Desktop/support/sitl_compass.cpp
+++ b/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);
 }
 
 
diff --git a/libraries/Desktop/support/util.cpp b/libraries/Desktop/support/util.cpp
index 7656d36bf1..6a956533e3 100644
--- a/libraries/Desktop/support/util.cpp
+++ b/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;
+}
diff --git a/libraries/Desktop/support/util.h b/libraries/Desktop/support/util.h
index 7d55f827c6..7778adda14 100644
--- a/libraries/Desktop/support/util.h
+++ b/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