AP_Compass: remove HIL support

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

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

libraries/AP_Compass/AP_Compass_HIL.cpp

@@ -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 <http://www.gnu.org/licenses/>.
- */
-
-/*
- *       AP_Compass_HIL.cpp - HIL backend for AP_Compass
- *
- */
-
-
-#include <AP_HAL/AP_HAL.h>
-#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<HIL_NUM_COMPASSES; i++) {
-        uint32_t dev_id = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, i, 0, DEVTYPE_SITL);
-        if (!register_compass(dev_id, _compass_instance[i])) {
-            return false;
-        }
-        set_dev_id(_compass_instance[i], dev_id);
-    }
-    return true;
-}
-
-void AP_Compass_HIL::read()
-{
-    for (uint8_t i=0; i < ARRAY_SIZE(_compass_instance); i++) {
-        if (_compass._hil.healthy[i]) {
-            uint8_t compass_instance = _compass_instance[i];
-            Vector3f field = _compass._hil.field[compass_instance];
-            rotate_field(field, compass_instance);
-            publish_raw_field(field, compass_instance);
-            correct_field(field, compass_instance);
-            uint32_t saved_last_update = _compass.last_update_usec(compass_instance);
-            publish_filtered_field(field, compass_instance);
-            set_last_update_usec(saved_last_update, compass_instance);
-        }
-    }
-}

libraries/AP_Compass/AP_Compass_HIL.h

@@ -1,19 +0,0 @@
-#pragma once
-
-#include "AP_Compass.h"
-
-#define HIL_NUM_COMPASSES 1
-
-class AP_Compass_HIL : public AP_Compass_Backend
-{
-public:
-    AP_Compass_HIL();
-    void read(void) override;
-    bool init(void);
-
-    // detect the sensor
-    static AP_Compass_Backend *detect();
-
-private:
-    uint8_t     _compass_instance[HIL_NUM_COMPASSES];
-};