From 3370ec62a19e48355b8c74dd088d465d5fc1a279 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 May 2015 14:04:12 +1000 Subject: [PATCH] HAL_SITL: fix for multi-compass SITL --- libraries/AP_HAL_SITL/sitl_compass.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_SITL/sitl_compass.cpp b/libraries/AP_HAL_SITL/sitl_compass.cpp index 620122ff91..db96461f85 100644 --- a/libraries/AP_HAL_SITL/sitl_compass.cpp +++ b/libraries/AP_HAL_SITL/sitl_compass.cpp @@ -39,10 +39,11 @@ void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg) if (yawDeg < -180.0f) { yawDeg += 360.0f; } - _compass->setHIL(radians(rollDeg), radians(pitchDeg), radians(yawDeg)); + _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() + noise + motor; + Vector3f new_mag_data = _compass->getHIL(0) + noise + motor; uint32_t now = hal.scheduler->millis(); // add delay @@ -77,7 +78,8 @@ void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg) new_mag_data -= _sitl->mag_ofs.get(); - _compass->setHIL(new_mag_data); + _compass->setHIL(0, new_mag_data); + _compass->setHIL(1, new_mag_data); } #endif