diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 04b40df5a2..77073b0eab 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -14,7 +14,6 @@ #include "AP_InertialSensor.h" #include "AP_InertialSensor_BMI160.h" #include "AP_InertialSensor_Backend.h" -#include "AP_InertialSensor_HIL.h" #include "AP_InertialSensor_L3G4200D.h" #include "AP_InertialSensor_LSM9DS0.h" #include "AP_InertialSensor_LSM9DS1.h" @@ -939,12 +938,6 @@ AP_InertialSensor::detect_backends(void) // macro for use by HAL_INS_PROBE_LIST #define GET_I2C_DEVICE(bus, address) hal.i2c_mgr->get_device(bus, address) - if (_hil_mode) { - ADD_BACKEND(AP_InertialSensor_HIL::detect(*this)); - return; - } - - #if HAL_EXTERNAL_AHRS_ENABLED // if enabled, make the first IMU the external AHRS if (int8_t serial_port = AP::externalAHRS().get_port() >= 0) { @@ -962,8 +955,6 @@ AP_InertialSensor::detect_backends(void) #if defined(HAL_SITL_INVENSENSEV3) ADD_BACKEND(AP_InertialSensor_Invensensev3::probe(*this, hal.i2c_mgr->get_device(1, 1), ROTATION_NONE)); #endif -#elif HAL_INS_DEFAULT == HAL_INS_HIL - ADD_BACKEND(AP_InertialSensor_HIL::detect(*this)); #elif AP_FEATURE_BOARD_DETECT switch (AP_BoardConfig::get_board_type()) { case AP_BoardConfig::PX4_BOARD_PX4V1: @@ -1257,11 +1248,6 @@ failed: */ bool AP_InertialSensor::accel_calibrated_ok_all() const { - // calibration is not applicable for HIL mode - if (_hil_mode) { - return true; - } - // check each accelerometer has offsets saved for (uint8_t i=0; idelay_microseconds_boost(wait_per_loop); wait_counter++; } - } now = AP_HAL::micros(); - if (_hil_mode && _hil.delta_time > 0) { - _delta_time = _hil.delta_time; - _hil.delta_time = 0; - } else { - _delta_time = (now - _last_sample_usec) * 1.0e-6f; - } + _delta_time = (now - _last_sample_usec) * 1.0e-6f; _last_sample_usec = now; #if 0 @@ -1793,78 +1770,6 @@ bool AP_InertialSensor::get_delta_velocity(uint8_t i, Vector3f &delta_velocity, return false; } -/* - support for setting accel and gyro vectors, for use by HIL - */ -void AP_InertialSensor::set_accel(uint8_t instance, const Vector3f &accel) -{ - if (_accel_count == 0) { - // we haven't initialised yet - return; - } - if (instance < INS_MAX_INSTANCES) { - _accel[instance] = accel; - _accel_healthy[instance] = true; - if (_accel_count <= instance) { - _accel_count = instance+1; - } - if (!_accel_healthy[_primary_accel]) { - _primary_accel = instance; - } - } -} - -void AP_InertialSensor::set_gyro(uint8_t instance, const Vector3f &gyro) -{ - if (_gyro_count == 0) { - // we haven't initialised yet - return; - } - if (instance < INS_MAX_INSTANCES) { - _gyro[instance] = gyro; - _gyro_healthy[instance] = true; - if (_gyro_count <= instance) { - _gyro_count = instance+1; - _gyro_cal_ok[instance] = true; - } - if (!_accel_healthy[_primary_accel]) { - _primary_accel = instance; - } - } -} - -/* - set delta time for next ins.update() - */ -void AP_InertialSensor::set_delta_time(float delta_time) -{ - _hil.delta_time = delta_time; -} - -/* - set delta velocity for next update - */ -void AP_InertialSensor::set_delta_velocity(uint8_t instance, float deltavt, const Vector3f &deltav) -{ - if (instance < INS_MAX_INSTANCES) { - _delta_velocity_valid[instance] = true; - _delta_velocity[instance] = deltav; - _delta_velocity_dt[instance] = deltavt; - } -} - -/* - set delta angle for next update - */ -void AP_InertialSensor::set_delta_angle(uint8_t instance, const Vector3f &deltaa, float deltaat) -{ - if (instance < INS_MAX_INSTANCES) { - _delta_angle_valid[instance] = true; - _delta_angle[instance] = deltaa; - _delta_angle_dt[instance] = deltaat; - } -} - /* * Get an AuxiliaryBus of N @instance of backend identified by @backend_id */ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 2d5cdc7d19..fa21f7b99a 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -243,9 +243,6 @@ public: // Update the harmonic notch frequencies void update_harmonic_notch_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]); - // enable HIL mode - void set_hil_mode(void) { _hil_mode = true; } - // get the gyro filter rate in Hz uint16_t get_gyro_filter_hz(void) const { return _gyro_filter_cutoff; } @@ -301,17 +298,6 @@ public: // return true if harmonic notch enabled bool gyro_harmonic_notch_enabled(void) const { return _harmonic_notch_filter.enabled(); } - /* - HIL set functions. The minimum for HIL is set_accel() and - set_gyro(). The others are option for higher fidelity log - playback - */ - void set_accel(uint8_t instance, const Vector3f &accel); - void set_gyro(uint8_t instance, const Vector3f &gyro); - void set_delta_time(float delta_time); - void set_delta_velocity(uint8_t instance, float deltavt, const Vector3f &deltav); - void set_delta_angle(uint8_t instance, const Vector3f &deltaa, float deltaat); - AuxiliaryBus *get_auxiliary_bus(int16_t backend_id) { return get_auxiliary_bus(backend_id, 0); } AuxiliaryBus *get_auxiliary_bus(int16_t backend_id, uint8_t instance); @@ -622,9 +608,6 @@ private: // has wait_for_sample() found a sample? bool _have_sample:1; - // are we in HIL mode? - bool _hil_mode:1; - bool _backends_detected:1; // are gyros or accels currently being calibrated @@ -667,13 +650,6 @@ private: // threshold for detecting stillness AP_Float _still_threshold; - /* - state for HIL support - */ - struct { - float delta_time; - } _hil {}; - // Trim options AP_Int8 _acc_body_aligned; AP_Int8 _trim_option; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp deleted file mode 100644 index 56d1d114d3..0000000000 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp +++ /dev/null @@ -1,44 +0,0 @@ -#include -#include "AP_InertialSensor_HIL.h" - -const extern AP_HAL::HAL& hal; - -AP_InertialSensor_HIL::AP_InertialSensor_HIL(AP_InertialSensor &imu) : - AP_InertialSensor_Backend(imu) -{ -} - -/* - detect the sensor - */ -AP_InertialSensor_Backend *AP_InertialSensor_HIL::detect(AP_InertialSensor &_imu) -{ - AP_InertialSensor_HIL *sensor = new AP_InertialSensor_HIL(_imu); - if (sensor == nullptr) { - return nullptr; - } - if (!sensor->_init_sensor()) { - delete sensor; - return nullptr; - } - return sensor; -} - -bool AP_InertialSensor_HIL::_init_sensor(void) -{ - // grab the used instances - uint8_t instance; - _imu.register_gyro(instance, 1200, 1); - _imu.register_accel(instance, 1200, 1); - - _imu.set_hil_mode(); - - return true; -} - -bool AP_InertialSensor_HIL::update(void) -{ - // the data is stored directly in the frontend, so update() - // doesn't need to do anything - return true; -} diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h deleted file mode 100644 index db2627da35..0000000000 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h +++ /dev/null @@ -1,19 +0,0 @@ -#pragma once - -#include "AP_InertialSensor.h" -#include "AP_InertialSensor_Backend.h" - -class AP_InertialSensor_HIL : public AP_InertialSensor_Backend -{ -public: - AP_InertialSensor_HIL(AP_InertialSensor &imu); - - /* update accel and gyro state */ - bool update() override; - - // detect the sensor - static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); - -private: - bool _init_sensor(void); -};