Line data Source code
1 : #pragma once
2 :
3 : #include <AP_HAL/AP_HAL_Boards.h>
4 :
5 : #ifndef INS_AUX_INSTANCES
6 : #define INS_AUX_INSTANCES 0
7 : #endif
8 :
9 : #ifndef INS_MAX_INSTANCES
10 : #define INS_MAX_INSTANCES (3+INS_AUX_INSTANCES)
11 : #endif
12 :
13 : #if INS_MAX_INSTANCES < 3 && INS_AUX_INSTANCES > 0
14 : #error "INS_AUX_INSTANCES must be zero if INS_MAX_INSTANCES is less than 3"
15 : #endif
16 :
17 : #if INS_MAX_INSTANCES > 3 && INS_AUX_INSTANCES == 0
18 : #error "INS_AUX_INSTANCES must be non-zero if INS_MAX_INSTANCES is greater than 3"
19 : #endif
20 :
21 : #define INS_MAX_BACKENDS 2*INS_MAX_INSTANCES
22 : #define INS_MAX_NOTCHES 12
23 : #ifndef INS_VIBRATION_CHECK_INSTANCES
24 : #if HAL_MEM_CLASS >= HAL_MEM_CLASS_300
25 : #define INS_VIBRATION_CHECK_INSTANCES INS_MAX_INSTANCES
26 : #else
27 : #define INS_VIBRATION_CHECK_INSTANCES 1
28 : #endif
29 : #endif
30 : #define XYZ_AXIS_COUNT 3
31 : // The maximum we need to store is gyro-rate / loop-rate, worst case ArduCopter with BMI088 is 2000/400
32 : #define INS_MAX_GYRO_WINDOW_SAMPLES 8
33 :
34 : #define DEFAULT_IMU_LOG_BAT_MASK 0
35 :
36 : #include "AP_InertialSensor_config.h"
37 :
38 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
39 : #ifndef HAL_INS_NUM_HARMONIC_NOTCH_FILTERS
40 : #if HAL_PROGRAM_SIZE_LIMIT_KB > 1024
41 : # define HAL_INS_NUM_HARMONIC_NOTCH_FILTERS 3
42 : #else
43 : # define HAL_INS_NUM_HARMONIC_NOTCH_FILTERS 2
44 : #endif
45 : #endif
46 : #endif
47 :
48 : // time for the estimated gyro rates to converge
49 : #ifndef HAL_INS_CONVERGANCE_MS
50 : #define HAL_INS_CONVERGANCE_MS 30000
51 : #endif
52 :
53 : // Gyro and Accelerometer calibration criteria
54 : #define AP_INERTIAL_SENSOR_ACCEL_VIBE_FLOOR_FILT_HZ 5.0f // accel vibration floor filter hz
55 : #define AP_INERTIAL_SENSOR_ACCEL_VIBE_FILT_HZ 2.0f // accel vibration filter hz
56 : #define AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS 500 // peak-hold detector timeout
57 :
58 : #include <AP_HAL/AP_HAL_Boards.h>
59 :
60 : #include <stdint.h>
61 :
62 : #include <AP_AccelCal/AP_AccelCal.h>
63 : #include <AP_HAL/utility/RingBuffer.h>
64 : #include <AP_Math/AP_Math.h>
65 : #include <AP_ExternalAHRS/AP_ExternalAHRS.h>
66 : #include <Filter/LowPassFilter.h>
67 : #include <Filter/HarmonicNotchFilter.h>
68 : #include <AP_SerialManager/AP_SerialManager_config.h>
69 : #include "AP_InertialSensor_Params.h"
70 : #include "AP_InertialSensor_tempcal.h"
71 :
72 : #ifndef AP_SIM_INS_ENABLED
73 : #define AP_SIM_INS_ENABLED AP_SIM_ENABLED
74 : #endif
75 :
76 : #ifndef AP_SIM_INS_FILE_ENABLED
77 : #define AP_SIM_INS_FILE_ENABLED AP_SIM_ENABLED
78 : #endif
79 :
80 : class AP_InertialSensor_Backend;
81 : class AuxiliaryBus;
82 : class AP_AHRS;
83 : class FastRateBuffer;
84 :
85 : /*
86 : forward declare AP_Logger class. We can't include logger.h
87 : because of mutual dependencies
88 : */
89 : class AP_Logger;
90 :
91 : /* AP_InertialSensor is an abstraction for gyro and accel measurements
92 : * which are correctly aligned to the body axes and scaled to SI units.
93 : *
94 : * Gauss-Newton accel calibration routines borrowed from Rolfe Schmidt
95 : * blog post describing the method: http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
96 : * original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
97 : */
98 : class AP_InertialSensor : AP_AccelCal_Client
99 : {
100 : friend class AP_InertialSensor_Backend;
101 : friend class FastRateBuffer;
102 :
103 : public:
104 : AP_InertialSensor();
105 :
106 : /* Do not allow copies */
107 : CLASS_NO_COPY(AP_InertialSensor);
108 :
109 : static AP_InertialSensor *get_singleton();
110 :
111 : enum Gyro_Calibration_Timing {
112 : GYRO_CAL_NEVER = 0,
113 : GYRO_CAL_STARTUP_ONLY = 1
114 : };
115 :
116 : /// Perform startup initialisation.
117 : ///
118 : /// Called to initialise the state of the IMU.
119 : ///
120 : /// Gyros will be calibrated unless INS_GYRO_CAL is zero
121 : ///
122 : /// @param style The initialisation startup style.
123 : ///
124 : __INITFUNC__ void init(uint16_t sample_rate_hz);
125 :
126 : // get accel/gyro instance numbers that a backend will get when they register
127 : bool get_accel_instance(uint8_t &instance) const;
128 : bool get_gyro_instance(uint8_t &instance) const;
129 :
130 : /// Register a new gyro/accel driver, allocating an instance
131 : /// number
132 : bool register_gyro(uint8_t &instance, uint16_t raw_sample_rate_hz, uint32_t id);
133 : bool register_accel(uint8_t &instance, uint16_t raw_sample_rate_hz, uint32_t id);
134 :
135 : // a function called by the main thread at the main loop rate:
136 : void periodic();
137 :
138 : /// calibrating - returns true if the gyros or accels are currently being calibrated
139 : bool calibrating() const;
140 :
141 : /// calibrating - returns true if a temperature calibration is running
142 : bool temperature_cal_running() const;
143 :
144 : /// Perform cold-start initialisation for just the gyros.
145 : ///
146 : /// @note This should not be called unless ::init has previously
147 : /// been called, as ::init may perform other work
148 : ///
149 : void init_gyro(void);
150 :
151 : // get startup messages to output to the GCS
152 : bool get_output_banner(uint8_t instance_id, char* banner, uint8_t banner_len);
153 :
154 : /// Fetch the current gyro values
155 : ///
156 : /// @returns vector of rotational rates in radians/sec
157 : ///
158 229588803 : const Vector3f &get_gyro(uint8_t i) const { return _gyro[i]; }
159 84570008 : const Vector3f &get_gyro(void) const { return get_gyro(_first_usable_gyro); }
160 :
161 : // set gyro offsets in radians/sec
162 : const Vector3f &get_gyro_offsets(uint8_t i) const { return _gyro_offset(i); }
163 : const Vector3f &get_gyro_offsets(void) const { return get_gyro_offsets(_first_usable_gyro); }
164 :
165 : //get delta angle if available
166 : bool get_delta_angle(uint8_t i, Vector3f &delta_angle, float &delta_angle_dt) const;
167 41686716 : bool get_delta_angle(Vector3f &delta_angle, float &delta_angle_dt) const {
168 41686716 : return get_delta_angle(_first_usable_gyro, delta_angle, delta_angle_dt);
169 : }
170 :
171 : //get delta velocity if available
172 : bool get_delta_velocity(uint8_t i, Vector3f &delta_velocity, float &delta_velocity_dt) const;
173 47918 : bool get_delta_velocity(Vector3f &delta_velocity, float &delta_velocity_dt) const {
174 47918 : return get_delta_velocity(_first_usable_accel, delta_velocity, delta_velocity_dt);
175 : }
176 :
177 : /// Fetch the current accelerometer values
178 : ///
179 : /// @returns vector of current accelerations in m/s/s
180 : ///
181 199771948 : const Vector3f &get_accel(uint8_t i) const { return _accel[i]; }
182 92800162 : const Vector3f &get_accel(void) const { return get_accel(_first_usable_accel); }
183 :
184 : // multi-device interface
185 147934773 : bool get_gyro_health(uint8_t instance) const { return (instance<_gyro_count) ? _gyro_healthy[instance] : false; }
186 758815 : bool get_gyro_health(void) const { return get_gyro_health(_first_usable_gyro); }
187 : bool get_gyro_health_all(void) const;
188 : bool gyros_consistent(uint8_t threshold) const;
189 65569551 : uint8_t get_gyro_count(void) const { return MIN(INS_MAX_INSTANCES, _gyro_count); }
190 2580509 : bool gyro_calibrated_ok(uint8_t instance) const { return _gyro_cal_ok[instance]; }
191 : bool gyro_calibrated_ok_all() const;
192 : bool use_gyro(uint8_t instance) const;
193 : Gyro_Calibration_Timing gyro_calibration_timing();
194 :
195 239378691 : bool get_accel_health(uint8_t instance) const { return (instance<_accel_count) ? _accel_healthy[instance] : false; }
196 758815 : bool get_accel_health(void) const { return get_accel_health(_first_usable_accel); }
197 : bool get_accel_health_all(void) const;
198 : bool accels_consistent(float accel_error_threshold) const;
199 200586132 : uint8_t get_accel_count(void) const { return MIN(INS_MAX_INSTANCES, _accel_count); }
200 : bool accel_calibrated_ok_all() const;
201 : bool use_accel(uint8_t instance) const;
202 :
203 : // get observed sensor rates, including any internal sampling multiplier
204 20832976 : uint16_t get_gyro_rate_hz(uint8_t instance) const { return uint16_t(_gyro_raw_sample_rates[instance] * _gyro_over_sampling[instance]); }
205 20832976 : uint16_t get_accel_rate_hz(uint8_t instance) const { return uint16_t(_accel_raw_sample_rates[instance] * _accel_over_sampling[instance]); }
206 :
207 : // validate backend sample rates
208 : bool pre_arm_check_gyro_backend_rate_hz(char* fail_msg, uint16_t fail_msg_len) const;
209 :
210 : // FFT support access
211 : #if HAL_GYROFFT_ENABLED
212 31554 : const Vector3f& get_gyro_for_fft(void) const { return _gyro_for_fft[_primary]; }
213 502270225 : FloatBuffer& get_raw_gyro_window(uint8_t instance, uint8_t axis) { return _gyro_window[instance][axis]; }
214 502270225 : FloatBuffer& get_raw_gyro_window(uint8_t axis) { return get_raw_gyro_window(_primary, axis); }
215 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
216 : bool has_fft_notch() const;
217 : #endif
218 : #endif
219 579405 : uint16_t get_raw_gyro_rate_hz(uint8_t instance) const { return _gyro_raw_sample_rates[_first_usable_gyro]; }
220 579405 : uint16_t get_raw_gyro_rate_hz() const { return get_raw_gyro_rate_hz(_first_usable_gyro); }
221 : bool set_gyro_window_size(uint16_t size);
222 : // get accel offsets in m/s/s
223 : const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset(i); }
224 : const Vector3f &get_accel_offsets(void) const { return get_accel_offsets(_first_usable_accel); }
225 :
226 : // get accel scale
227 : const Vector3f &get_accel_scale(uint8_t i) const { return _accel_scale(i); }
228 : const Vector3f &get_accel_scale(void) const { return get_accel_scale(_first_usable_accel); }
229 :
230 : // return a 3D vector defining the position offset of the IMU accelerometer in metres relative to the body frame origin
231 123595814 : const Vector3f &get_imu_pos_offset(uint8_t instance) const {
232 123595814 : return _accel_pos(instance);
233 : }
234 : const Vector3f &get_imu_pos_offset(void) const {
235 : return _accel_pos(_first_usable_accel);
236 : }
237 :
238 : // return the gyro bias limit (rad/s) for this IMU instance.
239 : // The EKF clamps its gyro bias state to this value.
240 : float get_gyro_bias_limit_rads(uint8_t instance) const;
241 :
242 : // return the initial gyro bias 1-sigma uncertainty (deg/s) for this
243 : // IMU instance, used to seed the EKF's gyro bias covariance.
244 : float get_gyro_bias_init_dps(uint8_t instance) const;
245 :
246 : // return the temperature if supported. Zero is returned if no
247 : // temperature is available
248 567639076 : float get_temperature(uint8_t instance) const { return _temperature[instance]; }
249 :
250 : /* get_delta_time returns the time period in seconds
251 : * overwhich the sensor data was collected
252 : */
253 41686717 : float get_delta_time() const { return MIN(_delta_time, _loop_delta_t_max); }
254 :
255 : // return the maximum gyro drift rate in radians/s/s. This
256 : // depends on what gyro chips are being used
257 25788 : float get_gyro_drift_rate(void) const { return radians(0.5f/60); }
258 :
259 : // update gyro and accel values from accumulated samples
260 : void update(void) __RAMFUNC__;
261 :
262 : // wait for a sample to be available
263 : void wait_for_sample(void) __RAMFUNC__;
264 :
265 : // class level parameters
266 : static const struct AP_Param::GroupInfo var_info[];
267 : #if INS_AUX_INSTANCES
268 : AP_InertialSensor_Params params[INS_AUX_INSTANCES];
269 : #endif
270 :
271 : // set overall board orientation
272 54325 : void set_board_orientation(enum Rotation orientation) {
273 54325 : _board_orientation = orientation;
274 54325 : }
275 :
276 : // return the selected loop rate at which samples are made avilable
277 41156288 : uint16_t get_loop_rate_hz(void) const { return _loop_rate; }
278 :
279 : // return the main loop delta_t in seconds
280 123688019 : float get_loop_delta_t(void) const { return _loop_delta_t; }
281 :
282 758815 : bool healthy(void) const { return get_gyro_health() && get_accel_health(); }
283 :
284 207326397 : uint8_t get_first_usable_accel(void) const { return _first_usable_accel; }
285 207326397 : uint8_t get_first_usable_gyro(void) const { return _first_usable_gyro; }
286 :
287 : // get the gyro filter rate in Hz
288 83 : uint16_t get_gyro_filter_hz(void) const { return _gyro_filter_cutoff; }
289 :
290 : // get the accel filter rate in Hz
291 : uint16_t get_accel_filter_hz(void) const { return _accel_filter_cutoff; }
292 :
293 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
294 : // setup the notch for throttle based tracking
295 : bool setup_throttle_gyro_harmonic_notch(float center_freq_hz, float lower_freq_hz, float ref, uint8_t harmonics);
296 :
297 : // write out harmonic notch log messages
298 : void write_notch_log_messages() const;
299 : #endif
300 :
301 : // indicate which bit in LOG_BITMASK indicates raw logging enabled
302 2055 : void set_log_raw_bit(uint32_t log_raw_bit) { _log_raw_bit = log_raw_bit; }
303 :
304 : // Logging Functions
305 : void Write_IMU() const;
306 : void Write_Vibration() const;
307 :
308 : // calculate vibration levels and check for accelerometer clipping (called by a backends)
309 : void calc_vibration_and_clipping(uint8_t instance, const Vector3f &accel, float dt);
310 :
311 : // retrieve latest calculated vibration levels
312 1553651 : Vector3f get_vibration_levels() const { return get_vibration_levels(_first_usable_accel); }
313 : Vector3f get_vibration_levels(uint8_t instance) const;
314 :
315 : // retrieve and clear accelerometer clipping count
316 : uint32_t get_accel_clip_count(uint8_t instance) const;
317 :
318 : // check for vibration movement. True when all axis show nearly zero movement
319 : bool is_still();
320 :
321 0 : AuxiliaryBus *get_auxiliary_bus(int16_t backend_id) { return get_auxiliary_bus(backend_id, 0); }
322 : AuxiliaryBus *get_auxiliary_bus(int16_t backend_id, uint8_t instance);
323 :
324 : void detect_backends(void);
325 : void update_backends();
326 :
327 : // accel peak hold detector
328 : void set_accel_peak_hold(uint8_t instance, const Vector3f &accel);
329 0 : float get_accel_peak_hold_neg_x() const { return _peak_hold_state.accel_peak_hold_neg_x; }
330 :
331 : //Returns accel calibrator interface object pointer
332 7456 : AP_AccelCal* get_acal() const { return _acal; }
333 :
334 : // Returns body fixed accelerometer level data averaged during accel calibration's first step
335 : bool get_fixed_mount_accel_cal_sample(uint8_t sample_num, Vector3f& ret) const;
336 :
337 : // Returns primary accelerometer level data averaged during accel calibration's first step
338 : bool get_first_usable_accel_cal_sample_avg(uint8_t sample_num, Vector3f& ret) const;
339 :
340 : // Returns newly calculated trim values if calculated
341 : bool get_new_trim(Vector3f &trim_rad);
342 :
343 : // notify IMUs of the new primary
344 : void set_primary(uint8_t instance);
345 :
346 : #if HAL_INS_ACCELCAL_ENABLED
347 : // initialise and register accel calibrator
348 : // called during the startup of accel cal
349 : void acal_init();
350 :
351 : // update accel calibrator
352 : void acal_update();
353 : #endif
354 :
355 : #if HAL_GCS_ENABLED
356 : bool calibrate_gyros();
357 :
358 : MAV_RESULT calibrate_trim();
359 :
360 : // simple accel calibration
361 : MAV_RESULT simple_accel_cal();
362 : private:
363 : uint32_t last_accel_cal_ms;
364 : public:
365 : #endif
366 :
367 64813 : bool accel_cal_requires_reboot() const { return _accel_cal_requires_reboot; }
368 :
369 : // return time in microseconds of last update() call
370 166918840 : uint32_t get_last_update_usec(void) const { return _last_update_usec; }
371 :
372 : // for killing an IMU for testing purposes
373 : void kill_imu(uint8_t imu_idx, bool kill_it);
374 :
375 : #if AP_SERIALMANAGER_IMUOUT_ENABLED
376 : // optional UART for sending IMU data to an external process
377 : void set_imu_out_uart(AP_HAL::UARTDriver *uart);
378 : void send_uart_data(void);
379 :
380 : struct {
381 : uint16_t counter;
382 : AP_HAL::UARTDriver *imu_out_uart;
383 : } uart;
384 : #endif // AP_SERIALMANAGER_IMUOUT_ENABLED
385 :
386 : enum IMU_SENSOR_TYPE {
387 : IMU_SENSOR_TYPE_ACCEL = 0,
388 : IMU_SENSOR_TYPE_GYRO = 1,
389 : };
390 :
391 : #if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
392 : class BatchSampler {
393 : public:
394 2098 : BatchSampler(const AP_InertialSensor &imu) :
395 2098 : type(IMU_SENSOR_TYPE_ACCEL),
396 2098 : _imu(imu) {
397 2098 : AP_Param::setup_object_defaults(this, var_info);
398 2098 : };
399 :
400 : void init();
401 : void sample(uint8_t instance, IMU_SENSOR_TYPE _type, uint64_t sample_us, const Vector3f &sample) __RAMFUNC__;
402 :
403 : // a function called by the main thread at the main loop rate:
404 : void periodic();
405 :
406 2600657042 : bool doing_sensor_rate_logging() const { return _doing_sensor_rate_logging; }
407 545430875 : bool doing_post_filter_logging() const {
408 4462518 : return (_doing_post_filter_logging && (post_filter || !_doing_sensor_rate_logging))
409 549893393 : || (_doing_pre_post_filter_logging && post_filter);
410 : }
411 :
412 : // Getters for arming check
413 11865 : bool is_initialised() const { return initialised; }
414 45832 : bool enabled() const { return _sensor_mask > 0; }
415 :
416 : // class level parameters
417 : static const struct AP_Param::GroupInfo var_info[];
418 :
419 :
420 : // Parameters
421 : AP_Int16 _required_count;
422 : uint16_t _real_required_count;
423 : AP_Int8 _sensor_mask;
424 : AP_Int8 _batch_options_mask;
425 :
426 : // Parameters controlling pushing data to AP_Logger:
427 : // Each DF message is ~ 108 bytes in size, so we use about 1kB/s of
428 : // logging bandwidth with a 100ms interval. If we are taking
429 : // 1024 samples then we need to send 32 packets, so it will
430 : // take ~3 seconds to push a complete batch to the log. If
431 : // you are running a on an FMU with three IMUs then you
432 : // will loop back around to the first sensor after about
433 : // twenty seconds.
434 : AP_Int16 samples_per_msg;
435 : AP_Int8 push_interval_ms;
436 :
437 : // end Parameters
438 :
439 : private:
440 :
441 : enum batch_opt_t {
442 : BATCH_OPT_SENSOR_RATE = (1<<0),
443 : BATCH_OPT_POST_FILTER = (1<<1),
444 : BATCH_OPT_PRE_POST_FILTER = (1<<2),
445 : };
446 :
447 : void rotate_to_next_sensor();
448 : void update_doing_sensor_rate_logging();
449 :
450 : bool should_log(uint8_t instance, IMU_SENSOR_TYPE type) __RAMFUNC__;
451 : void push_data_to_log();
452 :
453 : // Logging functions
454 : bool Write_ISBH(const float sample_rate_hz) const;
455 : bool Write_ISBD() const;
456 :
457 39384 : bool has_option(batch_opt_t option) const { return _batch_options_mask & uint16_t(option); }
458 :
459 : uint64_t measurement_started_us;
460 :
461 : bool initialised;
462 : bool isbh_sent;
463 : bool _doing_sensor_rate_logging;
464 : bool _doing_post_filter_logging;
465 : bool _doing_pre_post_filter_logging;
466 : uint8_t instance; // instance we are sending data for
467 : bool post_filter; // whether we are sending post-filter data
468 : AP_InertialSensor::IMU_SENSOR_TYPE type;
469 : uint16_t isb_seqnum;
470 : int16_t *data_x;
471 : int16_t *data_y;
472 : int16_t *data_z;
473 : uint16_t data_write_offset; // units: samples
474 : uint16_t data_read_offset; // units: samples
475 : uint32_t last_sent_ms;
476 :
477 : // all samples are multiplied by this
478 : uint16_t multiplier; // initialised as part of init()
479 :
480 : const AP_InertialSensor &_imu;
481 : };
482 : BatchSampler batchsampler{*this};
483 : #endif
484 :
485 : #if AP_EXTERNAL_AHRS_ENABLED
486 : // handle external AHRS data
487 : void handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt);
488 : #endif
489 :
490 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
491 : /*
492 : get a string representation of parameters that should be made
493 : persistent across changes of firmware type
494 : */
495 : void get_persistent_params(ExpandingString &str) const;
496 : #endif
497 :
498 : // force save of current calibration as valid
499 : void force_save_calibration(void);
500 :
501 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
502 : // structure per harmonic notch filter. This is public to allow for
503 : // easy iteration
504 : class HarmonicNotch {
505 : public:
506 : HarmonicNotchFilterParams params;
507 : HarmonicNotchFilterVector3f filter[INS_MAX_INSTANCES];
508 :
509 : uint8_t num_dynamic_notches;
510 :
511 : // the current center frequency for the notch
512 : float calculated_notch_freq_hz[INS_MAX_NOTCHES];
513 : uint8_t num_calculated_notch_frequencies;
514 :
515 : // runtime update of notch parameters
516 : void update_params(uint8_t instance, bool converging, float gyro_rate);
517 :
518 : // Update the harmonic notch frequencies
519 : void update_freq_hz(float scaled_freq);
520 : void update_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]);
521 :
522 : // enable/disable the notch
523 44424 : void set_inactive(bool _inactive) {
524 44424 : inactive = _inactive;
525 44424 : }
526 :
527 4309615 : bool is_inactive(void) const {
528 4309615 : return inactive;
529 : }
530 :
531 : private:
532 : // support for updating harmonic filter at runtime
533 : float last_center_freq_hz[INS_MAX_INSTANCES];
534 : float last_bandwidth_hz[INS_MAX_INSTANCES];
535 : float last_attenuation_dB[INS_MAX_INSTANCES];
536 : bool inactive;
537 : } harmonic_notches[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS];
538 : #endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
539 :
540 : private:
541 : // load backend drivers
542 : bool _add_backend(AP_InertialSensor_Backend *backend);
543 : void _start_backends();
544 : AP_InertialSensor_Backend *_find_backend(int16_t backend_id, uint8_t instance);
545 :
546 : // gyro initialisation
547 : void _init_gyro();
548 :
549 : // Calibration routines borrowed from Rolfe Schmidt
550 : // blog post describing the method: http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
551 : // original sketch available at http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
552 :
553 : bool _calculate_trim(const Vector3f &accel_sample, Vector3f &trim_rad);
554 :
555 : // save gyro calibration values to eeprom
556 : void _save_gyro_calibration();
557 :
558 : // Logging function
559 : void Write_IMU_instance(const uint64_t time_us, const uint8_t imu_instance) const;
560 :
561 : // look up the backend that owns the given gyro instance, or nullptr
562 : const AP_InertialSensor_Backend *_find_gyro_backend(uint8_t instance) const;
563 :
564 : // backend objects
565 : AP_InertialSensor_Backend *_backends[INS_MAX_BACKENDS];
566 :
567 : // number of gyros and accel drivers. Note that most backends
568 : // provide both accel and gyro data, so will increment both
569 : // counters on initialisation
570 : uint8_t _gyro_count;
571 : uint8_t _accel_count;
572 : uint8_t _backend_count;
573 :
574 : // the selected loop rate at which samples are made available
575 : uint16_t _loop_rate;
576 : float _loop_delta_t;
577 : float _loop_delta_t_max;
578 :
579 : // Most recent accelerometer reading
580 : Vector3f _accel[INS_MAX_INSTANCES];
581 : Vector3f _delta_velocity[INS_MAX_INSTANCES];
582 : float _delta_velocity_dt[INS_MAX_INSTANCES];
583 : bool _delta_velocity_valid[INS_MAX_INSTANCES];
584 : // delta velocity accumulator
585 : Vector3f _delta_velocity_acc[INS_MAX_INSTANCES];
586 : // time accumulator for delta velocity accumulator
587 : float _delta_velocity_acc_dt[INS_MAX_INSTANCES];
588 :
589 : // Low Pass filters for gyro and accel
590 : LowPassFilter2pVector3f _accel_filter[INS_MAX_INSTANCES];
591 : LowPassFilter2pVector3f _gyro_filter[INS_MAX_INSTANCES];
592 : Vector3f _accel_filtered[INS_MAX_INSTANCES];
593 : Vector3f _gyro_filtered[INS_MAX_INSTANCES];
594 : #if HAL_GYROFFT_ENABLED
595 : // Thread-safe public version of _last_raw_gyro
596 : Vector3f _gyro_for_fft[INS_MAX_INSTANCES];
597 : Vector3f _last_gyro_for_fft[INS_MAX_INSTANCES];
598 : FloatBuffer _gyro_window[INS_MAX_INSTANCES][XYZ_AXIS_COUNT];
599 : uint16_t _gyro_window_size;
600 : // capture a gyro window after the filters
601 : LowPassFilter2pVector3f _post_filter_gyro_filter[INS_MAX_INSTANCES];
602 : bool _post_filter_fft;
603 : uint8_t _fft_window_phase;
604 : #endif
605 : bool _new_accel_data[INS_MAX_INSTANCES];
606 : bool _new_gyro_data[INS_MAX_INSTANCES];
607 :
608 : // Most recent gyro reading
609 : Vector3f _gyro[INS_MAX_INSTANCES];
610 : Vector3f _delta_angle[INS_MAX_INSTANCES];
611 : float _delta_angle_dt[INS_MAX_INSTANCES];
612 : bool _delta_angle_valid[INS_MAX_INSTANCES];
613 : // time accumulator for delta angle accumulator
614 : float _delta_angle_acc_dt[INS_MAX_INSTANCES];
615 : Vector3f _delta_angle_acc[INS_MAX_INSTANCES];
616 : Vector3f _last_delta_angle[INS_MAX_INSTANCES];
617 : Vector3f _last_raw_gyro[INS_MAX_INSTANCES];
618 :
619 : // bitmask indicating if a sensor is doing sensor-rate sampling:
620 : uint8_t _accel_sensor_rate_sampling_enabled;
621 : uint8_t _gyro_sensor_rate_sampling_enabled;
622 :
623 : // multipliers for data supplied via sensor-rate logging:
624 : uint16_t _accel_raw_sampling_multiplier[INS_MAX_INSTANCES];
625 : uint16_t _gyro_raw_sampling_multiplier[INS_MAX_INSTANCES];
626 :
627 : // IDs to uniquely identify each sensor: shall remain
628 : // the same across reboots
629 : AP_Int32 _accel_id_old_param[INS_MAX_INSTANCES-INS_AUX_INSTANCES];
630 : AP_Int32 _gyro_id_old_param[INS_MAX_INSTANCES-INS_AUX_INSTANCES];
631 :
632 : // accelerometer scaling and offsets
633 : AP_Vector3f _accel_scale_old_param[INS_MAX_INSTANCES-INS_AUX_INSTANCES];
634 : AP_Vector3f _accel_offset_old_param[INS_MAX_INSTANCES-INS_AUX_INSTANCES];
635 : AP_Vector3f _gyro_offset_old_param[INS_MAX_INSTANCES-INS_AUX_INSTANCES];
636 :
637 : // accelerometer position offset in body frame
638 : AP_Vector3f _accel_pos_old_param[INS_MAX_INSTANCES-INS_AUX_INSTANCES];
639 :
640 : // Use Accessor methods to access above variables
641 : #if INS_AUX_INSTANCES
642 : #define INS_PARAM_WRAPPER(var) \
643 : inline decltype(var##_old_param[0])& var(uint8_t i) { \
644 : if (i<(INS_MAX_INSTANCES-INS_AUX_INSTANCES)) { \
645 : return var##_old_param[i]; \
646 : } else { \
647 : return params[i-(INS_MAX_INSTANCES-INS_AUX_INSTANCES)].var; \
648 : } \
649 : } \
650 : inline decltype(var##_old_param[0])& var(uint8_t i) const { \
651 : return const_cast<AP_InertialSensor*>(this)->var(i); \
652 : }
653 : #else
654 : #define INS_PARAM_WRAPPER(var) \
655 : inline decltype(var##_old_param[0])& var(uint8_t i) { \
656 : return var##_old_param[i]; \
657 : } \
658 : inline decltype(var##_old_param[0])& var(uint8_t i) const { \
659 : return const_cast<AP_InertialSensor*>(this)->var(i); \
660 : }
661 : #endif
662 :
663 : // Accessor methods for old parameters
664 2059436 : INS_PARAM_WRAPPER(_accel_id);
665 2380962 : INS_PARAM_WRAPPER(_gyro_id);
666 281982727 : INS_PARAM_WRAPPER(_accel_scale);
667 282122870 : INS_PARAM_WRAPPER(_accel_offset);
668 268253928 : INS_PARAM_WRAPPER(_gyro_offset);
669 247191628 : INS_PARAM_WRAPPER(_accel_pos);
670 :
671 : // accelerometer and gyro raw sample rate in units of Hz
672 : float _accel_raw_sample_rates[INS_MAX_INSTANCES];
673 : float _gyro_raw_sample_rates[INS_MAX_INSTANCES];
674 :
675 : // how many sensors samples per notify to the backend
676 : uint8_t _accel_over_sampling[INS_MAX_INSTANCES];
677 : uint8_t _gyro_over_sampling[INS_MAX_INSTANCES];
678 :
679 : // last sample time in microseconds. Use for deltaT calculations
680 : // on non-FIFO sensors
681 : uint64_t _accel_last_sample_us[INS_MAX_INSTANCES];
682 : uint64_t _gyro_last_sample_us[INS_MAX_INSTANCES];
683 :
684 : // sample times for checking real sensor rate for FIFO sensors
685 : uint16_t _sample_accel_count[INS_MAX_INSTANCES];
686 : uint32_t _sample_accel_start_us[INS_MAX_INSTANCES];
687 : uint16_t _sample_gyro_count[INS_MAX_INSTANCES];
688 : uint32_t _sample_gyro_start_us[INS_MAX_INSTANCES];
689 :
690 : // temperatures for an instance if available
691 : float _temperature[INS_MAX_INSTANCES];
692 :
693 : // filtering frequency (0 means default)
694 : AP_Int16 _accel_filter_cutoff;
695 : AP_Int16 _gyro_filter_cutoff;
696 : AP_Int8 _gyro_cal_timing;
697 :
698 : // use for attitude, velocity, position estimates
699 : AP_Int8 _use_old_param[INS_MAX_INSTANCES - INS_AUX_INSTANCES];
700 759662112 : INS_PARAM_WRAPPER(_use);
701 :
702 : // control enable of fast sampling
703 : AP_Int8 _fast_sampling_mask;
704 :
705 : // control enable of fast sampling
706 : AP_Int8 _fast_sampling_rate;
707 :
708 : // control enable of detected sensors
709 : AP_Int8 _enable_mask;
710 :
711 : // board orientation from AHRS
712 : enum Rotation _board_orientation;
713 :
714 : // per-sensor orientation to allow for board type defaults at runtime
715 : enum Rotation _gyro_orientation[INS_MAX_INSTANCES];
716 : enum Rotation _accel_orientation[INS_MAX_INSTANCES];
717 :
718 : // calibrated_ok/id_ok flags
719 : bool _gyro_cal_ok[INS_MAX_INSTANCES];
720 : bool _accel_id_ok[INS_MAX_INSTANCES];
721 :
722 : // first usable gyro and accel
723 : uint8_t _first_usable_gyro;
724 : uint8_t _first_usable_accel;
725 :
726 : // primary instance
727 : uint8_t _primary;
728 :
729 : // mask of accels and gyros which we will be actively using
730 : // and this should wait for in wait_for_sample()
731 : uint8_t _gyro_wait_mask;
732 : uint8_t _accel_wait_mask;
733 :
734 : // bitmask bit which indicates if we should log raw accel and gyro data
735 : uint32_t _log_raw_bit;
736 :
737 : // has wait_for_sample() found a sample?
738 : bool _have_sample:1;
739 :
740 : bool _backends_detected:1;
741 :
742 : // are gyros or accels currently being calibrated
743 : bool _calibrating_accel;
744 : bool _calibrating_gyro;
745 : bool _trimming_accel;
746 :
747 : // the delta time in seconds for the last sample
748 : float _delta_time;
749 :
750 : // last time a wait_for_sample() returned a sample
751 : uint32_t _last_sample_usec;
752 :
753 : // target time for next wait_for_sample() return
754 : uint32_t _next_sample_usec;
755 :
756 : // time between samples in microseconds
757 : uint32_t _sample_period_usec;
758 :
759 : // last time update() completed
760 : uint32_t _last_update_usec;
761 :
762 : // health of gyros and accels
763 : bool _gyro_healthy[INS_MAX_INSTANCES];
764 : bool _accel_healthy[INS_MAX_INSTANCES];
765 :
766 : uint32_t _accel_error_count[INS_MAX_INSTANCES];
767 : uint32_t _gyro_error_count[INS_MAX_INSTANCES];
768 :
769 : // vibration and clipping
770 : uint32_t _accel_clip_count[INS_MAX_INSTANCES];
771 : LowPassFilterVector3f _accel_vibe_floor_filter[INS_VIBRATION_CHECK_INSTANCES];
772 : LowPassFilterVector3f _accel_vibe_filter[INS_VIBRATION_CHECK_INSTANCES];
773 :
774 : // peak hold detector state for primary accel
775 : struct PeakHoldState {
776 : float accel_peak_hold_neg_x;
777 : uint32_t accel_peak_hold_neg_x_age;
778 : } _peak_hold_state;
779 :
780 : // threshold for detecting stillness
781 : AP_Float _still_threshold;
782 :
783 : // Trim options
784 : AP_Int8 _acc_body_aligned;
785 : AP_Int8 _trim_option;
786 :
787 : static AP_InertialSensor *_singleton;
788 : AP_AccelCal* _acal;
789 :
790 : AccelCalibrator *_accel_calibrator;
791 :
792 : //save accelerometer bias and scale factors
793 : void _acal_save_calibrations() override;
794 : void _acal_event_failure() override;
795 :
796 : // Returns AccelCalibrator objects pointer for specified acceleromter
797 13142 : AccelCalibrator* _acal_get_calibrator(uint8_t i) override { return i<get_accel_count()?&(_accel_calibrator[i]):nullptr; }
798 :
799 : Vector3f _trim_rad;
800 : bool _new_trim;
801 :
802 : bool _accel_cal_requires_reboot;
803 :
804 : // sensor error count at startup (used to ignore errors within 2 seconds of startup)
805 : uint32_t _accel_startup_error_count[INS_MAX_INSTANCES];
806 : uint32_t _gyro_startup_error_count[INS_MAX_INSTANCES];
807 : bool _startup_error_counts_set;
808 : uint32_t _startup_ms;
809 :
810 : #if AP_INERTIALSENSOR_KILL_IMU_ENABLED
811 : uint8_t imu_kill_mask;
812 : #endif
813 :
814 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
815 : public:
816 : // instance number for logging
817 : #if INS_AUX_INSTANCES
818 : uint8_t tcal_instance(const AP_InertialSensor_TCal &tc) const {
819 : for (uint8_t i=0; i<INS_MAX_INSTANCES - INS_AUX_INSTANCES; i++) {
820 : if (&tc == &tcal_old_param[i]) {
821 : return i;
822 : }
823 : }
824 : for (uint8_t i=0; i<INS_AUX_INSTANCES; i++) {
825 : if (&tc == ¶ms[i].tcal) {
826 : return i + INS_MAX_INSTANCES;
827 : }
828 : }
829 : return 0;
830 : }
831 : #else
832 310 : uint8_t tcal_instance(const AP_InertialSensor_TCal &tc) const {
833 310 : return &tc - &tcal(0);
834 : }
835 : #endif
836 : private:
837 : AP_InertialSensor_TCal tcal_old_param[INS_MAX_INSTANCES - INS_AUX_INSTANCES];
838 :
839 : enum class TCalOptions : uint8_t {
840 : PERSIST_TEMP_CAL = (1U<<0),
841 : PERSIST_ACCEL_CAL = (1U<<1),
842 : };
843 :
844 : // temperature that last calibration was run at
845 : AP_Float caltemp_accel_old_param[INS_MAX_INSTANCES - INS_AUX_INSTANCES];
846 : AP_Float caltemp_gyro_old_param[INS_MAX_INSTANCES - INS_AUX_INSTANCES];
847 :
848 275561233 : INS_PARAM_WRAPPER(caltemp_accel);
849 268251176 : INS_PARAM_WRAPPER(caltemp_gyro);
850 544868612 : INS_PARAM_WRAPPER(tcal);
851 :
852 : AP_Int32 tcal_options;
853 : bool tcal_learning;
854 : #endif
855 :
856 : // Raw logging options bitmask and parameter
857 : enum class RAW_LOGGING_OPTION {
858 : PRIMARY_GYRO_ONLY = (1U<<0),
859 : ALL_GYROS = (1U<<1),
860 : POST_FILTER = (1U<<2),
861 : PRE_AND_POST_FILTER = (1U<<3),
862 : };
863 : AP_Int16 raw_logging_options;
864 539379140 : bool raw_logging_option_set(RAW_LOGGING_OPTION option) const {
865 539379140 : return (raw_logging_options.get() & int32_t(option)) != 0;
866 : }
867 : // if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
868 : // Support for the fast rate thread in copter
869 : FastRateBuffer* fast_rate_buffer;
870 : bool fast_rate_buffer_enabled;
871 :
872 : public:
873 : // enable the fast rate buffer and start pushing samples to it
874 : void enable_fast_rate_buffer();
875 : // disable the fast rate buffer and stop pushing samples to it
876 : void disable_fast_rate_buffer();
877 : // get the next available gyro sample from the fast rate buffer
878 : bool get_next_gyro_sample(Vector3f& gyro);
879 : // get the number of available gyro samples in the fast rate buffer
880 : uint32_t get_num_gyro_samples();
881 : // set the rate at which samples are collected, unused samples are dropped
882 : void set_rate_decimation(uint8_t rdec);
883 : // push a new gyro sample into the fast rate buffer
884 : bool push_next_gyro_sample(const Vector3f& gyro);
885 : // run the filter parmeter update code.
886 : void update_backend_filters();
887 : // are rate loop samples enabled for this instance?
888 : bool is_rate_loop_gyro_enabled(uint8_t instance) const;
889 : // is dynamic fifo enabled for this instance
890 : bool is_dynamic_fifo_enabled(uint8_t instance) const;
891 : // endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
892 :
893 : uint8_t get_first_onboard_imu_instance() const { return _first_onboard_imu_instance; }
894 : private:
895 : uint8_t _first_onboard_imu_instance;
896 : };
897 :
898 : namespace AP {
899 : AP_InertialSensor &ins();
900 : };
|
