From 2cf5493b4051c1f5263d327b2d2c1e4a114903e5 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Fri, 17 Jun 2016 13:48:15 +1000
Subject: [PATCH] AP_HAL_SITL: Ensure simulated magnetic field is always used

---
 libraries/AP_HAL_SITL/sitl_compass.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_HAL_SITL/sitl_compass.cpp b/libraries/AP_HAL_SITL/sitl_compass.cpp
index c6c96cdb89..4b5fa3206a 100644
--- a/libraries/AP_HAL_SITL/sitl_compass.cpp
+++ b/libraries/AP_HAL_SITL/sitl_compass.cpp
@@ -41,11 +41,6 @@ void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg)
     if (yawDeg < -180.0f) {
         yawDeg += 360.0f;
     }
-    _compass->setHIL(0, radians(rollDeg), radians(pitchDeg), radians(yawDeg));
-    _compass->setHIL(1, radians(rollDeg), radians(pitchDeg), radians(yawDeg));
-    Vector3f noise = _rand_vec3f() * _sitl->mag_noise;
-    Vector3f motor = _sitl->mag_mot.get() * _current;
-    Vector3f new_mag_data = _compass->getHIL(0) + noise + motor;
 
     // 100Hz
     uint32_t now = AP_HAL::millis();
@@ -54,6 +49,11 @@ void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg)
     }
     last_update = now;
 
+    // calculate sensor noise and add to 'truth' field in body frame
+    // units are milli-Gauss
+    Vector3f noise = _rand_vec3f() * _sitl->mag_noise;
+    Vector3f new_mag_data = _sitl->state.bodyMagField + noise;
+
     // add delay
     uint32_t best_time_delta_mag = 1000; // initialise large time representing buffer entry closest to current time - delay.
     uint8_t best_index_mag = 0; // initialise number representing the index of the entry in buffer closest to delay.