diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
index 77ac477eaa..cf32e46cd9 100644
--- a/libraries/AP_Compass/AP_Compass.cpp
+++ b/libraries/AP_Compass/AP_Compass.cpp
@@ -12,7 +12,6 @@
#include "AP_Compass_AK8963.h"
#include "AP_Compass_Backend.h"
#include "AP_Compass_BMM150.h"
-#include "AP_Compass_HIL.h"
#include "AP_Compass_HMC5843.h"
#include "AP_Compass_IST8308.h"
#include "AP_Compass_IST8310.h"
@@ -1194,13 +1193,6 @@ void Compass::_probe_external_i2c_compasses(void)
*/
void Compass::_detect_backends(void)
{
-#ifndef HAL_BUILD_AP_PERIPH
- if (_hil_mode) {
- _add_backend(AP_Compass_HIL::detect());
- return;
- }
-#endif
-
#if HAL_EXTERNAL_AHRS_ENABLED
if (int8_t serial_port = AP::externalAHRS().get_port() >= 0) {
ADD_BACKEND(DRIVER_SERIAL, new AP_Compass_ExternalAHRS(serial_port));
@@ -1235,8 +1227,6 @@ void Compass::_detect_backends(void)
#if defined(HAL_MAG_PROBE_LIST)
// driver probes defined by COMPASS lines in hwdef.dat
HAL_MAG_PROBE_LIST;
-#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL
- ADD_BACKEND(DRIVER_SITL, AP_Compass_HIL::detect());
#elif AP_FEATURE_BOARD_DETECT
switch (AP_BoardConfig::get_board_type()) {
case AP_BoardConfig::PX4_BOARD_PX4V1:
@@ -1866,68 +1856,20 @@ bool Compass::configured(char *failure_msg, uint8_t failure_msg_len)
return all_configured;
}
-// Update raw magnetometer values from HIL data
-//
-void Compass::setHIL(uint8_t instance, float roll, float pitch, float yaw)
-{
- Matrix3f R;
-
- // create a rotation matrix for the given attitude
- R.from_euler(roll, pitch, yaw);
-
- if (!is_equal(_hil.last_declination,get_declination())) {
- _setup_earth_field();
- _hil.last_declination = get_declination();
- }
-
- // convert the earth frame magnetic vector to body frame, and
- // apply the offsets
- _hil.field[instance] = R.mul_transpose(_hil.Bearth);
-
- // apply default board orientation for this compass type. This is
- // a noop on most boards
- _hil.field[instance].rotate(MAG_BOARD_ORIENTATION);
-
- // add user selectable orientation
- _hil.field[instance].rotate((enum Rotation)_state[StateIndex(0)].orientation.get());
-
- if (!_state[StateIndex(0)].external) {
- // and add in AHRS_ORIENTATION setting if not an external compass
- if (_board_orientation == ROTATION_CUSTOM && _custom_rotation) {
- _hil.field[instance] = *_custom_rotation * _hil.field[instance];
- } else {
- _hil.field[instance].rotate(_board_orientation);
- }
- }
- _hil.healthy[instance] = true;
-}
-
-// Update raw magnetometer values from HIL mag vector
-//
-void Compass::setHIL(uint8_t instance, const Vector3f &mag, uint32_t update_usec)
-{
- _hil.field[instance] = mag;
- _hil.healthy[instance] = true;
- _state[StateIndex(instance)].last_update_usec = update_usec;
-}
-
-const Vector3f& Compass::getHIL(uint8_t instance) const
-{
- return _hil.field[instance];
-}
-
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// setup _Bearth
void Compass::_setup_earth_field(void)
{
// assume a earth field strength of 400
- _hil.Bearth = {400, 0, 0};
+ _sitl.Bearth = {400, 0, 0};
// rotate _Bearth for inclination and declination. -66 degrees
// is the inclination in Canberra, Australia
Matrix3f R;
R.from_euler(0, ToRad(66), get_declination());
- _hil.Bearth = R * _hil.Bearth;
+ _sitl.Bearth = R * _sitl.Bearth;
}
+#endif
/*
set the type of motor compensation to use
diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h
index 5457b77fce..1b73e193e3 100644
--- a/libraries/AP_Compass/AP_Compass.h
+++ b/libraries/AP_Compass/AP_Compass.h
@@ -289,14 +289,9 @@ public:
bool configured(uint8_t i);
bool configured(char *failure_msg, uint8_t failure_msg_len);
- // HIL methods
- void setHIL(uint8_t instance, float roll, float pitch, float yaw);
- void setHIL(uint8_t instance, const Vector3f &mag, uint32_t last_update_usec);
- const Vector3f& getHIL(uint8_t instance) const;
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
void _setup_earth_field();
-
- // enable HIL mode
- void set_hil_mode(void) { _hil_mode = true; }
+#endif
// return last update time in microseconds
uint32_t last_update_usec(void) const { return last_update_usec(_first_usable); }
@@ -310,10 +305,10 @@ public:
// HIL variables
struct {
Vector3f Bearth;
- float last_declination;
- bool healthy[COMPASS_MAX_INSTANCES];
- Vector3f field[COMPASS_MAX_INSTANCES];
- } _hil;
+ // float last_declination;
+ // bool healthy[COMPASS_MAX_INSTANCES];
+ // Vector3f field[COMPASS_MAX_INSTANCES];
+ } _sitl;
enum LearnType {
LEARN_NONE=0,
@@ -590,9 +585,6 @@ private:
Compass_PerMotor _per_motor{*this};
#endif
- // if we want HIL only
- bool _hil_mode:1;
-
AP_Float _calibration_threshold;
// mask of driver types to not load. Bit positions match DEVTYPE_ in backend
diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp
deleted file mode 100644
index ecf57af227..0000000000
--- a/libraries/AP_Compass/AP_Compass_HIL.cpp
+++ /dev/null
@@ -1,76 +0,0 @@
-/*
- This program is free software: you can redistribute it and/or modify
- it under the terms of the GNU General Public License as published by
- the Free Software Foundation, either version 3 of the License, or
- (at your option) any later version.
-
- This program is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- GNU General Public License for more details.
-
- You should have received a copy of the GNU General Public License
- along with this program. If not, see .
- */
-
-/*
- * AP_Compass_HIL.cpp - HIL backend for AP_Compass
- *
- */
-
-
-#include
-#include "AP_Compass_HIL.h"
-
-extern const AP_HAL::HAL& hal;
-
-// constructor
-AP_Compass_HIL::AP_Compass_HIL()
-{
- memset(_compass_instance, 0, sizeof(_compass_instance));
- _compass._setup_earth_field();
-}
-
-// detect the sensor
-AP_Compass_Backend *AP_Compass_HIL::detect()
-{
- AP_Compass_HIL *sensor = new AP_Compass_HIL();
- if (sensor == nullptr) {
- return nullptr;
- }
- if (!sensor->init()) {
- delete sensor;
- return nullptr;
- }
- return sensor;
-}
-
-bool
-AP_Compass_HIL::init(void)
-{
- // register compass instances
- for (uint8_t i=0; i