From 76aa7c485c28b3cb7b7db4af38f8c9c6790967d1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 9 Jun 2021 21:31:34 +1000 Subject: [PATCH] AP_Compass: remove HIL support --- libraries/AP_Compass/AP_Compass.cpp | 66 ++------------------- libraries/AP_Compass/AP_Compass.h | 20 ++----- libraries/AP_Compass/AP_Compass_HIL.cpp | 76 ------------------------- libraries/AP_Compass/AP_Compass_HIL.h | 19 ------- 4 files changed, 10 insertions(+), 171 deletions(-) delete mode 100644 libraries/AP_Compass/AP_Compass_HIL.cpp delete mode 100644 libraries/AP_Compass/AP_Compass_HIL.h 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