From a4bad1c7038ed1ed676060146980b4d18d63683c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 21 Apr 2015 08:29:51 +1000 Subject: [PATCH] HAL_SITL: use SIM_MAG_OFS in SITL compass --- libraries/AP_HAL_AVR_SITL/SITL_State.h | 2 -- libraries/AP_HAL_AVR_SITL/sitl_compass.cpp | 9 +++++---- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.h b/libraries/AP_HAL_AVR_SITL/SITL_State.h index d83b290c6c..aa51d4c80e 100644 --- a/libraries/AP_HAL_AVR_SITL/SITL_State.h +++ b/libraries/AP_HAL_AVR_SITL/SITL_State.h @@ -154,8 +154,6 @@ private: VectorN buffer_mag; uint32_t time_delta_mag; uint32_t delayed_time_mag; - Vector3f mag_data; - Vector3f new_mag_data; // airspeed sensor delay buffer variables struct readings_wind { diff --git a/libraries/AP_HAL_AVR_SITL/sitl_compass.cpp b/libraries/AP_HAL_AVR_SITL/sitl_compass.cpp index 5ea684e76d..5ca48896b9 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_compass.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_compass.cpp @@ -43,8 +43,7 @@ void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg) _compass->setHIL(radians(rollDeg), radians(pitchDeg), radians(yawDeg)); Vector3f noise = _rand_vec3f() * _sitl->mag_noise; Vector3f motor = _sitl->mag_mot.get() * _current; - - new_mag_data = _compass->getHIL() + noise + motor; + Vector3f new_mag_data = _compass->getHIL() + noise + motor; uint32_t now = hal.scheduler->millis(); // add delay @@ -74,10 +73,12 @@ void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg) } } if (best_time_delta_mag < 1000) { // only output stored state if < 1 sec retrieval error - mag_data = buffer_mag[best_index_mag].data; + new_mag_data = buffer_mag[best_index_mag].data; } - _compass->setHIL(mag_data); + new_mag_data -= _sitl->mag_ofs.get(); + + _compass->setHIL(new_mag_data); } #endif