From bc2d17e76e88ee38227a07eea69cfee953d1f932 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 29 Dec 2013 22:55:20 +1100 Subject: [PATCH] AP_Compass: added vector compass setHIL method --- libraries/AP_Compass/AP_Compass_HIL.cpp | 10 ++++++++++ libraries/AP_Compass/AP_Compass_HIL.h | 2 ++ 2 files changed, 12 insertions(+) diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index 6b4af4c80b..805c3af966 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/libraries/AP_Compass/AP_Compass_HIL.cpp @@ -106,6 +106,16 @@ void AP_Compass_HIL::setHIL(float roll, float pitch, float yaw) _healthy[0] = true; } +// Update raw magnetometer values from HIL mag vector +// +void AP_Compass_HIL::setHIL(const Vector3i &mag) +{ + _hil_mag.x = mag.x; + _hil_mag.y = mag.y; + _hil_mag.z = mag.z; + _healthy[0] = true; +} + void AP_Compass_HIL::accumulate(void) { // nothing to do diff --git a/libraries/AP_Compass/AP_Compass_HIL.h b/libraries/AP_Compass/AP_Compass_HIL.h index 53e3c0e848..3bb6383724 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.h +++ b/libraries/AP_Compass/AP_Compass_HIL.h @@ -11,6 +11,8 @@ public: bool read(void); void accumulate(void); void setHIL(float roll, float pitch, float yaw); + void setHIL(const Vector3i &mag); + private: Vector3f _hil_mag; Vector3f _Bearth;