diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index 05e06119c2..a6945bf93a 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/libraries/AP_Compass/AP_Compass_HIL.cpp @@ -27,9 +27,9 @@ extern const AP_HAL::HAL& hal; // constructor AP_Compass_HIL::AP_Compass_HIL(Compass &compass): - AP_Compass_Backend(compass), - _compass_instance(0) + AP_Compass_Backend(compass) { + memset(_compass_instance, 0, sizeof(_compass_instance)); _compass._setup_earth_field(); } @@ -50,12 +50,18 @@ AP_Compass_Backend *AP_Compass_HIL::detect(Compass &compass) bool AP_Compass_HIL::init(void) { - // register the compass instance in the frontend - _compass_instance = register_compass(); + // register two compass instances + for (uint8_t i=0; imicros(); } -const Vector3f& Compass::getHIL() const { - return _hil.field; +const Vector3f& Compass::getHIL(uint8_t instance) const +{ + return _hil.field[instance]; } // setup _Bearth diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index dc26ea2122..a4393693f6 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -35,7 +35,7 @@ maximum number of compass instances available on this platform. If more than 1 then redundent sensors may be available */ -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN +#if HAL_CPU_CLASS > HAL_CPU_CLASS_16 #define COMPASS_MAX_INSTANCES 3 #define COMPASS_MAX_BACKEND 3 #else @@ -223,9 +223,9 @@ public: uint8_t get_primary(void) const { return _primary; } // HIL methods - void setHIL(float roll, float pitch, float yaw); - void setHIL(const Vector3f &mag); - const Vector3f& getHIL() const; + void setHIL(uint8_t instance, float roll, float pitch, float yaw); + void setHIL(uint8_t instance, const Vector3f &mag); + const Vector3f& getHIL(uint8_t instance) const; void _setup_earth_field(); // enable HIL mode @@ -240,7 +240,8 @@ public: struct { Vector3f Bearth; float last_declination; - Vector3f field; + bool healthy[COMPASS_MAX_INSTANCES]; + Vector3f field[COMPASS_MAX_INSTANCES]; } _hil; private: