Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: remove HIL support

This commit is contained in:
Peter Barker 2021-06-09 21:31:35 +10:00 committed by Andrew Tridgell
parent dddd0d6a14
commit 52a52e1e5b
4 changed files with 1 additions and 183 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -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 = _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
*/

24
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

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

44
libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp
View File

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

19
libraries/AP_InertialSensor/AP_InertialSensor_HIL.h
View File

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