Line data Source code
1 : #define AP_INLINE_VECTOR_OPS
2 :
3 : #include <AP_HAL/AP_HAL.h>
4 : #include <AP_AHRS/AP_AHRS.h>
5 : #include "AP_InertialSensor_rate_config.h"
6 : #include "AP_InertialSensor.h"
7 : #include "AP_InertialSensor_Backend.h"
8 : #include <AP_Logger/AP_Logger.h>
9 : #include <AP_BoardConfig/AP_BoardConfig.h>
10 : #if AP_MODULE_SUPPORTED
11 : #include <AP_Module/AP_Module.h>
12 : #endif
13 : #include <stdio.h>
14 :
15 : #ifndef AP_HEATER_IMU_INSTANCE
16 : #define AP_HEATER_IMU_INSTANCE 0
17 : #endif
18 :
19 : #define PRIMARY_UPDATE_TIMEOUT_US 200000UL // continue to notify the primary at 5Hz
20 :
21 : const extern AP_HAL::HAL& hal;
22 :
23 4183 : AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
24 4183 : _imu(imu)
25 : {
26 4183 : }
27 :
28 : /*
29 : notify of a FIFO reset so we don't use bad data to update observed sensor rate
30 : */
31 0 : void AP_InertialSensor_Backend::notify_accel_fifo_reset(uint8_t instance)
32 : {
33 0 : if (instance >= INS_MAX_INSTANCES) {
34 : // can be called from probe() before registration
35 0 : return;
36 : }
37 0 : _imu._sample_accel_count[instance] = 0;
38 0 : _imu._sample_accel_start_us[instance] = 0;
39 : }
40 :
41 : /*
42 : notify of a FIFO reset so we don't use bad data to update observed sensor rate
43 : */
44 0 : void AP_InertialSensor_Backend::notify_gyro_fifo_reset(uint8_t instance)
45 : {
46 0 : if (instance >= INS_MAX_INSTANCES) {
47 : // can be called from probe() before registration
48 0 : return;
49 : }
50 0 : _imu._sample_gyro_count[instance] = 0;
51 0 : _imu._sample_gyro_start_us[instance] = 0;
52 : }
53 :
54 : // set the amount of oversamping a accel is doing
55 0 : void AP_InertialSensor_Backend::_set_accel_oversampling(uint8_t instance, uint8_t n)
56 : {
57 0 : _imu._accel_over_sampling[instance] = n;
58 0 : }
59 :
60 : // set the amount of oversamping a gyro is doing
61 0 : void AP_InertialSensor_Backend::_set_gyro_oversampling(uint8_t instance, uint8_t n)
62 : {
63 0 : _imu._gyro_over_sampling[instance] = n;
64 0 : }
65 :
66 : /*
67 : while sensors are converging to get the true sample rate we re-init the notch filters.
68 : stop doing this if the user arms
69 : */
70 86763445 : bool AP_InertialSensor_Backend::sensors_converging() const
71 : {
72 86763445 : return AP_HAL::millis64() < HAL_INS_CONVERGANCE_MS && !hal.util->get_soft_armed();
73 : }
74 :
75 : /*
76 : update the sensor rate for FIFO sensors
77 :
78 : FIFO sensors produce samples at a fixed rate, but the clock in the
79 : sensor may vary slightly from the system clock. This slowly adjusts
80 : the rate to the observed rate
81 : */
82 545430903 : void AP_InertialSensor_Backend::_update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const
83 : {
84 545430903 : uint32_t now = AP_HAL::micros();
85 545430932 : if (start_us == 0) {
86 8366 : count = 0;
87 8366 : start_us = now;
88 : } else {
89 545422566 : count++;
90 545422566 : if (now - start_us > 1000000UL) {
91 609017 : float observed_rate_hz = count * 1.0e6f / (now - start_us);
92 : #if 0
93 : printf("IMU RATE: %.1f should be %.1f\n", observed_rate_hz, rate_hz);
94 : #endif
95 609017 : float filter_constant = 0.98f;
96 609017 : float upper_limit = 1.05f;
97 609017 : float lower_limit = 0.95f;
98 609017 : if (sensors_converging()) {
99 : // converge quickly for first 30s, then more slowly
100 111695 : filter_constant = 0.8f;
101 111695 : upper_limit = 2.0f;
102 111695 : lower_limit = 0.5f;
103 : }
104 609017 : observed_rate_hz = constrain_float(observed_rate_hz, rate_hz*lower_limit, rate_hz*upper_limit);
105 609017 : rate_hz = filter_constant * rate_hz + (1-filter_constant) * observed_rate_hz;
106 609017 : count = 0;
107 609017 : start_us = now;
108 : }
109 : }
110 545430932 : }
111 :
112 275741852 : void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vector3f &accel)
113 : {
114 : /*
115 : accel calibration is always done in sensor frame with this
116 : version of the code. That means we apply the rotation after the
117 : offsets and scaling.
118 : */
119 :
120 : // rotate for sensor orientation
121 275741852 : accel.rotate(_imu._accel_orientation[instance]);
122 :
123 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
124 275741839 : if (_imu.tcal_learning) {
125 381279 : _imu.tcal(instance).update_accel_learning(accel, _imu.get_temperature(instance));
126 : }
127 : #endif
128 :
129 275866579 : if (!_imu._calibrating_accel && (_imu._acal == nullptr
130 : #if HAL_INS_ACCELCAL_ENABLED
131 124796 : || !_imu._acal->running()
132 : #endif
133 : )) {
134 :
135 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
136 : // apply temperature corrections
137 275561183 : _imu.tcal(instance).correct_accel(_imu.get_temperature(instance), _imu.caltemp_accel(instance), accel);
138 : #endif
139 :
140 : // apply offsets
141 275561149 : accel -= _imu._accel_offset(instance);
142 :
143 :
144 : // apply scaling
145 275561163 : const Vector3f &accel_scale = _imu._accel_scale(instance).get();
146 275561124 : accel.x *= accel_scale.x;
147 275561124 : accel.y *= accel_scale.y;
148 275561124 : accel.z *= accel_scale.z;
149 : }
150 :
151 : // rotate to body frame
152 275741724 : accel.rotate(_imu._board_orientation);
153 275741715 : }
154 :
155 269689762 : void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro)
156 : {
157 : // rotate for sensor orientation
158 269689762 : gyro.rotate(_imu._gyro_orientation[instance]);
159 :
160 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
161 269689806 : if (_imu.tcal_learning) {
162 372903 : _imu.tcal(instance).update_gyro_learning(gyro, _imu.get_temperature(instance));
163 : }
164 : #endif
165 :
166 269689807 : if (!_imu._calibrating_gyro) {
167 :
168 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
169 : // apply temperature corrections
170 268244260 : _imu.tcal(instance).correct_gyro(_imu.get_temperature(instance), _imu.caltemp_gyro(instance), gyro);
171 : #endif
172 :
173 : // gyro calibration is always assumed to have been done in sensor frame
174 268244222 : gyro -= _imu._gyro_offset(instance);
175 :
176 : // calibration samples are wanted in board frame, so the vehicle
177 : // rotation is applied only outside it. Zeroing _board_orientation for
178 : // the duration instead would also strip it from the accel, which DCM
179 : // reads for its startup alignment.
180 268244223 : gyro.rotate(_imu._board_orientation);
181 : }
182 269689794 : }
183 :
184 : /*
185 : rotate gyro vector and add the gyro offset
186 : */
187 83766540 : void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &gyro) /* front end */
188 : {
189 83766540 : if (has_been_killed(instance)) {
190 0 : return;
191 : }
192 83766540 : _imu._gyro[instance] = gyro;
193 83766540 : _imu._gyro_healthy[instance] = true;
194 :
195 : // publish delta angle
196 83766540 : _imu._delta_angle[instance] = _imu._delta_angle_acc[instance];
197 83766540 : _imu._delta_angle_dt[instance] = _imu._delta_angle_acc_dt[instance];
198 83766540 : _imu._delta_angle_valid[instance] = true;
199 :
200 83766540 : _imu._delta_angle_acc[instance].zero();
201 83766540 : _imu._delta_angle_acc_dt[instance] = 0;
202 : }
203 :
204 :
205 273999258 : void AP_InertialSensor_Backend::save_gyro_window(const uint8_t instance, const Vector3f &gyro, uint8_t phase)
206 : {
207 : #if HAL_GYROFFT_ENABLED
208 : // capture gyro window for FFT analysis
209 273999258 : if (_imu._fft_window_phase == phase) {
210 269682737 : if (_imu._gyro_window_size > 0) {
211 3675978 : Vector3f scaled_gyro = gyro * _imu._gyro_raw_sampling_multiplier[instance];
212 : // LPF always must come last to remove high-frequency shot noise, but the FFT still
213 : // needs to see the same data so gets its own LPF at the tap point
214 3675978 : if (_imu._post_filter_fft) {
215 394783 : scaled_gyro = _imu._post_filter_gyro_filter[instance].apply(scaled_gyro);
216 : }
217 3675978 : _imu._gyro_window[instance][0].push(scaled_gyro.x);
218 3675978 : _imu._gyro_window[instance][1].push(scaled_gyro.y);
219 3675978 : _imu._gyro_window[instance][2].push(scaled_gyro.z);
220 3675978 : _imu._last_gyro_for_fft[instance] = scaled_gyro;
221 : } else {
222 266006759 : _imu._last_gyro_for_fft[instance] = gyro * _imu._gyro_raw_sampling_multiplier[instance];;
223 : }
224 : }
225 : #endif
226 273999316 : }
227 :
228 : /*
229 : apply harmonic notch and low pass gyro filters
230 : */
231 269689670 : void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const Vector3f &gyro)
232 : {
233 269689670 : uint8_t filter_phase = 0;
234 269689670 : save_gyro_window(instance, gyro, filter_phase++);
235 :
236 269689718 : Vector3f gyro_filtered = gyro;
237 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
238 : // apply the harmonic notch filters
239 1078758499 : for (auto ¬ch : _imu.harmonic_notches) {
240 809068705 : if (!notch.params.enabled()) {
241 804759166 : continue;
242 : }
243 4309611 : bool inactive = notch.is_inactive();
244 : // by default we only run the expensive notch filters on the
245 : // currently active IMU we reset the inactive notch filters so
246 : // that if we switch IMUs we're not left with old data
247 8619230 : if (!notch.params.hasOption(HarmonicNotchFilterParams::Options::EnableOnAllIMUs) &&
248 4309615 : instance != _imu._primary) {
249 1861604 : inactive = true;
250 : }
251 4309615 : if (inactive) {
252 : // while inactive we reset the filter so when it activates the first output
253 : // will be the first input sample
254 1997829 : notch.filter[instance].reset();
255 : } else {
256 2311786 : gyro_filtered = notch.filter[instance].apply(gyro_filtered);
257 : }
258 4309615 : save_gyro_window(instance, gyro_filtered, filter_phase++);
259 : }
260 : #endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
261 :
262 : // apply the low pass filter last to attenuate any notch induced noise
263 269689794 : gyro_filtered = _imu._gyro_filter[instance].apply(gyro_filtered);
264 :
265 : // if the filtering failed in any way then reset the filters and keep the old value
266 269689694 : if (gyro_filtered.is_nan() || gyro_filtered.is_inf()) {
267 0 : _imu._gyro_filter[instance].reset();
268 : #if HAL_GYROFFT_ENABLED
269 0 : _imu._post_filter_gyro_filter[instance].reset();
270 : #endif
271 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
272 0 : for (auto ¬ch : _imu.harmonic_notches) {
273 0 : notch.filter[instance].reset();
274 : }
275 : #endif
276 0 : gyro_filtered = _imu._gyro_filtered[instance];
277 : }
278 :
279 : #if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
280 135288617 : if (_imu.is_rate_loop_gyro_enabled(instance)) {
281 288225 : if (_imu.push_next_gyro_sample(gyro_filtered)) {
282 : // if we used the value, record it for publication to the front-end
283 288225 : _imu._gyro_filtered[instance] = gyro_filtered;
284 : }
285 : } else {
286 135000392 : _imu._gyro_filtered[instance] = gyro_filtered;
287 : }
288 : #else
289 134401128 : _imu._gyro_filtered[instance] = gyro_filtered;
290 : #endif
291 269689745 : }
292 :
293 269689713 : void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
294 : const Vector3f &gyro,
295 : uint64_t sample_us)
296 : {
297 269689713 : if (has_been_killed(instance)) {
298 0 : return;
299 : }
300 : float dt;
301 :
302 269689720 : _update_sensor_rate(_imu._sample_gyro_count[instance], _imu._sample_gyro_start_us[instance],
303 269689720 : _imu._gyro_raw_sample_rates[instance]);
304 :
305 269689729 : uint64_t last_sample_us = _imu._gyro_last_sample_us[instance];
306 :
307 : /*
308 : we have two classes of sensors. FIFO based sensors produce data
309 : at a very predictable overall rate, but the data comes in
310 : bunches, so we use the provided sample rate for deltaT. Non-FIFO
311 : sensors don't bunch up samples, but also tend to vary in actual
312 : rate, so we use the provided sample_us to get the deltaT. The
313 : difference between the two is whether sample_us is provided.
314 : */
315 269689729 : if (sample_us != 0 && _imu._gyro_last_sample_us[instance] != 0) {
316 269685592 : dt = (sample_us - _imu._gyro_last_sample_us[instance]) * 1.0e-6f;
317 269685592 : _imu._gyro_last_sample_us[instance] = sample_us;
318 : } else {
319 : // don't accept below 40Hz
320 4137 : if (_imu._gyro_raw_sample_rates[instance] < 40) {
321 : // Still record the timestamp so future samples with valid timestamps can work.
322 : // This breaks the bootstrap deadlock where we need samples to measure rate,
323 : // but reject samples due to low rate.
324 0 : if (sample_us != 0) {
325 0 : _imu._gyro_last_sample_us[instance] = sample_us;
326 : }
327 0 : return;
328 : }
329 :
330 4137 : dt = 1.0f / _imu._gyro_raw_sample_rates[instance];
331 4137 : _imu._gyro_last_sample_us[instance] = AP_HAL::micros64();
332 4183 : sample_us = _imu._gyro_last_sample_us[instance];
333 : }
334 :
335 : #if AP_MODULE_SUPPORTED
336 : // call gyro_sample hook if any
337 0 : AP_Module::call_hook_gyro_sample(instance, dt, gyro);
338 : #endif
339 :
340 : // push gyros if optical flow present
341 269689775 : if (hal.opticalflow) {
342 269689754 : hal.opticalflow->push_gyro(gyro.x, gyro.y, dt);
343 : }
344 :
345 : // compute delta angle
346 269689783 : Vector3f delta_angle = (gyro + _imu._last_raw_gyro[instance]) * 0.5f * dt;
347 :
348 : // compute coning correction
349 : // see page 26 of:
350 : // Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
351 : // Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
352 : // see also examples/coning.py
353 269689819 : Vector3f delta_coning = (_imu._delta_angle_acc[instance] +
354 269689818 : _imu._last_delta_angle[instance] * (1.0f / 6.0f));
355 269689815 : delta_coning = delta_coning % delta_angle;
356 269689770 : delta_coning *= 0.5f;
357 :
358 : {
359 269689778 : WITH_SEMAPHORE(_sem);
360 :
361 : // Check for unhealthy gap between samples.
362 : // Use sample_us (which may be synced from external source) for the comparison
363 : // to stay in the same time domain as last_sample_us.
364 269689763 : if (sample_us - last_sample_us > 100000U) {
365 : // zero accumulator if sensor was unhealthy for 0.1s
366 4181 : _imu._delta_angle_acc[instance].zero();
367 4181 : _imu._delta_angle_acc_dt[instance] = 0;
368 4181 : dt = 0;
369 4181 : delta_angle.zero();
370 : }
371 :
372 : // integrate delta angle accumulator
373 : // the angles and coning corrections are accumulated separately in the
374 : // referenced paper, but in simulation little difference was found between
375 : // integrating together and integrating separately (see examples/coning.py)
376 269689763 : _imu._delta_angle_acc[instance] += delta_angle + delta_coning;
377 269689745 : _imu._delta_angle_acc_dt[instance] += dt;
378 :
379 : // save previous delta angle for coning correction
380 269689745 : _imu._last_delta_angle[instance] = delta_angle;
381 269689745 : _imu._last_raw_gyro[instance] = gyro;
382 :
383 : // apply gyro filters and sample for FFT
384 269689745 : apply_gyro_filters(instance, gyro);
385 :
386 269689688 : _imu._new_gyro_data[instance] = true;
387 269689688 : }
388 :
389 : // 5us
390 269689811 : log_gyro_raw(instance, sample_us, gyro, _imu._gyro_filtered[instance]);
391 269689599 : update_primary();
392 : }
393 :
394 : /*
395 : handle a delta-angle sample from the backend. This assumes FIFO
396 : style sampling and the sample should not be rotated or corrected for
397 : offsets.
398 : This function should be used when the sensor driver can directly
399 : provide delta-angle values from the sensor.
400 : */
401 0 : void AP_InertialSensor_Backend::_notify_new_delta_angle(uint8_t instance, const Vector3f &dangle)
402 : {
403 0 : if (has_been_killed(instance)) {
404 0 : return;
405 : }
406 : float dt;
407 :
408 0 : _update_sensor_rate(_imu._sample_gyro_count[instance], _imu._sample_gyro_start_us[instance],
409 0 : _imu._gyro_raw_sample_rates[instance]);
410 :
411 0 : uint64_t last_sample_us = _imu._gyro_last_sample_us[instance];
412 :
413 : // don't accept below 40Hz
414 0 : if (_imu._gyro_raw_sample_rates[instance] < 40) {
415 0 : return;
416 : }
417 :
418 0 : dt = 1.0f / _imu._gyro_raw_sample_rates[instance];
419 0 : _imu._gyro_last_sample_us[instance] = AP_HAL::micros64();
420 0 : uint64_t sample_us = _imu._gyro_last_sample_us[instance];
421 :
422 0 : Vector3f gyro = dangle / dt;
423 :
424 0 : _rotate_and_correct_gyro(instance, gyro);
425 :
426 : #if AP_MODULE_SUPPORTED
427 : // call gyro_sample hook if any
428 0 : AP_Module::call_hook_gyro_sample(instance, dt, gyro);
429 : #endif
430 :
431 : // push gyros if optical flow present
432 0 : if (hal.opticalflow) {
433 0 : hal.opticalflow->push_gyro(gyro.x, gyro.y, dt);
434 : }
435 :
436 : // compute delta angle, including corrections
437 0 : Vector3f delta_angle = gyro * dt;
438 :
439 : // compute coning correction
440 : // see page 26 of:
441 : // Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
442 : // Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
443 : // see also examples/coning.py
444 0 : Vector3f delta_coning = (_imu._delta_angle_acc[instance] +
445 0 : _imu._last_delta_angle[instance] * (1.0f / 6.0f));
446 0 : delta_coning = delta_coning % delta_angle;
447 0 : delta_coning *= 0.5f;
448 :
449 : {
450 0 : WITH_SEMAPHORE(_sem);
451 0 : uint64_t now = AP_HAL::micros64();
452 :
453 0 : if (now - last_sample_us > 100000U) {
454 : // zero accumulator if sensor was unhealthy for 0.1s
455 0 : _imu._delta_angle_acc[instance].zero();
456 0 : _imu._delta_angle_acc_dt[instance] = 0;
457 0 : dt = 0;
458 0 : delta_angle.zero();
459 : }
460 :
461 : // integrate delta angle accumulator
462 : // the angles and coning corrections are accumulated separately in the
463 : // referenced paper, but in simulation little difference was found between
464 : // integrating together and integrating separately (see examples/coning.py)
465 0 : _imu._delta_angle_acc[instance] += delta_angle + delta_coning;
466 0 : _imu._delta_angle_acc_dt[instance] += dt;
467 :
468 : // save previous delta angle for coning correction
469 0 : _imu._last_delta_angle[instance] = delta_angle;
470 0 : _imu._last_raw_gyro[instance] = gyro;
471 :
472 : // apply gyro filters and sample for FFT
473 0 : apply_gyro_filters(instance, gyro);
474 :
475 0 : _imu._new_gyro_data[instance] = true;
476 0 : }
477 :
478 0 : log_gyro_raw(instance, sample_us, gyro, _imu._gyro_filtered[instance]);
479 0 : update_primary();
480 : }
481 :
482 269689681 : void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &raw_gyro, const Vector3f &filtered_gyro)
483 : {
484 : #if HAL_LOGGING_ENABLED
485 269689681 : AP_Logger *logger = AP_Logger::get_singleton();
486 269689664 : if (logger == nullptr) {
487 : // should not have been called
488 0 : return;
489 : }
490 :
491 : #if AP_AHRS_ENABLED
492 269689664 : const bool log_because_primary_gyro = _imu.raw_logging_option_set(AP_InertialSensor::RAW_LOGGING_OPTION::PRIMARY_GYRO_ONLY) && (instance == _imu._primary);
493 : #else
494 : const bool log_because_primary_gyro = false;
495 : #endif
496 :
497 539379307 : if (_imu.raw_logging_option_set(AP_InertialSensor::RAW_LOGGING_OPTION::ALL_GYROS) ||
498 539379301 : log_because_primary_gyro ||
499 269689623 : should_log_imu_raw()) {
500 :
501 0 : if (_imu.raw_logging_option_set(AP_InertialSensor::RAW_LOGGING_OPTION::PRE_AND_POST_FILTER)) {
502 : // Both pre and post, offset post instance as batch sampler does
503 0 : Write_GYR(instance, sample_us, raw_gyro);
504 0 : Write_GYR(instance + _imu._gyro_count, sample_us, filtered_gyro);
505 :
506 0 : } else if (_imu.raw_logging_option_set(AP_InertialSensor::RAW_LOGGING_OPTION::POST_FILTER)) {
507 : // Just post
508 0 : Write_GYR(instance, sample_us, filtered_gyro);
509 :
510 : } else {
511 : // Just pre
512 0 : Write_GYR(instance, sample_us, raw_gyro);
513 :
514 : }
515 : } else {
516 : #if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
517 269689634 : if (!_imu.batchsampler.doing_sensor_rate_logging()) {
518 269689612 : _imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_GYRO, sample_us,
519 269689591 : !_imu.batchsampler.doing_post_filter_logging() ? raw_gyro : filtered_gyro);
520 : }
521 : #endif
522 : }
523 : #endif
524 : }
525 :
526 : /*
527 : rotate accel vector, scale and add the accel offset
528 : */
529 83766540 : void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f &accel) /* front end */
530 : {
531 83766540 : if (has_been_killed(instance)) {
532 0 : return;
533 : }
534 83766540 : _imu._accel[instance] = accel;
535 83766540 : _imu._accel_healthy[instance] = true;
536 :
537 : // publish delta velocity
538 83766540 : _imu._delta_velocity[instance] = _imu._delta_velocity_acc[instance];
539 83766540 : _imu._delta_velocity_dt[instance] = _imu._delta_velocity_acc_dt[instance];
540 83766540 : _imu._delta_velocity_valid[instance] = true;
541 :
542 83766540 : _imu._delta_velocity_acc[instance].zero();
543 83766540 : _imu._delta_velocity_acc_dt[instance] = 0;
544 :
545 83766540 : if (_imu._accel_calibrator != nullptr && _imu._accel_calibrator[instance].get_status() == ACCEL_CAL_COLLECTING_SAMPLE) {
546 311 : Vector3f cal_sample = _imu._delta_velocity[instance];
547 :
548 : // remove rotation. Note that we don't need to remove offsets or scale factor as those
549 : // are not applied when calibrating
550 311 : cal_sample.rotate_inverse(_imu._board_orientation);
551 :
552 311 : _imu._accel_calibrator[instance].new_sample(cal_sample, _imu._delta_velocity_dt[instance]);
553 : }
554 : }
555 :
556 275741621 : void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
557 : const Vector3f &accel,
558 : uint64_t sample_us,
559 : bool fsync_set)
560 : {
561 275741621 : if (has_been_killed(instance)) {
562 0 : return;
563 : }
564 : float dt;
565 :
566 275741622 : _update_sensor_rate(_imu._sample_accel_count[instance], _imu._sample_accel_start_us[instance],
567 275741622 : _imu._accel_raw_sample_rates[instance]);
568 :
569 275741628 : uint64_t last_sample_us = _imu._accel_last_sample_us[instance];
570 :
571 : /*
572 : we have two classes of sensors. FIFO based sensors produce data
573 : at a very predictable overall rate, but the data comes in
574 : bunches, so we use the provided sample rate for deltaT. Non-FIFO
575 : sensors don't bunch up samples, but also tend to vary in actual
576 : rate, so we use the provided sample_us to get the deltaT. The
577 : difference between the two is whether sample_us is provided.
578 : */
579 275741628 : if (sample_us != 0 && _imu._accel_last_sample_us[instance] != 0) {
580 275737487 : dt = (sample_us - _imu._accel_last_sample_us[instance]) * 1.0e-6f;
581 275737487 : _imu._accel_last_sample_us[instance] = sample_us;
582 : } else {
583 : // don't accept below 40Hz
584 4141 : if (_imu._accel_raw_sample_rates[instance] < 40) {
585 : // Still record the timestamp so future samples with valid timestamps can work.
586 : // This breaks the bootstrap deadlock where we need samples to measure rate,
587 : // but reject samples due to low rate.
588 0 : if (sample_us != 0) {
589 0 : _imu._accel_last_sample_us[instance] = sample_us;
590 : }
591 0 : return;
592 : }
593 :
594 4141 : dt = 1.0f / _imu._accel_raw_sample_rates[instance];
595 4141 : _imu._accel_last_sample_us[instance] = AP_HAL::micros64();
596 4183 : sample_us = _imu._accel_last_sample_us[instance];
597 : }
598 :
599 : #if AP_MODULE_SUPPORTED
600 : // call accel_sample hook if any
601 0 : AP_Module::call_hook_accel_sample(instance, dt, accel, fsync_set);
602 : #endif
603 :
604 275741670 : _imu.calc_vibration_and_clipping(instance, accel, dt);
605 :
606 : {
607 275741647 : WITH_SEMAPHORE(_sem);
608 :
609 : // Check for unhealthy gap between samples.
610 : // Use sample_us (which may be synced from external source) for the comparison
611 : // to stay in the same time domain as last_sample_us.
612 275741715 : if (sample_us - last_sample_us > 100000U) {
613 : // zero accumulator if sensor was unhealthy for 0.1s
614 4181 : _imu._delta_velocity_acc[instance].zero();
615 4181 : _imu._delta_velocity_acc_dt[instance] = 0;
616 4181 : dt = 0;
617 : }
618 :
619 : // delta velocity
620 275741715 : _imu._delta_velocity_acc[instance] += accel * dt;
621 275741707 : _imu._delta_velocity_acc_dt[instance] += dt;
622 :
623 275741707 : _imu._accel_filtered[instance] = _imu._accel_filter[instance].apply(accel);
624 275741577 : if (_imu._accel_filtered[instance].is_nan() || _imu._accel_filtered[instance].is_inf()) {
625 0 : _imu._accel_filter[instance].reset();
626 : }
627 :
628 275741577 : _imu.set_accel_peak_hold(instance, _imu._accel_filtered[instance]);
629 :
630 275741519 : _imu._new_accel_data[instance] = true;
631 275741519 : }
632 :
633 : // 5us
634 : #if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
635 275741720 : if (!_imu.batchsampler.doing_post_filter_logging()) {
636 272504535 : log_accel_raw(instance, sample_us, accel);
637 : } else {
638 3237005 : log_accel_raw(instance, sample_us, _imu._accel_filtered[instance]);
639 : }
640 : #else
641 : // assume we're doing pre-filter logging:
642 : log_accel_raw(instance, sample_us, accel);
643 : #endif
644 : }
645 :
646 : /*
647 : handle a delta-velocity sample from the backend. This assumes FIFO style sampling and
648 : the sample should not be rotated or corrected for offsets
649 :
650 : This function should be used when the sensor driver can directly
651 : provide delta-velocity values from the sensor.
652 : */
653 0 : void AP_InertialSensor_Backend::_notify_new_delta_velocity(uint8_t instance, const Vector3f &dvel)
654 : {
655 0 : if (has_been_killed(instance)) {
656 0 : return;
657 : }
658 : float dt;
659 :
660 0 : _update_sensor_rate(_imu._sample_accel_count[instance], _imu._sample_accel_start_us[instance],
661 0 : _imu._accel_raw_sample_rates[instance]);
662 :
663 0 : uint64_t last_sample_us = _imu._accel_last_sample_us[instance];
664 :
665 : // don't accept below 40Hz
666 0 : if (_imu._accel_raw_sample_rates[instance] < 40) {
667 0 : return;
668 : }
669 :
670 0 : dt = 1.0f / _imu._accel_raw_sample_rates[instance];
671 0 : _imu._accel_last_sample_us[instance] = AP_HAL::micros64();
672 0 : uint64_t sample_us = _imu._accel_last_sample_us[instance];
673 :
674 0 : Vector3f accel = dvel / dt;
675 :
676 0 : _rotate_and_correct_accel(instance, accel);
677 :
678 : #if AP_MODULE_SUPPORTED
679 : // call accel_sample hook if any
680 0 : AP_Module::call_hook_accel_sample(instance, dt, accel, false);
681 : #endif
682 :
683 0 : _imu.calc_vibration_and_clipping(instance, accel, dt);
684 :
685 : {
686 0 : WITH_SEMAPHORE(_sem);
687 :
688 0 : uint64_t now = AP_HAL::micros64();
689 :
690 0 : if (now - last_sample_us > 100000U) {
691 : // zero accumulator if sensor was unhealthy for 0.1s
692 0 : _imu._delta_velocity_acc[instance].zero();
693 0 : _imu._delta_velocity_acc_dt[instance] = 0;
694 0 : dt = 0;
695 : }
696 :
697 : // delta velocity including corrections
698 0 : _imu._delta_velocity_acc[instance] += accel * dt;
699 0 : _imu._delta_velocity_acc_dt[instance] += dt;
700 :
701 0 : _imu._accel_filtered[instance] = _imu._accel_filter[instance].apply(accel);
702 0 : if (_imu._accel_filtered[instance].is_nan() || _imu._accel_filtered[instance].is_inf()) {
703 0 : _imu._accel_filter[instance].reset();
704 : }
705 :
706 0 : _imu.set_accel_peak_hold(instance, _imu._accel_filtered[instance]);
707 :
708 0 : _imu._new_accel_data[instance] = true;
709 0 : }
710 :
711 : #if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
712 0 : if (!_imu.batchsampler.doing_post_filter_logging()) {
713 0 : log_accel_raw(instance, sample_us, accel);
714 : } else {
715 0 : log_accel_raw(instance, sample_us, _imu._accel_filtered[instance]);
716 : }
717 : #else
718 : // assume we're doing pre-filter logging
719 : log_accel_raw(instance, sample_us, accel);
720 : #endif
721 : }
722 :
723 :
724 728655862 : void AP_InertialSensor_Backend::_notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &_accel)
725 : {
726 : #if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
727 728655862 : if (!_imu.batchsampler.doing_sensor_rate_logging()) {
728 728655862 : return;
729 : }
730 :
731 : // get batch sampling in correct orientation
732 0 : Vector3f accel = _accel;
733 0 : accel.rotate(_imu._accel_orientation[instance]);
734 :
735 0 : _imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_ACCEL, AP_HAL::micros64(), accel);
736 : #endif
737 : }
738 :
739 1326571246 : void AP_InertialSensor_Backend::_notify_new_gyro_sensor_rate_sample(uint8_t instance, const Vector3f &_gyro)
740 : {
741 : #if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
742 1326571246 : if (!_imu.batchsampler.doing_sensor_rate_logging()) {
743 1326571220 : return;
744 : }
745 :
746 : // get batch sampling in correct orientation
747 1 : Vector3f gyro = _gyro;
748 1 : gyro.rotate(_imu._gyro_orientation[instance]);
749 :
750 0 : _imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_GYRO, AP_HAL::micros64(), gyro);
751 : #endif
752 : }
753 :
754 275741515 : void AP_InertialSensor_Backend::log_accel_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &accel)
755 : {
756 : #if HAL_LOGGING_ENABLED
757 275741515 : AP_Logger *logger = AP_Logger::get_singleton();
758 275741503 : if (logger == nullptr) {
759 : // should not have been called
760 0 : return;
761 : }
762 275741503 : if (should_log_imu_raw()) {
763 0 : Write_ACC(instance, sample_us, accel);
764 : } else {
765 : #if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
766 275741520 : if (!_imu.batchsampler.doing_sensor_rate_logging()) {
767 275741524 : _imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_ACCEL, sample_us, accel);
768 : }
769 : #endif
770 : }
771 : #endif
772 : }
773 :
774 : // increment accelerometer error_count
775 0 : void AP_InertialSensor_Backend::_inc_accel_error_count(uint8_t instance)
776 : {
777 0 : _imu._accel_error_count[instance]++;
778 0 : }
779 :
780 : // increment gyro error_count
781 0 : void AP_InertialSensor_Backend::_inc_gyro_error_count(uint8_t instance)
782 : {
783 0 : _imu._gyro_error_count[instance]++;
784 0 : }
785 :
786 : /*
787 : publish a temperature value for an instance
788 : */
789 275741828 : void AP_InertialSensor_Backend::_publish_temperature(uint8_t instance, float temperature) /* front end */
790 : {
791 275741828 : if (instance >= INS_MAX_INSTANCES) {
792 : // registration failed, this backend owns no instance
793 0 : return;
794 : }
795 275741828 : if (has_been_killed(instance)) {
796 0 : return;
797 : }
798 275741811 : _imu._temperature[instance] = temperature;
799 :
800 : #if HAL_HAVE_IMU_HEATER
801 : /* give the temperature to the control loop in order to keep it constant.
802 : AP_HEATER_IMU_INSTANCE is offset by the occasional EAHRS which gets
803 : interjected first in the registration order. */
804 : const uint8_t offset = _imu.get_first_onboard_imu_instance();
805 : if (instance == (AP_HEATER_IMU_INSTANCE + offset)) {
806 : AP_BoardConfig *bc = AP::boardConfig();
807 : if (bc) {
808 : bc->set_imu_temp(temperature);
809 : }
810 : }
811 : #endif
812 : }
813 :
814 : /*
815 : common gyro update function for all backends
816 : */
817 83766951 : void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
818 : {
819 83766951 : WITH_SEMAPHORE(_sem);
820 :
821 83766951 : if (instance >= INS_MAX_INSTANCES) {
822 : // registration failed, this backend owns no instance
823 0 : return;
824 : }
825 :
826 83766951 : if (has_been_killed(instance)) {
827 0 : _imu._gyro_healthy[instance] = false;
828 0 : return;
829 : }
830 :
831 83766951 : if (_imu._new_gyro_data[instance]) {
832 83766540 : _publish_gyro(instance, _imu._gyro_filtered[instance]);
833 : #if HAL_GYROFFT_ENABLED
834 : // copy the gyro samples from the backend to the frontend window for FFTs sampling at less than IMU rate
835 83766540 : _imu._gyro_for_fft[instance] = _imu._last_gyro_for_fft[instance];
836 : #endif
837 83766540 : _imu._new_gyro_data[instance] = false;
838 : } else {
839 : // no fresh sample this cycle. _publish_gyro() sets the flag true, so
840 : // a healthy sensor never sees it transiently cleared
841 411 : _imu._gyro_healthy[instance] = false;
842 : }
843 :
844 83766951 : update_gyro_filters(instance);
845 83766951 : }
846 :
847 269689584 : void AP_InertialSensor_Backend::update_primary()
848 : {
849 : // timing changes need to be made in the bus thread in order to take effect which is
850 : // why they are actioned here. Currently the primary gyro and primary accel can never
851 : // be different for a particular IMU
852 269689584 : const bool is_new_primary = (gyro_instance == _imu._primary);
853 269689584 : uint32_t now_us = AP_HAL::micros();
854 539379135 : if (is_primary != is_new_primary
855 269689575 : || AP_HAL::timeout_expired(last_primary_update_us, now_us, PRIMARY_UPDATE_TIMEOUT_US)) {
856 1529148 : set_primary(is_new_primary);
857 1529148 : is_primary = is_new_primary;
858 1529148 : last_primary_update_us = now_us;
859 : }
860 269689560 : }
861 :
862 : /*
863 : propagate filter changes from front end to backend
864 : */
865 84055165 : void AP_InertialSensor_Backend::update_gyro_filters(uint8_t instance) /* front end */
866 : {
867 84055165 : if (instance >= INS_MAX_INSTANCES) {
868 : // registration failed, this backend owns no instance
869 0 : return;
870 : }
871 : // possibly update filter frequency
872 84055165 : const float gyro_rate = _gyro_raw_sample_rate(instance);
873 :
874 84055158 : if (_last_gyro_filter_hz != _gyro_filter_cutoff() || sensors_converging()) {
875 14827216 : _imu._gyro_filter[instance].set_cutoff_frequency(gyro_rate, _gyro_filter_cutoff());
876 : #if HAL_GYROFFT_ENABLED
877 14827215 : _imu._post_filter_gyro_filter[instance].set_cutoff_frequency(gyro_rate, _gyro_filter_cutoff());
878 : #endif
879 14827215 : _last_gyro_filter_hz = _gyro_filter_cutoff();
880 : }
881 :
882 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
883 336220491 : for (auto ¬ch : _imu.harmonic_notches) {
884 252165357 : if (notch.params.enabled()) {
885 2103537 : notch.update_params(instance, sensors_converging(), gyro_rate);
886 : }
887 : }
888 : #endif
889 : }
890 :
891 : /*
892 : common accel update function for all backends
893 : */
894 83766951 : void AP_InertialSensor_Backend::update_accel(uint8_t instance) /* front end */
895 : {
896 83766951 : WITH_SEMAPHORE(_sem);
897 :
898 83766951 : if (instance >= INS_MAX_INSTANCES) {
899 : // registration failed, this backend owns no instance
900 0 : return;
901 : }
902 :
903 83766951 : if (has_been_killed(instance)) {
904 0 : _imu._accel_healthy[instance] = false;
905 0 : return;
906 : }
907 83766951 : if (_imu._new_accel_data[instance]) {
908 83766540 : _publish_accel(instance, _imu._accel_filtered[instance]);
909 83766540 : _imu._new_accel_data[instance] = false;
910 : } else {
911 : // as for the gyro above
912 411 : _imu._accel_healthy[instance] = false;
913 : }
914 :
915 83766951 : update_accel_filters(instance);
916 83766951 : }
917 :
918 : /*
919 : propagate filter changes from front end to backend
920 : */
921 84055141 : void AP_InertialSensor_Backend::update_accel_filters(uint8_t instance) /* front end */
922 : {
923 84055141 : if (instance >= INS_MAX_INSTANCES) {
924 : // registration failed, this backend owns no instance
925 0 : return;
926 : }
927 : // possibly update filter frequency
928 84055141 : if (_last_accel_filter_hz != _accel_filter_cutoff()) {
929 4183 : _imu._accel_filter[instance].set_cutoff_frequency(_accel_raw_sample_rate(instance), _accel_filter_cutoff());
930 4183 : _last_accel_filter_hz = _accel_filter_cutoff();
931 : }
932 : }
933 :
934 : #if HAL_LOGGING_ENABLED
935 545430728 : bool AP_InertialSensor_Backend::should_log_imu_raw() const
936 : {
937 545430728 : if (_imu._log_raw_bit == (uint32_t)-1) {
938 : // tracker does not set a bit
939 8524791 : return false;
940 : }
941 536905937 : const AP_Logger *logger = AP_Logger::get_singleton();
942 536905980 : if (logger == nullptr) {
943 0 : return false;
944 : }
945 536905980 : if (!logger->should_log(_imu._log_raw_bit)) {
946 536905920 : return false;
947 : }
948 0 : return true;
949 : }
950 : #endif // HAL_LOGGING_ENABLED
951 :
952 : // log an unexpected change in a register for an IMU
953 0 : void AP_InertialSensor_Backend::log_register_change(uint32_t bus_id, const AP_HAL::Device::checkreg ®)
954 : {
955 : #if HAL_LOGGING_ENABLED
956 : // @LoggerMessage: IREG
957 : // @Description: IMU Register unexpected value change
958 : // @Field: TimeUS: Time since system startup
959 : // @Field: DevID: bus ID
960 : // @Field: Bank: device register bank
961 : // @Field: Reg: device register
962 : // @Field: Val: unexpected value
963 0 : AP::logger().Write("IREG", "TimeUS,DevID,Bank,Reg,Val", "QIBBB",
964 : AP_HAL::micros64(),
965 : bus_id,
966 0 : reg.bank,
967 0 : reg.regnum,
968 0 : reg.value);
969 : #endif
970 0 : }
|
