APM:Libraries
AP_InertialSensor.h
Go to the documentation of this file.
1 #pragma once
2 
3 // Gyro and Accelerometer calibration criteria
4 #define AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE 4.0f
5 #define AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET 250.0f
6 #define AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS (15.5f*GRAVITY_MSS) // accelerometer values over 15.5G are recorded as a clipping error
7 #define AP_INERTIAL_SENSOR_ACCEL_VIBE_FLOOR_FILT_HZ 5.0f // accel vibration floor filter hz
8 #define AP_INERTIAL_SENSOR_ACCEL_VIBE_FILT_HZ 2.0f // accel vibration filter hz
9 #define AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS 500 // peak-hold detector timeout
10 
15 #define INS_MAX_INSTANCES 3
16 #define INS_MAX_BACKENDS 6
17 #define INS_VIBRATION_CHECK_INSTANCES 2
18 
19 #define DEFAULT_IMU_LOG_BAT_MASK 0
20 
21 #include <stdint.h>
22 
24 #include <AP_HAL/AP_HAL.h>
25 #include <AP_Math/AP_Math.h>
26 #include <Filter/LowPassFilter2p.h>
27 #include <Filter/LowPassFilter.h>
28 #include <Filter/NotchFilter.h>
29 
31 class AuxiliaryBus;
32 class AP_AHRS;
33 
34 /*
35  forward declare DataFlash class. We can't include DataFlash.h
36  because of mutual dependencies
37  */
38 class DataFlash_Class;
39 
40 /* AP_InertialSensor is an abstraction for gyro and accel measurements
41  * which are correctly aligned to the body axes and scaled to SI units.
42  *
43  * Gauss-Newton accel calibration routines borrowed from Rolfe Schmidt
44  * blog post describing the method: http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
45  * original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
46  */
48 {
50 
51 public:
53 
54  /* Do not allow copies */
55  AP_InertialSensor(const AP_InertialSensor &other) = delete;
57 
59 
63  };
64 
73  void init(uint16_t sample_rate_hz);
74 
77  uint8_t register_gyro(uint16_t raw_sample_rate_hz, uint32_t id);
78  uint8_t register_accel(uint16_t raw_sample_rate_hz, uint32_t id);
79 
80  // a function called by the main thread at the main loop rate:
81  void periodic();
82 
83  bool calibrate_trim(float &trim_roll, float &trim_pitch);
84 
86  bool calibrating() const { return _calibrating; }
87 
93  void init_gyro(void);
94 
99  const Vector3f &get_gyro(uint8_t i) const { return _gyro[i]; }
100  const Vector3f &get_gyro(void) const { return get_gyro(_primary_gyro); }
101 
102  // set gyro offsets in radians/sec
103  const Vector3f &get_gyro_offsets(uint8_t i) const { return _gyro_offset[i]; }
104  const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_primary_gyro); }
105 
106  //get delta angle if available
107  bool get_delta_angle(uint8_t i, Vector3f &delta_angle) const;
108  bool get_delta_angle(Vector3f &delta_angle) const { return get_delta_angle(_primary_gyro, delta_angle); }
109 
110  float get_delta_angle_dt(uint8_t i) const;
112 
113  //get delta velocity if available
114  bool get_delta_velocity(uint8_t i, Vector3f &delta_velocity) const;
115  bool get_delta_velocity(Vector3f &delta_velocity) const { return get_delta_velocity(_primary_accel, delta_velocity); }
116 
117  float get_delta_velocity_dt(uint8_t i) const;
119 
124  const Vector3f &get_accel(uint8_t i) const { return _accel[i]; }
125  const Vector3f &get_accel(void) const { return get_accel(_primary_accel); }
126 
127  uint32_t get_gyro_error_count(uint8_t i) const { return _gyro_error_count[i]; }
128  uint32_t get_accel_error_count(uint8_t i) const { return _accel_error_count[i]; }
129 
130  // multi-device interface
131  bool get_gyro_health(uint8_t instance) const { return (instance<_gyro_count) ? _gyro_healthy[instance] : false; }
132  bool get_gyro_health(void) const { return get_gyro_health(_primary_gyro); }
133  bool get_gyro_health_all(void) const;
134  uint8_t get_gyro_count(void) const { return _gyro_count; }
135  bool gyro_calibrated_ok(uint8_t instance) const { return _gyro_cal_ok[instance]; }
136  bool gyro_calibrated_ok_all() const;
137  bool use_gyro(uint8_t instance) const;
139 
140  bool get_accel_health(uint8_t instance) const { return (instance<_accel_count) ? _accel_healthy[instance] : false; }
141  bool get_accel_health(void) const { return get_accel_health(_primary_accel); }
142  bool get_accel_health_all(void) const;
143  uint8_t get_accel_count(void) const { return _accel_count; }
144  bool accel_calibrated_ok_all() const;
145  bool use_accel(uint8_t instance) const;
146 
147  // get observed sensor rates, including any internal sampling multiplier
148  uint16_t get_gyro_rate_hz(uint8_t instance) const { return uint16_t(_gyro_raw_sample_rates[instance] * _gyro_over_sampling[instance]); }
149  uint16_t get_accel_rate_hz(uint8_t instance) const { return uint16_t(_accel_raw_sample_rates[instance] * _accel_over_sampling[instance]); }
150 
151  // get accel offsets in m/s/s
152  const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset[i]; }
154 
155  // get accel scale
156  const Vector3f &get_accel_scale(uint8_t i) const { return _accel_scale[i]; }
157  const Vector3f &get_accel_scale(void) const { return get_accel_scale(_primary_accel); }
158 
159  // return a 3D vector defining the position offset of the IMU accelerometer in metres relative to the body frame origin
160  const Vector3f &get_imu_pos_offset(uint8_t instance) const {
161  return _accel_pos[instance];
162  }
163  const Vector3f &get_imu_pos_offset(void) const {
164  return _accel_pos[_primary_accel];
165  }
166 
167  // return the temperature if supported. Zero is returned if no
168  // temperature is available
169  float get_temperature(uint8_t instance) const { return _temperature[instance]; }
170 
171  /* get_delta_time returns the time period in seconds
172  * overwhich the sensor data was collected
173  */
174  float get_delta_time() const { return MIN(_delta_time, _loop_delta_t_max); }
175 
176  // return the maximum gyro drift rate in radians/s/s. This
177  // depends on what gyro chips are being used
178  float get_gyro_drift_rate(void) const { return ToRad(0.5f/60); }
179 
180  // update gyro and accel values from accumulated samples
181  void update(void);
182 
183  // wait for a sample to be available
184  void wait_for_sample(void);
185 
186  // class level parameters
187  static const struct AP_Param::GroupInfo var_info[];
188 
189  // set overall board orientation
190  void set_board_orientation(enum Rotation orientation, Matrix3f* custom_rotation = nullptr) {
191  _board_orientation = orientation;
192  _custom_rotation = custom_rotation;
193  }
194 
195  // return the selected sample rate
196  uint16_t get_sample_rate(void) const { return _sample_rate; }
197 
198  // return the main loop delta_t in seconds
199  float get_loop_delta_t(void) const { return _loop_delta_t; }
200 
201  bool healthy(void) const { return get_gyro_health() && get_accel_health(); }
202 
203  uint8_t get_primary_accel(void) const { return _primary_accel; }
204  uint8_t get_primary_gyro(void) const { return _primary_gyro; }
205 
206  // enable HIL mode
207  void set_hil_mode(void) { _hil_mode = true; }
208 
209  // get the gyro filter rate in Hz
210  uint8_t get_gyro_filter_hz(void) const { return _gyro_filter_cutoff; }
211 
212  // get the accel filter rate in Hz
213  uint8_t get_accel_filter_hz(void) const { return _accel_filter_cutoff; }
214 
215  // indicate which bit in LOG_BITMASK indicates raw logging enabled
216  void set_log_raw_bit(uint32_t log_raw_bit) { _log_raw_bit = log_raw_bit; }
217 
218  // calculate vibration levels and check for accelerometer clipping (called by a backends)
219  void calc_vibration_and_clipping(uint8_t instance, const Vector3f &accel, float dt);
220 
221  // retrieve latest calculated vibration levels
223  Vector3f get_vibration_levels(uint8_t instance) const;
224 
225  // retrieve and clear accelerometer clipping count
226  uint32_t get_accel_clip_count(uint8_t instance) const;
227 
228  // check for vibration movement. True when all axis show nearly zero movement
229  bool is_still();
230 
231  /*
232  HIL set functions. The minimum for HIL is set_accel() and
233  set_gyro(). The others are option for higher fidelity log
234  playback
235  */
236  void set_accel(uint8_t instance, const Vector3f &accel);
237  void set_gyro(uint8_t instance, const Vector3f &gyro);
238  void set_delta_time(float delta_time);
239  void set_delta_velocity(uint8_t instance, float deltavt, const Vector3f &deltav);
240  void set_delta_angle(uint8_t instance, const Vector3f &deltaa, float deltaat);
241 
242  AuxiliaryBus *get_auxiliary_bus(int16_t backend_id) { return get_auxiliary_bus(backend_id, 0); }
243  AuxiliaryBus *get_auxiliary_bus(int16_t backend_id, uint8_t instance);
244 
245  void detect_backends(void);
246 
247  // accel peak hold detector
248  void set_accel_peak_hold(uint8_t instance, const Vector3f &accel);
250 
251  //Returns accel calibrator interface object pointer
252  AP_AccelCal* get_acal() const { return _acal; }
253 
254  // Returns body fixed accelerometer level data averaged during accel calibration's first step
255  bool get_fixed_mount_accel_cal_sample(uint8_t sample_num, Vector3f& ret) const;
256 
257  // Returns primary accelerometer level data averaged during accel calibration's first step
258  bool get_primary_accel_cal_sample_avg(uint8_t sample_num, Vector3f& ret) const;
259 
260  // Returns newly calculated trim values if calculated
261  bool get_new_trim(float& trim_roll, float &trim_pitch);
262 
263  // initialise and register accel calibrator
264  // called during the startup of accel cal
265  void acal_init();
266 
267  // update accel calibrator
268  void acal_update();
269 
270  // simple accel calibration
271  MAV_RESULT simple_accel_cal();
272 
274 
275  // return time in microseconds of last update() call
276  uint32_t get_last_update_usec(void) const { return _last_update_usec; }
277 
281  };
282 
283  class BatchSampler {
284  public:
287  _imu(imu) {
289  };
290 
291  void init();
292  void sample(uint8_t instance, IMU_SENSOR_TYPE _type, uint64_t sample_us, const Vector3f &sample);
293 
294  // a function called by the main thread at the main loop rate:
295  void periodic();
296 
298 
299  // class level parameters
300  static const struct AP_Param::GroupInfo var_info[];
301 
302  // Parameters
303  AP_Int16 _required_count;
304  AP_Int8 _sensor_mask;
306 
307  // Parameters controlling pushing data to DataFlash:
308  // Each DF message is ~ 108 bytes in size, so we use about 1kB/s of
309  // logging bandwidth with a 100ms interval. If we are taking
310  // 1024 samples then we need to send 32 packets, so it will
311  // take ~3 seconds to push a complete batch to the log. If
312  // you are running a on an FMU with three IMUs then you
313  // will loop back around to the first sensor after about
314  // twenty seconds.
315  AP_Int16 samples_per_msg;
317 
318  // end Parameters
319 
320  private:
321 
322  enum batch_opt_t {
324  };
325 
326  void rotate_to_next_sensor();
328 
329  bool should_log(uint8_t instance, IMU_SENSOR_TYPE type);
330  void push_data_to_log();
331 
333 
334  bool initialised : 1;
335  bool isbh_sent : 1;
337  uint8_t instance : 3; // instance we are sending data for
339  uint16_t isb_seqnum;
340  int16_t *data_x;
341  int16_t *data_y;
342  int16_t *data_z;
343  uint16_t data_write_offset; // units: samples
344  uint16_t data_read_offset; // units: samples
345  uint32_t last_sent_ms;
346 
347  // all samples are multiplied by this
348  uint16_t multiplier; // initialised as part of init()
349 
351  };
353 
354 private:
355  // load backend drivers
357  void _start_backends();
358  AP_InertialSensor_Backend *_find_backend(int16_t backend_id, uint8_t instance);
359 
360  // gyro initialisation
361  void _init_gyro();
362 
363  // Calibration routines borrowed from Rolfe Schmidt
364  // blog post describing the method: http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
365  // original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
366 
367  bool _calculate_trim(const Vector3f &accel_sample, float& trim_roll, float& trim_pitch);
368 
369  // save gyro calibration values to eeprom
370  void _save_gyro_calibration();
371 
372  // backend objects
374 
375  // number of gyros and accel drivers. Note that most backends
376  // provide both accel and gyro data, so will increment both
377  // counters on initialisation
378  uint8_t _gyro_count;
379  uint8_t _accel_count;
380  uint8_t _backend_count;
381 
382  // the selected sample rate
383  uint16_t _sample_rate;
386 
387  // Most recent accelerometer reading
392  // delta velocity accumulator
394  // time accumulator for delta velocity accumulator
396 
397  // Low Pass filters for gyro and accel
404 
405  // optional notch filter on gyro
407 
408  // Most recent gyro reading
413  // time accumulator for delta angle accumulator
418 
419  // bitmask indicating if a sensor is doing sensor-rate sampling:
422 
423  // multipliers for data supplied via sensor-rate logging:
426 
427  // product id
428  AP_Int16 _old_product_id;
429 
430  // IDs to uniquely identify each sensor: shall remain
431  // the same across reboots
434 
435  // accelerometer scaling and offsets
439 
440  // accelerometer position offset in body frame
442 
443  // accelerometer max absolute offsets to be used for calibration
445 
446  // accelerometer and gyro raw sample rate in units of Hz
449 
450  // how many sensors samples per notify to the backend
453 
454  // last sample time in microseconds. Use for deltaT calculations
455  // on non-FIFO sensors
458 
459  // sample times for checking real sensor rate for FIFO sensors
464 
465  // temperatures for an instance if available
467 
468  // filtering frequency (0 means default)
472 
473  // use for attitude, velocity, position estimates
475 
476  // control enable of fast sampling
478 
479  // control enable of detected sensors
480  AP_Int8 _enable_mask;
481 
482  // board orientation from AHRS
485 
486  // per-sensor orientation to allow for board type defaults at runtime
489 
490  // calibrated_ok/id_ok flags
493 
494  // primary accel and gyro
495  uint8_t _primary_gyro;
496  uint8_t _primary_accel;
497 
498  // bitmask bit which indicates if we should log raw accel and gyro data
499  uint32_t _log_raw_bit;
500 
501  // has wait_for_sample() found a sample?
502  bool _have_sample:1;
503 
504  // are we in HIL mode?
505  bool _hil_mode:1;
506 
507  // are gyros or accels currently being calibrated
508  bool _calibrating:1;
509 
511 
512  // the delta time in seconds for the last sample
513  float _delta_time;
514 
515  // last time a wait_for_sample() returned a sample
517 
518  // target time for next wait_for_sample() return
520 
521  // time between samples in microseconds
523 
524  // last time update() completed
526 
527  // health of gyros and accels
530 
533 
534  // vibration and clipping
538 
539  // peak hold detector state for primary accel
540  struct PeakHoldState {
544 
545  // threshold for detecting stillness
547 
548  /*
549  state for HIL support
550  */
551  struct {
552  float delta_time;
553  } _hil {};
554 
555  // Trim options
557  AP_Int8 _trim_option;
558 
561 
563 
564  //save accelerometer bias and scale factors
566  void _acal_event_failure();
567 
568  // Returns AccelCalibrator objects pointer for specified acceleromter
569  AccelCalibrator* _acal_get_calibrator(uint8_t i) { return i<get_accel_count()?&(_accel_calibrator[i]):nullptr; }
570 
571  float _trim_pitch;
572  float _trim_roll;
573  bool _new_trim;
574 
576 
577  // sensor error count at startup (used to ignore errors within 2 seconds of startup)
581  uint32_t _startup_ms;
582 };
583 
584 namespace AP {
586 };
bool accel_cal_requires_reboot() const
struct AP_InertialSensor::@119 _hil
uint32_t get_last_update_usec(void) const
void set_delta_velocity(uint8_t instance, float deltavt, const Vector3f &deltav)
float get_accel_peak_hold_neg_x() const
uint32_t _accel_startup_error_count[INS_MAX_INSTANCES]
void set_delta_time(float delta_time)
static const struct AP_Param::GroupInfo var_info[]
uint32_t _accel_error_count[INS_MAX_INSTANCES]
static AP_InertialSensor * _s_instance
const Vector3f & get_accel_scale(void) const
const Vector3f & get_accel(void) const
const Vector3f & get_imu_pos_offset(void) const
Vector3f _accel_filtered[INS_MAX_INSTANCES]
bool _gyro_healthy[INS_MAX_INSTANCES]
bool _new_gyro_data[INS_MAX_INSTANCES]
AP_InertialSensor::IMU_SENSOR_TYPE type
const Vector3f & get_gyro(uint8_t i) const
#define ToRad(x)
Definition: AP_Common.h:53
AP_Int8 _use[INS_MAX_INSTANCES]
uint8_t register_gyro(uint16_t raw_sample_rate_hz, uint32_t id)
bool healthy(void) const
uint8_t get_primary_gyro(void) const
float _accel_raw_sample_rates[INS_MAX_INSTANCES]
void set_delta_angle(uint8_t instance, const Vector3f &deltaa, float deltaat)
uint8_t get_accel_filter_hz(void) const
AP_InertialSensor_Backend * _backends[INS_MAX_BACKENDS]
float get_temperature(uint8_t instance) const
LowPassFilterVector3f _accel_vibe_floor_filter[INS_VIBRATION_CHECK_INSTANCES]
static const struct AP_Param::GroupInfo var_info[]
bool gyro_calibrated_ok(uint8_t instance) const
bool should_log(uint8_t instance, IMU_SENSOR_TYPE type)
bool _delta_angle_valid[INS_MAX_INSTANCES]
float _delta_velocity_dt[INS_MAX_INSTANCES]
bool get_delta_velocity(uint8_t i, Vector3f &delta_velocity) const
const Vector3f & get_gyro(void) const
float _delta_velocity_acc_dt[INS_MAX_INSTANCES]
bool _add_backend(AP_InertialSensor_Backend *backend)
bool use_accel(uint8_t instance) const
uint16_t _sample_accel_count[INS_MAX_INSTANCES]
Matrix3f * _custom_rotation
float _temperature[INS_MAX_INSTANCES]
Rotation
Definition: rotations.h:27
float _accel_max_abs_offsets[INS_MAX_INSTANCES]
#define MIN(a, b)
Definition: usb_conf.h:215
uint64_t _accel_last_sample_us[INS_MAX_INSTANCES]
uint16_t _accel_raw_sampling_multiplier[INS_MAX_INSTANCES]
uint64_t _gyro_last_sample_us[INS_MAX_INSTANCES]
uint32_t _sample_gyro_start_us[INS_MAX_INSTANCES]
Vector3f _delta_angle_acc[INS_MAX_INSTANCES]
void sample(uint8_t instance, IMU_SENSOR_TYPE _type, uint64_t sample_us, const Vector3f &sample)
static AP_InertialSensor ins
Definition: AHRS_Test.cpp:18
uint32_t _sample_accel_start_us[INS_MAX_INSTANCES]
const Vector3f & get_accel(uint8_t i) const
float _gyro_raw_sample_rates[INS_MAX_INSTANCES]
#define INS_VIBRATION_CHECK_INSTANCES
AccelCalibrator * _acal_get_calibrator(uint8_t i)
uint8_t get_primary_accel(void) const
bool get_delta_velocity(Vector3f &delta_velocity) const
bool _accel_id_ok[INS_MAX_INSTANCES]
uint16_t get_gyro_rate_hz(uint8_t instance) const
AP_Vector3f _accel_scale[INS_MAX_INSTANCES]
bool get_delta_angle(uint8_t i, Vector3f &delta_angle) const
Vector3f _delta_angle[INS_MAX_INSTANCES]
AP_InertialSensor_Backend * _find_backend(int16_t backend_id, uint8_t instance)
AP_Int32 _gyro_id[INS_MAX_INSTANCES]
bool _new_accel_data[INS_MAX_INSTANCES]
uint8_t register_accel(uint16_t raw_sample_rate_hz, uint32_t id)
LowPassFilter2pVector3f _gyro_filter[INS_MAX_INSTANCES]
uint8_t _gyro_over_sampling[INS_MAX_INSTANCES]
bool calibrating() const
calibrating - returns true if the gyros or accels are currently being calibrated
Definition: AP_AHRS.cpp:486
bool get_accel_health_all(void) const
bool _calculate_trim(const Vector3f &accel_sample, float &trim_roll, float &trim_pitch)
bool use_gyro(uint8_t instance) const
Vector3f _gyro_filtered[INS_MAX_INSTANCES]
#define f(i)
uint32_t get_accel_error_count(uint8_t i) const
uint32_t _gyro_error_count[INS_MAX_INSTANCES]
const Vector3f & get_gyro_offsets(void) const
uint8_t get_gyro_filter_hz(void) const
void set_accel(uint8_t instance, const Vector3f &accel)
void set_board_orientation(enum Rotation orientation, Matrix3f *custom_rotation=nullptr)
const Vector3f & get_gyro_offsets(uint8_t i) const
AP_Int32 _accel_id[INS_MAX_INSTANCES]
LowPassFilterVector3f _accel_vibe_filter[INS_VIBRATION_CHECK_INSTANCES]
float _delta_angle_acc_dt[INS_MAX_INSTANCES]
Vector3f _last_raw_gyro[INS_MAX_INSTANCES]
bool get_delta_angle(Vector3f &delta_angle) const
bool _gyro_cal_ok[INS_MAX_INSTANCES]
uint32_t get_accel_clip_count(uint8_t instance) const
bool _delta_velocity_valid[INS_MAX_INSTANCES]
const AP_InertialSensor & _imu
const Vector3f & get_imu_pos_offset(uint8_t instance) const
Vector3f _gyro[INS_MAX_INSTANCES]
AP_InertialSensor & operator=(const AP_InertialSensor &)=delete
uint16_t get_sample_rate(void) const
MAV_RESULT simple_accel_cal()
Vector3f _delta_velocity[INS_MAX_INSTANCES]
Gyro_Calibration_Timing gyro_calibration_timing()
void set_accel_peak_hold(uint8_t instance, const Vector3f &accel)
bool get_gyro_health(void) const
float get_delta_time() const
bool get_primary_accel_cal_sample_avg(uint8_t sample_num, Vector3f &ret) const
uint16_t _gyro_raw_sampling_multiplier[INS_MAX_INSTANCES]
bool calibrate_trim(float &trim_roll, float &trim_pitch)
bool get_accel_health(void) const
A class to implement a second order low pass filter.
enum Rotation _board_orientation
A class to implement a low pass filter without losing precision even for int types the downside being...
void set_gyro(uint8_t instance, const Vector3f &gyro)
uint16_t _sample_gyro_count[INS_MAX_INSTANCES]
uint8_t _accel_sensor_rate_sampling_enabled
void init(uint16_t sample_rate_hz)
BatchSampler batchsampler
struct AP_InertialSensor::PeakHoldState _peak_hold_state
AuxiliaryBus * get_auxiliary_bus(int16_t backend_id)
#define INS_MAX_BACKENDS
AP_AccelCal * get_acal() const
bool gyro_calibrated_ok_all() const
float _delta_angle_dt[INS_MAX_INSTANCES]
AP_Vector3f _gyro_offset[INS_MAX_INSTANCES]
bool get_new_trim(float &trim_roll, float &trim_pitch)
AP_Vector3f _accel_pos[INS_MAX_INSTANCES]
const Vector3f & get_accel_offsets(uint8_t i) const
Vector3f _delta_velocity_acc[INS_MAX_INSTANCES]
uint8_t get_gyro_count(void) const
void set_log_raw_bit(uint32_t log_raw_bit)
const Vector3f & get_accel_scale(uint8_t i) const
enum Rotation _accel_orientation[INS_MAX_INSTANCES]
bool get_gyro_health(uint8_t instance) const
const Vector3f & get_accel_offsets(void) const
void calc_vibration_and_clipping(uint8_t instance, const Vector3f &accel, float dt)
uint8_t _accel_over_sampling[INS_MAX_INSTANCES]
uint32_t _accel_clip_count[INS_MAX_INSTANCES]
bool _accel_healthy[INS_MAX_INSTANCES]
uint32_t get_gyro_error_count(uint8_t i) const
float get_delta_velocity_dt() const
#define INS_MAX_INSTANCES
AP_Vector3f _accel_offset[INS_MAX_INSTANCES]
static AP_InertialSensor * get_instance()
float get_delta_angle_dt() const
bool accel_calibrated_ok_all() const
float get_loop_delta_t(void) const
Vector3f get_vibration_levels() const
Vector3f _accel[INS_MAX_INSTANCES]
LowPassFilter2pVector3f _accel_filter[INS_MAX_INSTANCES]
NotchFilterVector3fParam _notch_filter
uint32_t _gyro_startup_error_count[INS_MAX_INSTANCES]
AccelCalibrator * _accel_calibrator
bool get_gyro_health_all(void) const
BatchSampler(const AP_InertialSensor &imu)
float get_gyro_drift_rate(void) const
enum Rotation _gyro_orientation[INS_MAX_INSTANCES]
bool get_fixed_mount_accel_cal_sample(uint8_t sample_num, Vector3f &ret) const
uint8_t get_accel_count(void) const
bool get_accel_health(uint8_t instance) const
static void setup_object_defaults(const void *object_pointer, const struct GroupInfo *group_info)
Definition: AP_Param.cpp:1214
uint16_t get_accel_rate_hz(uint8_t instance) const
uint8_t _gyro_sensor_rate_sampling_enabled
Vector3f _last_delta_angle[INS_MAX_INSTANCES]