AP_InertialSensor: remove HIL support

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; i<get_accel_count(); i++) {
         if (!_accel_id_ok[i]) {
@@ -1495,7 +1481,6 @@ void AP_InertialSensor::update(void)
     // wait_for_sample(), and a wait is implied
     wait_for_sample();
 
-    if (!_hil_mode) {
         for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
             // mark sensors unhealthy and let update() in each backend
             // mark them healthy via _publish_gyro() and
@@ -1576,7 +1561,6 @@ void AP_InertialSensor::update(void)
                 break;
             }
         }
-    }
 
     _last_update_usec = AP_HAL::micros();
     
@@ -1650,7 +1634,6 @@ void AP_InertialSensor::wait_for_sample(void)
     }
 
 check_sample:
-    if (!_hil_mode) {
         // now we wait until we have the gyro and accel samples we need
         uint8_t gyro_available_mask = 0;
         uint8_t accel_available_mask = 0;
@@ -1714,15 +1697,9 @@ check_sample:
             hal.scheduler->delay_microseconds_boost(wait_per_loop);
             wait_counter++;
         }
-    }
 
     now = AP_HAL::micros();
-    if (_hil_mode && _hil.delta_time > 0) {
+    _delta_time = (now - _last_sample_usec) * 1.0e-6f;
-        _delta_time = _hil.delta_time;
-        _hil.delta_time = 0;
-    } else {
-        _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
  */

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;

libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp

@@ -1,44 +0,0 @@
-#include <AP_HAL/AP_HAL.h>
-#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;
-}

libraries/AP_InertialSensor/AP_InertialSensor_HIL.h

@@ -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);
-};