mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 23:18:28 -04:00
0e9b2a26c5
use regulated time for frequency noise to avoid spurious harmonics SITL sensors must be true separate instances don't compile in FFT structures if DSP disabled
649 lines
24 KiB
C++
649 lines
24 KiB
C++
#pragma once
|
|
|
|
// Gyro and Accelerometer calibration criteria
|
|
#define AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE 4.0f
|
|
#define AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET 250.0f
|
|
#define AP_INERTIAL_SENSOR_ACCEL_VIBE_FLOOR_FILT_HZ 5.0f // accel vibration floor filter hz
|
|
#define AP_INERTIAL_SENSOR_ACCEL_VIBE_FILT_HZ 2.0f // accel vibration filter hz
|
|
#define AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS 500 // peak-hold detector timeout
|
|
|
|
/**
|
|
maximum number of INS instances available on this platform. If more
|
|
than 1 then redundant sensors may be available
|
|
*/
|
|
#define INS_MAX_INSTANCES 3
|
|
#define INS_MAX_BACKENDS 6
|
|
#define INS_VIBRATION_CHECK_INSTANCES 2
|
|
#define XYZ_AXIS_COUNT 3
|
|
// The maximum we need to store is gyro-rate / loop-rate, worst case ArduCopter with BMI088 is 2000/400
|
|
#define INS_MAX_GYRO_WINDOW_SAMPLES 8
|
|
typedef float* GyroWindow;
|
|
|
|
#define DEFAULT_IMU_LOG_BAT_MASK 0
|
|
|
|
#include <stdint.h>
|
|
|
|
#include <AP_AccelCal/AP_AccelCal.h>
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <Filter/LowPassFilter2p.h>
|
|
#include <Filter/LowPassFilter.h>
|
|
#include <Filter/NotchFilter.h>
|
|
#include <Filter/HarmonicNotchFilter.h>
|
|
|
|
class AP_InertialSensor_Backend;
|
|
class AuxiliaryBus;
|
|
class AP_AHRS;
|
|
|
|
/*
|
|
forward declare AP_Logger class. We can't include logger.h
|
|
because of mutual dependencies
|
|
*/
|
|
class AP_Logger;
|
|
|
|
/* AP_InertialSensor is an abstraction for gyro and accel measurements
|
|
* which are correctly aligned to the body axes and scaled to SI units.
|
|
*
|
|
* Gauss-Newton accel calibration routines borrowed from Rolfe Schmidt
|
|
* blog post describing the method: http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
|
|
* original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
|
|
*/
|
|
class AP_InertialSensor : AP_AccelCal_Client
|
|
{
|
|
friend class AP_InertialSensor_Backend;
|
|
|
|
public:
|
|
AP_InertialSensor();
|
|
|
|
/* Do not allow copies */
|
|
AP_InertialSensor(const AP_InertialSensor &other) = delete;
|
|
AP_InertialSensor &operator=(const AP_InertialSensor&) = delete;
|
|
|
|
static AP_InertialSensor *get_singleton();
|
|
|
|
enum Gyro_Calibration_Timing {
|
|
GYRO_CAL_NEVER = 0,
|
|
GYRO_CAL_STARTUP_ONLY = 1
|
|
};
|
|
|
|
/// Perform startup initialisation.
|
|
///
|
|
/// Called to initialise the state of the IMU.
|
|
///
|
|
/// Gyros will be calibrated unless INS_GYRO_CAL is zero
|
|
///
|
|
/// @param style The initialisation startup style.
|
|
///
|
|
void init(uint16_t sample_rate_hz);
|
|
|
|
/// Register a new gyro/accel driver, allocating an instance
|
|
/// number
|
|
uint8_t register_gyro(uint16_t raw_sample_rate_hz, uint32_t id);
|
|
uint8_t register_accel(uint16_t raw_sample_rate_hz, uint32_t id);
|
|
|
|
// a function called by the main thread at the main loop rate:
|
|
void periodic();
|
|
|
|
bool calibrate_trim(float &trim_roll, float &trim_pitch);
|
|
|
|
/// calibrating - returns true if the gyros or accels are currently being calibrated
|
|
bool calibrating() const { return _calibrating; }
|
|
|
|
/// Perform cold-start initialisation for just the gyros.
|
|
///
|
|
/// @note This should not be called unless ::init has previously
|
|
/// been called, as ::init may perform other work
|
|
///
|
|
void init_gyro(void);
|
|
|
|
/// Fetch the current gyro values
|
|
///
|
|
/// @returns vector of rotational rates in radians/sec
|
|
///
|
|
const Vector3f &get_gyro(uint8_t i) const { return _gyro[i]; }
|
|
const Vector3f &get_gyro(void) const { return get_gyro(_primary_gyro); }
|
|
|
|
// set gyro offsets in radians/sec
|
|
const Vector3f &get_gyro_offsets(uint8_t i) const { return _gyro_offset[i]; }
|
|
const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_primary_gyro); }
|
|
|
|
//get delta angle if available
|
|
bool get_delta_angle(uint8_t i, Vector3f &delta_angle) const;
|
|
bool get_delta_angle(Vector3f &delta_angle) const { return get_delta_angle(_primary_gyro, delta_angle); }
|
|
|
|
float get_delta_angle_dt(uint8_t i) const;
|
|
float get_delta_angle_dt() const { return get_delta_angle_dt(_primary_accel); }
|
|
|
|
//get delta velocity if available
|
|
bool get_delta_velocity(uint8_t i, Vector3f &delta_velocity) const;
|
|
bool get_delta_velocity(Vector3f &delta_velocity) const { return get_delta_velocity(_primary_accel, delta_velocity); }
|
|
|
|
float get_delta_velocity_dt(uint8_t i) const;
|
|
float get_delta_velocity_dt() const { return get_delta_velocity_dt(_primary_accel); }
|
|
|
|
/// Fetch the current accelerometer values
|
|
///
|
|
/// @returns vector of current accelerations in m/s/s
|
|
///
|
|
const Vector3f &get_accel(uint8_t i) const { return _accel[i]; }
|
|
const Vector3f &get_accel(void) const { return get_accel(_primary_accel); }
|
|
|
|
uint32_t get_gyro_error_count(uint8_t i) const { return _gyro_error_count[i]; }
|
|
uint32_t get_accel_error_count(uint8_t i) const { return _accel_error_count[i]; }
|
|
|
|
// multi-device interface
|
|
bool get_gyro_health(uint8_t instance) const { return (instance<_gyro_count) ? _gyro_healthy[instance] : false; }
|
|
bool get_gyro_health(void) const { return get_gyro_health(_primary_gyro); }
|
|
bool get_gyro_health_all(void) const;
|
|
uint8_t get_gyro_count(void) const { return _gyro_count; }
|
|
bool gyro_calibrated_ok(uint8_t instance) const { return _gyro_cal_ok[instance]; }
|
|
bool gyro_calibrated_ok_all() const;
|
|
bool use_gyro(uint8_t instance) const;
|
|
Gyro_Calibration_Timing gyro_calibration_timing();
|
|
|
|
bool get_accel_health(uint8_t instance) const { return (instance<_accel_count) ? _accel_healthy[instance] : false; }
|
|
bool get_accel_health(void) const { return get_accel_health(_primary_accel); }
|
|
bool get_accel_health_all(void) const;
|
|
uint8_t get_accel_count(void) const { return _accel_count; }
|
|
bool accel_calibrated_ok_all() const;
|
|
bool use_accel(uint8_t instance) const;
|
|
|
|
// get observed sensor rates, including any internal sampling multiplier
|
|
uint16_t get_gyro_rate_hz(uint8_t instance) const { return uint16_t(_gyro_raw_sample_rates[instance] * _gyro_over_sampling[instance]); }
|
|
uint16_t get_accel_rate_hz(uint8_t instance) const { return uint16_t(_accel_raw_sample_rates[instance] * _accel_over_sampling[instance]); }
|
|
|
|
// FFT support access
|
|
#if HAL_WITH_DSP
|
|
const Vector3f &get_raw_gyro(void) const { return _gyro_raw[_primary_gyro]; }
|
|
GyroWindow get_raw_gyro_window(uint8_t instance, uint8_t axis) const { return _gyro_window[instance][axis]; }
|
|
GyroWindow get_raw_gyro_window(uint8_t axis) const { return get_raw_gyro_window(_primary_gyro, axis); }
|
|
// returns the index one above the last valid gyro sample
|
|
uint16_t get_raw_gyro_window_index(void) const { return get_raw_gyro_window_index(_primary_gyro); }
|
|
uint16_t get_raw_gyro_window_index(uint8_t instance) const { return _circular_buffer_idx[instance]; }
|
|
uint16_t get_raw_gyro_rate_hz() const { return get_raw_gyro_rate_hz(_primary_gyro); }
|
|
uint16_t get_raw_gyro_rate_hz(uint8_t instance) const { return _gyro_raw_sample_rates[_primary_gyro]; }
|
|
#endif
|
|
bool set_gyro_window_size(uint16_t size);
|
|
// get accel offsets in m/s/s
|
|
const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset[i]; }
|
|
const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(_primary_accel); }
|
|
|
|
// get accel scale
|
|
const Vector3f &get_accel_scale(uint8_t i) const { return _accel_scale[i]; }
|
|
const Vector3f &get_accel_scale(void) const { return get_accel_scale(_primary_accel); }
|
|
|
|
// return a 3D vector defining the position offset of the IMU accelerometer in metres relative to the body frame origin
|
|
const Vector3f &get_imu_pos_offset(uint8_t instance) const {
|
|
return _accel_pos[instance];
|
|
}
|
|
const Vector3f &get_imu_pos_offset(void) const {
|
|
return _accel_pos[_primary_accel];
|
|
}
|
|
|
|
// return the temperature if supported. Zero is returned if no
|
|
// temperature is available
|
|
float get_temperature(uint8_t instance) const { return _temperature[instance]; }
|
|
|
|
/* get_delta_time returns the time period in seconds
|
|
* overwhich the sensor data was collected
|
|
*/
|
|
float get_delta_time() const { return MIN(_delta_time, _loop_delta_t_max); }
|
|
|
|
// return the maximum gyro drift rate in radians/s/s. This
|
|
// depends on what gyro chips are being used
|
|
float get_gyro_drift_rate(void) const { return ToRad(0.5f/60); }
|
|
|
|
// update gyro and accel values from accumulated samples
|
|
void update(void);
|
|
|
|
// wait for a sample to be available
|
|
void wait_for_sample(void);
|
|
|
|
// class level parameters
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
// set overall board orientation
|
|
void set_board_orientation(enum Rotation orientation, Matrix3f* custom_rotation = nullptr) {
|
|
_board_orientation = orientation;
|
|
_custom_rotation = custom_rotation;
|
|
}
|
|
|
|
// return the selected sample rate
|
|
uint16_t get_sample_rate(void) const { return _sample_rate; }
|
|
|
|
// return the main loop delta_t in seconds
|
|
float get_loop_delta_t(void) const { return _loop_delta_t; }
|
|
|
|
bool healthy(void) const { return get_gyro_health() && get_accel_health(); }
|
|
|
|
uint8_t get_primary_accel(void) const { return _primary_accel; }
|
|
uint8_t get_primary_gyro(void) const { return _primary_gyro; }
|
|
|
|
// Update the harmonic notch frequency
|
|
void update_harmonic_notch_freq_hz(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; }
|
|
|
|
// get the accel filter rate in Hz
|
|
uint16_t get_accel_filter_hz(void) const { return _accel_filter_cutoff; }
|
|
|
|
// harmonic notch current center frequency
|
|
float get_gyro_dynamic_notch_center_freq_hz(void) const { return _calculated_harmonic_notch_freq_hz; }
|
|
|
|
// harmonic notch reference center frequency
|
|
float get_gyro_harmonic_notch_center_freq_hz(void) const { return _harmonic_notch_filter.center_freq_hz(); }
|
|
|
|
// harmonic notch reference scale factor
|
|
float get_gyro_harmonic_notch_reference(void) const { return _harmonic_notch_filter.reference(); }
|
|
|
|
// harmonic notch tracking mode
|
|
HarmonicNotchDynamicMode get_gyro_harmonic_notch_tracking_mode(void) const { return _harmonic_notch_filter.tracking_mode(); }
|
|
|
|
// harmonic notch harmonics
|
|
uint8_t get_gyro_harmonic_notch_harmonics(void) const { return _harmonic_notch_filter.harmonics(); }
|
|
|
|
// indicate which bit in LOG_BITMASK indicates raw logging enabled
|
|
void set_log_raw_bit(uint32_t log_raw_bit) { _log_raw_bit = log_raw_bit; }
|
|
|
|
// calculate vibration levels and check for accelerometer clipping (called by a backends)
|
|
void calc_vibration_and_clipping(uint8_t instance, const Vector3f &accel, float dt);
|
|
|
|
// retrieve latest calculated vibration levels
|
|
Vector3f get_vibration_levels() const { return get_vibration_levels(_primary_accel); }
|
|
Vector3f get_vibration_levels(uint8_t instance) const;
|
|
|
|
// retrieve and clear accelerometer clipping count
|
|
uint32_t get_accel_clip_count(uint8_t instance) const;
|
|
|
|
// check for vibration movement. True when all axis show nearly zero movement
|
|
bool is_still();
|
|
|
|
// 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);
|
|
|
|
void detect_backends(void);
|
|
|
|
// accel peak hold detector
|
|
void set_accel_peak_hold(uint8_t instance, const Vector3f &accel);
|
|
float get_accel_peak_hold_neg_x() const { return _peak_hold_state.accel_peak_hold_neg_x; }
|
|
|
|
//Returns accel calibrator interface object pointer
|
|
AP_AccelCal* get_acal() const { return _acal; }
|
|
|
|
// Returns body fixed accelerometer level data averaged during accel calibration's first step
|
|
bool get_fixed_mount_accel_cal_sample(uint8_t sample_num, Vector3f& ret) const;
|
|
|
|
// Returns primary accelerometer level data averaged during accel calibration's first step
|
|
bool get_primary_accel_cal_sample_avg(uint8_t sample_num, Vector3f& ret) const;
|
|
|
|
// Returns newly calculated trim values if calculated
|
|
bool get_new_trim(float& trim_roll, float &trim_pitch);
|
|
|
|
// initialise and register accel calibrator
|
|
// called during the startup of accel cal
|
|
void acal_init();
|
|
|
|
// update accel calibrator
|
|
void acal_update();
|
|
|
|
// simple accel calibration
|
|
MAV_RESULT simple_accel_cal();
|
|
|
|
bool accel_cal_requires_reboot() const { return _accel_cal_requires_reboot; }
|
|
|
|
// return time in microseconds of last update() call
|
|
uint32_t get_last_update_usec(void) const { return _last_update_usec; }
|
|
|
|
// for killing an IMU for testing purposes
|
|
void kill_imu(uint8_t imu_idx, bool kill_it);
|
|
|
|
enum IMU_SENSOR_TYPE {
|
|
IMU_SENSOR_TYPE_ACCEL = 0,
|
|
IMU_SENSOR_TYPE_GYRO = 1,
|
|
};
|
|
|
|
class BatchSampler {
|
|
public:
|
|
BatchSampler(const AP_InertialSensor &imu) :
|
|
type(IMU_SENSOR_TYPE_ACCEL),
|
|
_imu(imu) {
|
|
AP_Param::setup_object_defaults(this, var_info);
|
|
};
|
|
|
|
void init();
|
|
void sample(uint8_t instance, IMU_SENSOR_TYPE _type, uint64_t sample_us, const Vector3f &sample);
|
|
|
|
// a function called by the main thread at the main loop rate:
|
|
void periodic();
|
|
|
|
bool doing_sensor_rate_logging() const { return _doing_sensor_rate_logging; }
|
|
bool doing_post_filter_logging() const { return _doing_post_filter_logging; }
|
|
|
|
// class level parameters
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
// Parameters
|
|
AP_Int16 _required_count;
|
|
AP_Int8 _sensor_mask;
|
|
AP_Int8 _batch_options_mask;
|
|
|
|
// Parameters controlling pushing data to AP_Logger:
|
|
// Each DF message is ~ 108 bytes in size, so we use about 1kB/s of
|
|
// logging bandwidth with a 100ms interval. If we are taking
|
|
// 1024 samples then we need to send 32 packets, so it will
|
|
// take ~3 seconds to push a complete batch to the log. If
|
|
// you are running a on an FMU with three IMUs then you
|
|
// will loop back around to the first sensor after about
|
|
// twenty seconds.
|
|
AP_Int16 samples_per_msg;
|
|
AP_Int8 push_interval_ms;
|
|
|
|
// end Parameters
|
|
|
|
private:
|
|
|
|
enum batch_opt_t {
|
|
BATCH_OPT_SENSOR_RATE = (1<<0),
|
|
BATCH_OPT_POST_FILTER = (1<<1),
|
|
};
|
|
|
|
void rotate_to_next_sensor();
|
|
void update_doing_sensor_rate_logging();
|
|
|
|
bool should_log(uint8_t instance, IMU_SENSOR_TYPE type);
|
|
void push_data_to_log();
|
|
|
|
uint64_t measurement_started_us;
|
|
|
|
bool initialised : 1;
|
|
bool isbh_sent : 1;
|
|
bool _doing_sensor_rate_logging : 1;
|
|
bool _doing_post_filter_logging : 1;
|
|
uint8_t instance : 3; // instance we are sending data for
|
|
AP_InertialSensor::IMU_SENSOR_TYPE type : 1;
|
|
uint16_t isb_seqnum;
|
|
int16_t *data_x;
|
|
int16_t *data_y;
|
|
int16_t *data_z;
|
|
uint16_t data_write_offset; // units: samples
|
|
uint16_t data_read_offset; // units: samples
|
|
uint32_t last_sent_ms;
|
|
|
|
// all samples are multiplied by this
|
|
uint16_t multiplier; // initialised as part of init()
|
|
|
|
const AP_InertialSensor &_imu;
|
|
};
|
|
BatchSampler batchsampler{*this};
|
|
|
|
private:
|
|
// load backend drivers
|
|
bool _add_backend(AP_InertialSensor_Backend *backend);
|
|
void _start_backends();
|
|
AP_InertialSensor_Backend *_find_backend(int16_t backend_id, uint8_t instance);
|
|
|
|
// gyro initialisation
|
|
void _init_gyro();
|
|
|
|
// Calibration routines borrowed from Rolfe Schmidt
|
|
// blog post describing the method: http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
|
|
// original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
|
|
|
|
bool _calculate_trim(const Vector3f &accel_sample, float& trim_roll, float& trim_pitch);
|
|
|
|
// save gyro calibration values to eeprom
|
|
void _save_gyro_calibration();
|
|
|
|
// backend objects
|
|
AP_InertialSensor_Backend *_backends[INS_MAX_BACKENDS];
|
|
|
|
// number of gyros and accel drivers. Note that most backends
|
|
// provide both accel and gyro data, so will increment both
|
|
// counters on initialisation
|
|
uint8_t _gyro_count;
|
|
uint8_t _accel_count;
|
|
uint8_t _backend_count;
|
|
|
|
// the selected sample rate
|
|
uint16_t _sample_rate;
|
|
float _loop_delta_t;
|
|
float _loop_delta_t_max;
|
|
|
|
// Most recent accelerometer reading
|
|
Vector3f _accel[INS_MAX_INSTANCES];
|
|
Vector3f _delta_velocity[INS_MAX_INSTANCES];
|
|
float _delta_velocity_dt[INS_MAX_INSTANCES];
|
|
bool _delta_velocity_valid[INS_MAX_INSTANCES];
|
|
// delta velocity accumulator
|
|
Vector3f _delta_velocity_acc[INS_MAX_INSTANCES];
|
|
// time accumulator for delta velocity accumulator
|
|
float _delta_velocity_acc_dt[INS_MAX_INSTANCES];
|
|
|
|
// Low Pass filters for gyro and accel
|
|
LowPassFilter2pVector3f _accel_filter[INS_MAX_INSTANCES];
|
|
LowPassFilter2pVector3f _gyro_filter[INS_MAX_INSTANCES];
|
|
Vector3f _accel_filtered[INS_MAX_INSTANCES];
|
|
Vector3f _gyro_filtered[INS_MAX_INSTANCES];
|
|
#if HAL_WITH_DSP
|
|
// Thread-safe public version of _last_raw_gyro
|
|
Vector3f _gyro_raw[INS_MAX_INSTANCES];
|
|
// circular buffer of gyro data for frequency analysis
|
|
uint16_t _circular_buffer_idx[INS_MAX_INSTANCES];
|
|
GyroWindow _gyro_window[INS_MAX_INSTANCES][XYZ_AXIS_COUNT];
|
|
uint16_t _gyro_window_size;
|
|
#endif
|
|
bool _new_accel_data[INS_MAX_INSTANCES];
|
|
bool _new_gyro_data[INS_MAX_INSTANCES];
|
|
|
|
// optional notch filter on gyro
|
|
NotchFilterParams _notch_filter;
|
|
NotchFilterVector3f _gyro_notch_filter[INS_MAX_INSTANCES];
|
|
|
|
// optional harmonic notch filter on gyro
|
|
HarmonicNotchFilterParams _harmonic_notch_filter;
|
|
HarmonicNotchFilterVector3f _gyro_harmonic_notch_filter[INS_MAX_INSTANCES];
|
|
// the current center frequency for the notch
|
|
float _calculated_harmonic_notch_freq_hz;
|
|
|
|
// Most recent gyro reading
|
|
Vector3f _gyro[INS_MAX_INSTANCES];
|
|
Vector3f _delta_angle[INS_MAX_INSTANCES];
|
|
float _delta_angle_dt[INS_MAX_INSTANCES];
|
|
bool _delta_angle_valid[INS_MAX_INSTANCES];
|
|
// time accumulator for delta angle accumulator
|
|
float _delta_angle_acc_dt[INS_MAX_INSTANCES];
|
|
Vector3f _delta_angle_acc[INS_MAX_INSTANCES];
|
|
Vector3f _last_delta_angle[INS_MAX_INSTANCES];
|
|
Vector3f _last_raw_gyro[INS_MAX_INSTANCES];
|
|
|
|
// bitmask indicating if a sensor is doing sensor-rate sampling:
|
|
uint8_t _accel_sensor_rate_sampling_enabled;
|
|
uint8_t _gyro_sensor_rate_sampling_enabled;
|
|
|
|
// multipliers for data supplied via sensor-rate logging:
|
|
uint16_t _accel_raw_sampling_multiplier[INS_MAX_INSTANCES];
|
|
uint16_t _gyro_raw_sampling_multiplier[INS_MAX_INSTANCES];
|
|
|
|
// IDs to uniquely identify each sensor: shall remain
|
|
// the same across reboots
|
|
AP_Int32 _accel_id[INS_MAX_INSTANCES];
|
|
AP_Int32 _gyro_id[INS_MAX_INSTANCES];
|
|
|
|
// accelerometer scaling and offsets
|
|
AP_Vector3f _accel_scale[INS_MAX_INSTANCES];
|
|
AP_Vector3f _accel_offset[INS_MAX_INSTANCES];
|
|
AP_Vector3f _gyro_offset[INS_MAX_INSTANCES];
|
|
|
|
// accelerometer position offset in body frame
|
|
AP_Vector3f _accel_pos[INS_MAX_INSTANCES];
|
|
|
|
// accelerometer max absolute offsets to be used for calibration
|
|
float _accel_max_abs_offsets[INS_MAX_INSTANCES];
|
|
|
|
// accelerometer and gyro raw sample rate in units of Hz
|
|
float _accel_raw_sample_rates[INS_MAX_INSTANCES];
|
|
float _gyro_raw_sample_rates[INS_MAX_INSTANCES];
|
|
|
|
// how many sensors samples per notify to the backend
|
|
uint8_t _accel_over_sampling[INS_MAX_INSTANCES];
|
|
uint8_t _gyro_over_sampling[INS_MAX_INSTANCES];
|
|
|
|
// last sample time in microseconds. Use for deltaT calculations
|
|
// on non-FIFO sensors
|
|
uint64_t _accel_last_sample_us[INS_MAX_INSTANCES];
|
|
uint64_t _gyro_last_sample_us[INS_MAX_INSTANCES];
|
|
|
|
// sample times for checking real sensor rate for FIFO sensors
|
|
uint16_t _sample_accel_count[INS_MAX_INSTANCES];
|
|
uint32_t _sample_accel_start_us[INS_MAX_INSTANCES];
|
|
uint16_t _sample_gyro_count[INS_MAX_INSTANCES];
|
|
uint32_t _sample_gyro_start_us[INS_MAX_INSTANCES];
|
|
|
|
// temperatures for an instance if available
|
|
float _temperature[INS_MAX_INSTANCES];
|
|
|
|
// filtering frequency (0 means default)
|
|
AP_Int16 _accel_filter_cutoff;
|
|
AP_Int16 _gyro_filter_cutoff;
|
|
AP_Int8 _gyro_cal_timing;
|
|
|
|
// use for attitude, velocity, position estimates
|
|
AP_Int8 _use[INS_MAX_INSTANCES];
|
|
|
|
// control enable of fast sampling
|
|
AP_Int8 _fast_sampling_mask;
|
|
|
|
// control enable of detected sensors
|
|
AP_Int8 _enable_mask;
|
|
|
|
// board orientation from AHRS
|
|
enum Rotation _board_orientation;
|
|
Matrix3f* _custom_rotation;
|
|
|
|
// per-sensor orientation to allow for board type defaults at runtime
|
|
enum Rotation _gyro_orientation[INS_MAX_INSTANCES];
|
|
enum Rotation _accel_orientation[INS_MAX_INSTANCES];
|
|
|
|
// calibrated_ok/id_ok flags
|
|
bool _gyro_cal_ok[INS_MAX_INSTANCES];
|
|
bool _accel_id_ok[INS_MAX_INSTANCES];
|
|
|
|
// primary accel and gyro
|
|
uint8_t _primary_gyro;
|
|
uint8_t _primary_accel;
|
|
|
|
// mask of accels and gyros which we will be actively using
|
|
// and this should wait for in wait_for_sample()
|
|
uint8_t _gyro_wait_mask;
|
|
uint8_t _accel_wait_mask;
|
|
|
|
// bitmask bit which indicates if we should log raw accel and gyro data
|
|
uint32_t _log_raw_bit;
|
|
|
|
// has wait_for_sample() found a sample?
|
|
bool _have_sample:1;
|
|
|
|
// are we in HIL mode?
|
|
bool _hil_mode:1;
|
|
|
|
// are gyros or accels currently being calibrated
|
|
bool _calibrating:1;
|
|
|
|
bool _backends_detected:1;
|
|
|
|
// the delta time in seconds for the last sample
|
|
float _delta_time;
|
|
|
|
// last time a wait_for_sample() returned a sample
|
|
uint32_t _last_sample_usec;
|
|
|
|
// target time for next wait_for_sample() return
|
|
uint32_t _next_sample_usec;
|
|
|
|
// time between samples in microseconds
|
|
uint32_t _sample_period_usec;
|
|
|
|
// last time update() completed
|
|
uint32_t _last_update_usec;
|
|
|
|
// health of gyros and accels
|
|
bool _gyro_healthy[INS_MAX_INSTANCES];
|
|
bool _accel_healthy[INS_MAX_INSTANCES];
|
|
|
|
uint32_t _accel_error_count[INS_MAX_INSTANCES];
|
|
uint32_t _gyro_error_count[INS_MAX_INSTANCES];
|
|
|
|
// vibration and clipping
|
|
uint32_t _accel_clip_count[INS_MAX_INSTANCES];
|
|
LowPassFilterVector3f _accel_vibe_floor_filter[INS_VIBRATION_CHECK_INSTANCES];
|
|
LowPassFilterVector3f _accel_vibe_filter[INS_VIBRATION_CHECK_INSTANCES];
|
|
|
|
// peak hold detector state for primary accel
|
|
struct PeakHoldState {
|
|
float accel_peak_hold_neg_x;
|
|
uint32_t accel_peak_hold_neg_x_age;
|
|
} _peak_hold_state;
|
|
|
|
// 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;
|
|
|
|
static AP_InertialSensor *_singleton;
|
|
AP_AccelCal* _acal;
|
|
|
|
AccelCalibrator *_accel_calibrator;
|
|
|
|
//save accelerometer bias and scale factors
|
|
void _acal_save_calibrations() override;
|
|
void _acal_event_failure() override;
|
|
|
|
// Returns AccelCalibrator objects pointer for specified acceleromter
|
|
AccelCalibrator* _acal_get_calibrator(uint8_t i) override { return i<get_accel_count()?&(_accel_calibrator[i]):nullptr; }
|
|
|
|
float _trim_pitch;
|
|
float _trim_roll;
|
|
bool _new_trim;
|
|
|
|
bool _accel_cal_requires_reboot;
|
|
|
|
// sensor error count at startup (used to ignore errors within 2 seconds of startup)
|
|
uint32_t _accel_startup_error_count[INS_MAX_INSTANCES];
|
|
uint32_t _gyro_startup_error_count[INS_MAX_INSTANCES];
|
|
bool _startup_error_counts_set;
|
|
uint32_t _startup_ms;
|
|
|
|
uint8_t imu_kill_mask;
|
|
};
|
|
|
|
namespace AP {
|
|
AP_InertialSensor &ins();
|
|
};
|