diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 7bb6b294f2..88d1afb84f 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -34,7 +34,6 @@ #include "AP_Baro_BMP085.h" #include "AP_Baro_BMP280.h" #include "AP_Baro_SPL06.h" -#include "AP_Baro_HIL.h" #include "AP_Baro_KellerLD.h" #include "AP_Baro_MS5611.h" #include "AP_Baro_ICM20789.h" @@ -423,9 +422,6 @@ float AP_Baro::get_air_density_ratio(void) // note that this relies on read() being called regularly to get new data float AP_Baro::get_climb_rate(void) { - if (_hil.have_alt) { - return _hil.climb_rate; - } // we use a 7 point derivative filter on the climb rate. This seems // to produce somewhat reasonable results on real hardware return _climb_rate_filter.slope() * 1.0e3f; @@ -528,12 +524,6 @@ void AP_Baro::init(void) sensors[i].bus_id.set(0); } - if (_hil_mode) { - drivers[0] = new AP_Baro_HIL(*this); - _num_drivers = 1; - return; - } - #if HAL_ENABLE_LIBUAVCAN_DRIVERS // Detect UAVCAN Modules, try as many times as there are driver slots for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) { @@ -636,9 +626,6 @@ void AP_Baro::init(void) for(uint8_t i = 0; i < sitl->baro_count; i++) { ADD_BACKEND(new AP_Baro_SITL(*this)); } -#elif HAL_BARO_DEFAULT == HAL_BARO_HIL - drivers[0] = new AP_Baro_HIL(*this); - _num_drivers = 1; #elif HAL_BARO_DEFAULT == HAL_BARO_LPS25H_IMU_I2C ADD_BACKEND(AP_Baro_LPS2XH::probe_InvensenseIMU(*this, std::move(GET_I2C_DEVICE(HAL_BARO_LPS25H_I2C_BUS, HAL_BARO_LPS25H_I2C_ADDR)), @@ -837,10 +824,8 @@ void AP_Baro::update(void) _alt_offset_active = _alt_offset; } - if (!_hil_mode) { - for (uint8_t i=0; i<_num_drivers; i++) { - drivers[i]->backend_update(i); - } + for (uint8_t i=0; i<_num_drivers; i++) { + drivers[i]->backend_update(i); } for (uint8_t i=0; i<_num_sensors; i++) { @@ -868,12 +853,6 @@ void AP_Baro::update(void) sensors[i].altitude = altitude + _alt_offset_active; } } - if (_hil.have_alt) { - sensors[0].altitude = _hil.altitude; - } - if (_hil.have_last_update) { - sensors[0].last_update_ms = _hil.last_update_ms; - } } // ensure the climb rate filter is updated diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index fcaa9edd85..1973d827ec 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -145,13 +145,6 @@ public: float get_external_temperature(void) const { return get_external_temperature(_primary); }; float get_external_temperature(const uint8_t instance) const; - // HIL (and SITL) interface, setting altitude - void setHIL(float altitude_msl); - - // HIL (and SITL) interface, setting pressure, temperature, altitude and climb_rate - // used by Replay - void setHIL(uint8_t instance, float pressure, float temperature, float altitude, float climb_rate, uint32_t last_update_ms); - // Set the primary baro void set_primary_baro(uint8_t primary) { _primary_baro.set_and_save(primary); }; @@ -161,18 +154,6 @@ public: // Get the type (Air or Water) of a particular instance baro_type_t get_type(uint8_t instance) { return sensors[instance].type; }; - // HIL variables - struct { - float pressure; - float temperature; - float altitude; - float climb_rate; - uint32_t last_update_ms; - bool updated:1; - bool have_alt:1; - bool have_last_update:1; - } _hil; - // register a new sensor, claiming a sensor slot. If we are out of // slots it will panic uint8_t register_sensor(void); @@ -180,9 +161,6 @@ public: // return number of registered sensors uint8_t num_instances(void) const { return _num_sensors; } - // enable HIL mode - void set_hil_mode(void) { _hil_mode = true; } - // set baro drift amount void set_baro_drift_altitude(float alt) { _alt_offset = alt; } @@ -298,7 +276,6 @@ private: DerivativeFilterFloat_Size7 _climb_rate_filter; AP_Float _specific_gravity; // the specific gravity of fluid for an ROV 1.00 for freshwater, 1.024 for salt water AP_Float _user_ground_temperature; // user override of the ground temperature used for EAS2TAS - bool _hil_mode:1; float _guessed_ground_temperature; // currently ground temperature estimate using our best abailable source // when did we last notify the GCS of new pressure reference? diff --git a/libraries/AP_Baro/AP_Baro_HIL.cpp b/libraries/AP_Baro/AP_Baro_HIL.cpp index 52de181fb8..7a60f6221b 100644 --- a/libraries/AP_Baro/AP_Baro_HIL.cpp +++ b/libraries/AP_Baro/AP_Baro_HIL.cpp @@ -1,15 +1,9 @@ -#include "AP_Baro_HIL.h" +#include "AP_Baro.h" #include extern const AP_HAL::HAL& hal; -AP_Baro_HIL::AP_Baro_HIL(AP_Baro &baro) : - AP_Baro_Backend(baro) -{ - _instance = _frontend.register_sensor(); -} - // ========================================================================== // based on tables.cpp from http://www.pdas.com/atmosdownload.html @@ -76,50 +70,3 @@ void AP_Baro::SimpleUnderWaterAtmosphere( const float S = seaTempSurface * 0.338f; theta = 1.0f / ((1.8e-4f) * S * (alt * 1e3f) + 1.0f); } - -/* - convert an altitude in meters above sea level to a presssure and temperature - */ -void AP_Baro::setHIL(float altitude_msl) -{ - float sigma, delta, theta; - - SimpleAtmosphere(altitude_msl*0.001f, sigma, delta, theta); - float p = SSL_AIR_PRESSURE * delta; - float T = 303.16f * theta - C_TO_KELVIN; // Assume 30 degrees at sea level - converted to degrees Kelvin - - _hil.pressure = p; - _hil.temperature = T; - _hil.updated = true; -} - -/* - set HIL pressure and temperature for an instance - */ -void AP_Baro::setHIL(uint8_t instance, float pressure, float temperature, float altitude, float climb_rate, uint32_t last_update_ms) -{ - if (instance >= _num_sensors) { - // invalid - return; - } - _hil.pressure = pressure; - _hil.temperature = temperature; - _hil.altitude = altitude; - _hil.climb_rate = climb_rate; - _hil.updated = true; - _hil.have_alt = true; - - if (last_update_ms != 0) { - _hil.last_update_ms = last_update_ms; - _hil.have_last_update = true; - } -} - -// Read the sensor -void AP_Baro_HIL::update(void) -{ - if (_frontend._hil.updated) { - _frontend._hil.updated = false; - _copy_to_frontend(0, _frontend._hil.pressure, _frontend._hil.temperature); - } -} diff --git a/libraries/AP_Baro/AP_Baro_HIL.h b/libraries/AP_Baro/AP_Baro_HIL.h deleted file mode 100644 index 0d0811bc88..0000000000 --- a/libraries/AP_Baro/AP_Baro_HIL.h +++ /dev/null @@ -1,17 +0,0 @@ -/* - dummy backend for HIL (and SITL). This doesn't actually need to do - any work, as setHIL() is in the frontend - */ -#pragma once - -#include "AP_Baro_Backend.h" - -class AP_Baro_HIL : public AP_Baro_Backend -{ -public: - AP_Baro_HIL(AP_Baro &baro); - void update(void) override; - -private: - uint8_t _instance; -};