Line data Source code
1 : #include <assert.h>
2 :
3 : #include "AP_InertialSensor.h"
4 :
5 : #if AP_INERTIALSENSOR_ENABLED
6 :
7 : #include <AP_Common/AP_Common.h>
8 : #include <AP_HAL/AP_HAL.h>
9 : #include <AP_HAL/I2CDevice.h>
10 : #include <AP_HAL/SPIDevice.h>
11 : #include <AP_HAL/DSP.h>
12 : #include <AP_Math/AP_Math.h>
13 : #include <AP_Notify/AP_Notify.h>
14 : #include <AP_BoardConfig/AP_BoardConfig.h>
15 : #include <AP_AHRS/AP_AHRS.h>
16 : #include <AP_AHRS/AP_AHRS_View.h>
17 : #include <AP_ExternalAHRS/AP_ExternalAHRS.h>
18 : #include <AP_GyroFFT/AP_GyroFFT.h>
19 : #include <AP_Vehicle/AP_Vehicle_Type.h>
20 : #if !APM_BUILD_TYPE(APM_BUILD_Rover)
21 : #include <AP_Motors/AP_Motors_Class.h>
22 : #endif
23 : #include <GCS_MAVLink/GCS.h>
24 :
25 : #include "AP_InertialSensor_BMI160.h"
26 : #include "AP_InertialSensor_BMI270.h"
27 : #include "AP_InertialSensor_Backend.h"
28 : #include "AP_InertialSensor_L3G4200D.h"
29 : #include "AP_InertialSensor_LSM6DSV.h"
30 : #include "AP_InertialSensor_LSM9DS0.h"
31 : #include "AP_InertialSensor_LSM9DS1.h"
32 : #include "AP_InertialSensor_Invensense.h"
33 : #include "AP_InertialSensor_SITL.h"
34 : #include "AP_InertialSensor_BMI055.h"
35 : #include "AP_InertialSensor_BMI088.h"
36 : #include "AP_InertialSensor_Invensensev2.h"
37 : #include "AP_InertialSensor_ADIS1647x.h"
38 : #include "AP_InertialSensor_ExternalAHRS.h"
39 : #include "AP_InertialSensor_Invensensev3.h"
40 : #include "AP_InertialSensor_NONE.h"
41 : #include "AP_InertialSensor_SCHA63T.h"
42 : #include "AP_InertialSensor_ASM330.h"
43 : #include "AP_InertialSensor_ADIS16607.h"
44 : #include <AP_Scheduler/AP_Scheduler.h>
45 : #include "AP_InertialSensor_ZeroOne_FPGA_SCH16T.h"
46 :
47 : /* Define INS_TIMING_DEBUG to track down scheduling issues with the main loop.
48 : * Output is on the debug console. */
49 : #ifdef INS_TIMING_DEBUG
50 : #include <stdio.h>
51 : #define timing_printf(fmt, args...) do { printf("[timing] " fmt, ##args); } while(0)
52 : #else
53 : #define timing_printf(fmt, args...)
54 : #endif
55 :
56 : #ifndef HAL_DEFAULT_INS_FAST_SAMPLE
57 : #define HAL_DEFAULT_INS_FAST_SAMPLE 1
58 : #endif
59 :
60 : extern const AP_HAL::HAL& hal;
61 :
62 :
63 :
64 : #if APM_BUILD_COPTER_OR_HELI
65 : #define DEFAULT_GYRO_FILTER 20
66 : #define DEFAULT_ACCEL_FILTER 20
67 : #define DEFAULT_STILL_THRESH 2.5f
68 : #elif APM_BUILD_TYPE(APM_BUILD_Rover)
69 : #define DEFAULT_GYRO_FILTER 4
70 : #define DEFAULT_ACCEL_FILTER 10
71 : #define DEFAULT_STILL_THRESH 0.1f
72 : #else
73 : #define DEFAULT_GYRO_FILTER 20
74 : #define DEFAULT_ACCEL_FILTER 20
75 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) && CONFIG_HAL_BOARD == HAL_BOARD_SITL
76 : // In steady-state level flight on SITL Plane, especially while the motor is off, the INS system
77 : // returns ins.is_still()==true. Baseline vibes while airborne are unrealistically low: around 0.07.
78 : // A real aircraft would be experiencing micro turbulence and be rocking around a tiny bit. Therefore,
79 : // for Plane SIM the vibe threshold needs to be a little lower. Since plane.is_flying() uses
80 : // ins.is_still() during gps loss to detect if we're flying, we want to make sure we are not "perfectly"
81 : // still in the air like we are on the ground.
82 : #define DEFAULT_STILL_THRESH 0.05f
83 : #else
84 : #define DEFAULT_STILL_THRESH 0.1f
85 : #endif
86 : #endif
87 :
88 : #if defined(STM32H7) || defined(STM32F7)
89 : #define MPU_FIFO_FASTSAMPLE_DEFAULT 1
90 : #else
91 : #define MPU_FIFO_FASTSAMPLE_DEFAULT 0
92 : #endif
93 :
94 : #define GYRO_INIT_MAX_DIFF_DPS 0.1f
95 :
96 : #ifndef HAL_INS_TRIM_LIMIT_DEG
97 : #define HAL_INS_TRIM_LIMIT_DEG 10
98 : #endif
99 :
100 : // Class level parameters
101 : const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
102 : // 0 was PRODUCT_ID
103 :
104 : /*
105 : The following parameter indexes and reserved from previous use
106 : as accel offsets and scaling from before the 16g change in the
107 : PX4 backend:
108 :
109 : ACCSCAL : 1
110 : ACCOFFS : 2
111 : MPU6K_FILTER: 4
112 : ACC2SCAL : 5
113 : ACC2OFFS : 6
114 : ACC3SCAL : 8
115 : ACC3OFFS : 9
116 : CALSENSFRAME : 11
117 : */
118 :
119 : // @Param: _GYROFFS_X
120 : // @DisplayName: Gyro offsets of X axis
121 : // @Description: Gyro sensor offsets of X axis. This is setup on each boot during gyro calibrations
122 : // @Units: rad/s
123 : // @User: Advanced
124 : // @Calibration: 1
125 :
126 : // @Param: _GYROFFS_Y
127 : // @DisplayName: Gyro offsets of Y axis
128 : // @Description: Gyro sensor offsets of Y axis. This is setup on each boot during gyro calibrations
129 : // @Units: rad/s
130 : // @User: Advanced
131 : // @Calibration: 1
132 :
133 : // @Param: _GYROFFS_Z
134 : // @DisplayName: Gyro offsets of Z axis
135 : // @Description: Gyro sensor offsets of Z axis. This is setup on each boot during gyro calibrations
136 : // @Units: rad/s
137 : // @User: Advanced
138 : // @Calibration: 1
139 : AP_GROUPINFO("_GYROFFS", 3, AP_InertialSensor, _gyro_offset_old_param[0], 0),
140 :
141 : // @Param: _GYR2OFFS_X
142 : // @DisplayName: Gyro2 offsets of X axis
143 : // @Description: Gyro2 sensor offsets of X axis. This is setup on each boot during gyro calibrations
144 : // @Units: rad/s
145 : // @User: Advanced
146 : // @Calibration: 1
147 :
148 : // @Param: _GYR2OFFS_Y
149 : // @DisplayName: Gyro2 offsets of Y axis
150 : // @Description: Gyro2 sensor offsets of Y axis. This is setup on each boot during gyro calibrations
151 : // @Units: rad/s
152 : // @User: Advanced
153 : // @Calibration: 1
154 :
155 : // @Param: _GYR2OFFS_Z
156 : // @DisplayName: Gyro2 offsets of Z axis
157 : // @Description: Gyro2 sensor offsets of Z axis. This is setup on each boot during gyro calibrations
158 : // @Units: rad/s
159 : // @User: Advanced
160 : // @Calibration: 1
161 :
162 : #if INS_MAX_INSTANCES > 1
163 : AP_GROUPINFO("_GYR2OFFS", 7, AP_InertialSensor, _gyro_offset_old_param[1], 0),
164 : #endif
165 :
166 : // @Param: _GYR3OFFS_X
167 : // @DisplayName: Gyro3 offsets of X axis
168 : // @Description: Gyro3 sensor offsets of X axis. This is setup on each boot during gyro calibrations
169 : // @Units: rad/s
170 : // @User: Advanced
171 : // @Calibration: 1
172 :
173 : // @Param: _GYR3OFFS_Y
174 : // @DisplayName: Gyro3 offsets of Y axis
175 : // @Description: Gyro3 sensor offsets of Y axis. This is setup on each boot during gyro calibrations
176 : // @Units: rad/s
177 : // @User: Advanced
178 : // @Calibration: 1
179 :
180 : // @Param: _GYR3OFFS_Z
181 : // @DisplayName: Gyro3 offsets of Z axis
182 : // @Description: Gyro3 sensor offsets of Z axis. This is setup on each boot during gyro calibrations
183 : // @Units: rad/s
184 : // @User: Advanced
185 : // @Calibration: 1
186 :
187 : #if INS_MAX_INSTANCES > 2
188 : AP_GROUPINFO("_GYR3OFFS", 10, AP_InertialSensor, _gyro_offset_old_param[2], 0),
189 : #endif
190 :
191 : // @Param: _ACCSCAL_X
192 : // @DisplayName: Accelerometer scaling of X axis
193 : // @Description: Accelerometer scaling of X axis. Calculated during acceleration calibration routine
194 : // @Range: 0.8 1.2
195 : // @User: Advanced
196 : // @Calibration: 1
197 :
198 : // @Param: _ACCSCAL_Y
199 : // @DisplayName: Accelerometer scaling of Y axis
200 : // @Description: Accelerometer scaling of Y axis Calculated during acceleration calibration routine
201 : // @Range: 0.8 1.2
202 : // @User: Advanced
203 : // @Calibration: 1
204 :
205 : // @Param: _ACCSCAL_Z
206 : // @DisplayName: Accelerometer scaling of Z axis
207 : // @Description: Accelerometer scaling of Z axis Calculated during acceleration calibration routine
208 : // @Range: 0.8 1.2
209 : // @User: Advanced
210 : // @Calibration: 1
211 : AP_GROUPINFO("_ACCSCAL", 12, AP_InertialSensor, _accel_scale_old_param[0], 1.0),
212 :
213 : // @Param: _ACCOFFS_X
214 : // @DisplayName: Accelerometer offsets of X axis
215 : // @Description: Accelerometer offsets of X axis. This is setup using the acceleration calibration or level operations
216 : // @Units: m/s/s
217 : // @Range: -3.5 3.5
218 : // @User: Advanced
219 : // @Calibration: 1
220 :
221 : // @Param: _ACCOFFS_Y
222 : // @DisplayName: Accelerometer offsets of Y axis
223 : // @Description: Accelerometer offsets of Y axis. This is setup using the acceleration calibration or level operations
224 : // @Units: m/s/s
225 : // @Range: -3.5 3.5
226 : // @User: Advanced
227 : // @Calibration: 1
228 :
229 : // @Param: _ACCOFFS_Z
230 : // @DisplayName: Accelerometer offsets of Z axis
231 : // @Description: Accelerometer offsets of Z axis. This is setup using the acceleration calibration or level operations
232 : // @Units: m/s/s
233 : // @Range: -3.5 3.5
234 : // @User: Advanced
235 : // @Calibration: 1
236 : AP_GROUPINFO("_ACCOFFS", 13, AP_InertialSensor, _accel_offset_old_param[0], 0),
237 :
238 : // @Param: _ACC2SCAL_X
239 : // @DisplayName: Accelerometer2 scaling of X axis
240 : // @Description: Accelerometer2 scaling of X axis. Calculated during acceleration calibration routine
241 : // @Range: 0.8 1.2
242 : // @User: Advanced
243 : // @Calibration: 1
244 :
245 : // @Param: _ACC2SCAL_Y
246 : // @DisplayName: Accelerometer2 scaling of Y axis
247 : // @Description: Accelerometer2 scaling of Y axis Calculated during acceleration calibration routine
248 : // @Range: 0.8 1.2
249 : // @User: Advanced
250 : // @Calibration: 1
251 :
252 : // @Param: _ACC2SCAL_Z
253 : // @DisplayName: Accelerometer2 scaling of Z axis
254 : // @Description: Accelerometer2 scaling of Z axis Calculated during acceleration calibration routine
255 : // @Range: 0.8 1.2
256 : // @User: Advanced
257 : // @Calibration: 1
258 :
259 : #if INS_MAX_INSTANCES > 1
260 : AP_GROUPINFO("_ACC2SCAL", 14, AP_InertialSensor, _accel_scale_old_param[1], 1.0),
261 : #endif
262 :
263 : // @Param: _ACC2OFFS_X
264 : // @DisplayName: Accelerometer2 offsets of X axis
265 : // @Description: Accelerometer2 offsets of X axis. This is setup using the acceleration calibration or level operations
266 : // @Units: m/s/s
267 : // @Range: -3.5 3.5
268 : // @User: Advanced
269 : // @Calibration: 1
270 :
271 : // @Param: _ACC2OFFS_Y
272 : // @DisplayName: Accelerometer2 offsets of Y axis
273 : // @Description: Accelerometer2 offsets of Y axis. This is setup using the acceleration calibration or level operations
274 : // @Units: m/s/s
275 : // @Range: -3.5 3.5
276 : // @User: Advanced
277 : // @Calibration: 1
278 :
279 : // @Param: _ACC2OFFS_Z
280 : // @DisplayName: Accelerometer2 offsets of Z axis
281 : // @Description: Accelerometer2 offsets of Z axis. This is setup using the acceleration calibration or level operations
282 : // @Units: m/s/s
283 : // @Range: -3.5 3.5
284 : // @User: Advanced
285 : // @Calibration: 1
286 :
287 : #if INS_MAX_INSTANCES > 1
288 : AP_GROUPINFO("_ACC2OFFS", 15, AP_InertialSensor, _accel_offset_old_param[1], 0),
289 : #endif
290 :
291 : // @Param: _ACC3SCAL_X
292 : // @DisplayName: Accelerometer3 scaling of X axis
293 : // @Description: Accelerometer3 scaling of X axis. Calculated during acceleration calibration routine
294 : // @Range: 0.8 1.2
295 : // @User: Advanced
296 : // @Calibration: 1
297 :
298 : // @Param: _ACC3SCAL_Y
299 : // @DisplayName: Accelerometer3 scaling of Y axis
300 : // @Description: Accelerometer3 scaling of Y axis Calculated during acceleration calibration routine
301 : // @Range: 0.8 1.2
302 : // @User: Advanced
303 : // @Calibration: 1
304 :
305 : // @Param: _ACC3SCAL_Z
306 : // @DisplayName: Accelerometer3 scaling of Z axis
307 : // @Description: Accelerometer3 scaling of Z axis Calculated during acceleration calibration routine
308 : // @Range: 0.8 1.2
309 : // @User: Advanced
310 : // @Calibration: 1
311 :
312 : #if INS_MAX_INSTANCES > 2
313 : AP_GROUPINFO("_ACC3SCAL", 16, AP_InertialSensor, _accel_scale_old_param[2], 1.0),
314 : #endif
315 :
316 : // @Param: _ACC3OFFS_X
317 : // @DisplayName: Accelerometer3 offsets of X axis
318 : // @Description: Accelerometer3 offsets of X axis. This is setup using the acceleration calibration or level operations
319 : // @Units: m/s/s
320 : // @Range: -3.5 3.5
321 : // @User: Advanced
322 : // @Calibration: 1
323 :
324 : // @Param: _ACC3OFFS_Y
325 : // @DisplayName: Accelerometer3 offsets of Y axis
326 : // @Description: Accelerometer3 offsets of Y axis. This is setup using the acceleration calibration or level operations
327 : // @Units: m/s/s
328 : // @Range: -3.5 3.5
329 : // @User: Advanced
330 : // @Calibration: 1
331 :
332 : // @Param: _ACC3OFFS_Z
333 : // @DisplayName: Accelerometer3 offsets of Z axis
334 : // @Description: Accelerometer3 offsets of Z axis. This is setup using the acceleration calibration or level operations
335 : // @Units: m/s/s
336 : // @Range: -3.5 3.5
337 : // @User: Advanced
338 : // @Calibration: 1
339 :
340 : #if INS_MAX_INSTANCES > 2
341 : AP_GROUPINFO("_ACC3OFFS", 17, AP_InertialSensor, _accel_offset_old_param[2], 0),
342 : #endif
343 :
344 : // @Param: _GYRO_FILTER
345 : // @DisplayName: Gyro filter cutoff frequency
346 : // @Description: Filter cutoff frequency for gyroscopes. This can be set to a lower value to try to cope with very high vibration levels in aircraft. A value of zero means no filtering (not recommended!)
347 : // @Units: Hz
348 : // @Range: 0 256
349 : // @User: Advanced
350 : AP_GROUPINFO("_GYRO_FILTER", 18, AP_InertialSensor, _gyro_filter_cutoff, DEFAULT_GYRO_FILTER),
351 :
352 : // @Param: _ACCEL_FILTER
353 : // @DisplayName: Accel filter cutoff frequency
354 : // @Description: Filter cutoff frequency for accelerometers. This can be set to a lower value to try to cope with very high vibration levels in aircraft. A value of zero means no filtering (not recommended!)
355 : // @Units: Hz
356 : // @Range: 0 256
357 : // @User: Advanced
358 : AP_GROUPINFO("_ACCEL_FILTER", 19, AP_InertialSensor, _accel_filter_cutoff, DEFAULT_ACCEL_FILTER),
359 :
360 : // @Param: _USE
361 : // @DisplayName: Use first IMU for attitude, velocity and position estimates
362 : // @Description: Use first IMU for attitude, velocity and position estimates
363 : // @Values: 0:Disabled,1:Enabled
364 : // @User: Advanced
365 : AP_GROUPINFO("_USE", 20, AP_InertialSensor, _use_old_param[0], 1),
366 :
367 : // @Param: _USE2
368 : // @DisplayName: Use second IMU for attitude, velocity and position estimates
369 : // @Description: Use second IMU for attitude, velocity and position estimates
370 : // @Values: 0:Disabled,1:Enabled
371 : // @User: Advanced
372 :
373 : #if INS_MAX_INSTANCES > 1
374 : AP_GROUPINFO("_USE2", 21, AP_InertialSensor, _use_old_param[1], 1),
375 : #endif
376 :
377 : // @Param: _USE3
378 : // @DisplayName: Use third IMU for attitude, velocity and position estimates
379 : // @Description: Use third IMU for attitude, velocity and position estimates
380 : // @Values: 0:Disabled,1:Enabled
381 : // @User: Advanced
382 :
383 : #if INS_MAX_INSTANCES > 2
384 : AP_GROUPINFO("_USE3", 22, AP_InertialSensor, _use_old_param[2], 1),
385 : #endif
386 :
387 : // @Param: _STILL_THRESH
388 : // @DisplayName: Stillness threshold for detecting if we are moving
389 : // @Description: Threshold to tolerate vibration to determine if vehicle is motionless. This depends on the frame type and if there is a constant vibration due to motors before launch or after landing. Total motionless is about 0.05. Suggested values: Planes/rover use 0.1, multirotors use 1, tradHeli uses 5
390 : // @Range: 0.05 50
391 : // @User: Advanced
392 : AP_GROUPINFO("_STILL_THRESH", 23, AP_InertialSensor, _still_threshold, DEFAULT_STILL_THRESH),
393 :
394 : // @Param: _GYR_CAL
395 : // @DisplayName: Gyro Calibration scheme
396 : // @Description: Conrols when automatic gyro calibration is performed
397 : // @Values: 0:Never, 1:Start-up only
398 : // @User: Advanced
399 : AP_GROUPINFO("_GYR_CAL", 24, AP_InertialSensor, _gyro_cal_timing, 1),
400 :
401 : // @Param: _TRIM_OPTION
402 : // @DisplayName: Accel cal trim option
403 : // @Description: Specifies how the accel cal routine determines the trims
404 : // @User: Advanced
405 : // @Values: 0:Don't adjust the trims,1:Assume first orientation was level,2:Assume ACC_BODYFIX is perfectly aligned to the vehicle
406 : AP_GROUPINFO("_TRIM_OPTION", 25, AP_InertialSensor, _trim_option, 1),
407 :
408 : // @Param: _ACC_BODYFIX
409 : // @DisplayName: Body-fixed accelerometer
410 : // @Description: The body-fixed accelerometer to be used for trim calculation
411 : // @User: Advanced
412 : // @Values: 1:IMU 1,2:IMU 2,3:IMU 3
413 : AP_GROUPINFO("_ACC_BODYFIX", 26, AP_InertialSensor, _acc_body_aligned, 2),
414 :
415 : // @Param: _POS1_X
416 : // @DisplayName: IMU accelerometer X position
417 : // @Description: X position of the first IMU Accelerometer in body frame. Positive X is forward of the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
418 : // @Units: m
419 : // @Range: -5 5
420 : // @Increment: 0.01
421 : // @User: Advanced
422 :
423 : // @Param: _POS1_Y
424 : // @DisplayName: IMU accelerometer Y position
425 : // @Description: Y position of the first IMU accelerometer in body frame. Positive Y is to the right of the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
426 : // @Units: m
427 : // @Range: -5 5
428 : // @Increment: 0.01
429 : // @User: Advanced
430 :
431 : // @Param: _POS1_Z
432 : // @DisplayName: IMU accelerometer Z position
433 : // @Description: Z position of the first IMU accelerometer in body frame. Positive Z is down from the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
434 : // @Units: m
435 : // @Range: -5 5
436 : // @Increment: 0.01
437 : // @User: Advanced
438 : AP_GROUPINFO("_POS1", 27, AP_InertialSensor, _accel_pos_old_param[0], 0.0f),
439 :
440 : // @Param: _POS2_X
441 : // @DisplayName: IMU accelerometer X position
442 : // @Description: X position of the second IMU accelerometer in body frame. Positive X is forward of the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
443 : // @Units: m
444 : // @Range: -5 5
445 : // @Increment: 0.01
446 : // @User: Advanced
447 :
448 : // @Param: _POS2_Y
449 : // @DisplayName: IMU accelerometer Y position
450 : // @Description: Y position of the second IMU accelerometer in body frame. Positive Y is to the right of the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
451 : // @Units: m
452 : // @Range: -5 5
453 : // @Increment: 0.01
454 : // @User: Advanced
455 :
456 : // @Param: _POS2_Z
457 : // @DisplayName: IMU accelerometer Z position
458 : // @Description: Z position of the second IMU accelerometer in body frame. Positive Z is down from the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
459 : // @Units: m
460 : // @Range: -5 5
461 : // @Increment: 0.01
462 : // @User: Advanced
463 :
464 : #if INS_MAX_INSTANCES > 1
465 : AP_GROUPINFO("_POS2", 28, AP_InertialSensor, _accel_pos_old_param[1], 0.0f),
466 : #endif
467 :
468 : // @Param: _POS3_X
469 : // @DisplayName: IMU accelerometer X position
470 : // @Description: X position of the third IMU accelerometer in body frame. Positive X is forward of the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
471 : // @Units: m
472 : // @Range: -10 10
473 : // @User: Advanced
474 :
475 : // @Param: _POS3_Y
476 : // @DisplayName: IMU accelerometer Y position
477 : // @Description: Y position of the third IMU accelerometer in body frame. Positive Y is to the right of the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
478 : // @Units: m
479 : // @Range: -5 5
480 : // @Increment: 0.01
481 : // @User: Advanced
482 :
483 : // @Param: _POS3_Z
484 : // @DisplayName: IMU accelerometer Z position
485 : // @Description: Z position of the third IMU accelerometer in body frame. Positive Z is down from the origin. Attention: The IMU should be located as close to the vehicle c.g. as practical so that the value of this parameter is minimised. Failure to do so can result in noisy navigation velocity measurements due to vibration and IMU gyro noise. If the IMU cannot be moved and velocity noise is a problem, a location closer to the IMU can be used as the body frame origin.
486 : // @Units: m
487 : // @Range: -5 5
488 : // @Increment: 0.01
489 : // @User: Advanced
490 :
491 : #if INS_MAX_INSTANCES > 2
492 : AP_GROUPINFO("_POS3", 29, AP_InertialSensor, _accel_pos_old_param[2], 0.0f),
493 : #endif
494 :
495 : // @Param: _GYR_ID
496 : // @DisplayName: Gyro ID
497 : // @Description: Gyro sensor ID, taking into account its type, bus and instance
498 : // @ReadOnly: True
499 : // @User: Advanced
500 : AP_GROUPINFO("_GYR_ID", 30, AP_InertialSensor, _gyro_id_old_param[0], 0),
501 :
502 : // @Param: _GYR2_ID
503 : // @DisplayName: Gyro2 ID
504 : // @Description: Gyro2 sensor ID, taking into account its type, bus and instance
505 : // @ReadOnly: True
506 : // @User: Advanced
507 :
508 : #if INS_MAX_INSTANCES > 1
509 : AP_GROUPINFO("_GYR2_ID", 31, AP_InertialSensor, _gyro_id_old_param[1], 0),
510 : #endif
511 :
512 : // @Param: _GYR3_ID
513 : // @DisplayName: Gyro3 ID
514 : // @Description: Gyro3 sensor ID, taking into account its type, bus and instance
515 : // @ReadOnly: True
516 : // @User: Advanced
517 :
518 : #if INS_MAX_INSTANCES > 2
519 : AP_GROUPINFO("_GYR3_ID", 32, AP_InertialSensor, _gyro_id_old_param[2], 0),
520 : #endif
521 :
522 : // @Param: _ACC_ID
523 : // @DisplayName: Accelerometer ID
524 : // @Description: Accelerometer sensor ID, taking into account its type, bus and instance
525 : // @ReadOnly: True
526 : // @User: Advanced
527 : AP_GROUPINFO("_ACC_ID", 33, AP_InertialSensor, _accel_id_old_param[0], 0),
528 :
529 : // @Param: _ACC2_ID
530 : // @DisplayName: Accelerometer2 ID
531 : // @Description: Accelerometer2 sensor ID, taking into account its type, bus and instance
532 : // @ReadOnly: True
533 : // @User: Advanced
534 :
535 : #if INS_MAX_INSTANCES > 1
536 : AP_GROUPINFO("_ACC2_ID", 34, AP_InertialSensor, _accel_id_old_param[1], 0),
537 : #endif
538 :
539 : // @Param: _ACC3_ID
540 : // @DisplayName: Accelerometer3 ID
541 : // @Description: Accelerometer3 sensor ID, taking into account its type, bus and instance
542 : // @ReadOnly: True
543 : // @User: Advanced
544 :
545 : #if INS_MAX_INSTANCES > 2
546 : AP_GROUPINFO("_ACC3_ID", 35, AP_InertialSensor, _accel_id_old_param[2], 0),
547 : #endif
548 :
549 : // @Param: _FAST_SAMPLE
550 : // @DisplayName: Fast sampling mask
551 : // @Description: Mask of IMUs to enable fast sampling on, if available
552 : // @User: Advanced
553 : // @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU
554 : AP_GROUPINFO("_FAST_SAMPLE", 36, AP_InertialSensor, _fast_sampling_mask, HAL_DEFAULT_INS_FAST_SAMPLE),
555 :
556 : // index 37 was NOTCH_
557 :
558 : #if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
559 : // @Group: _LOG_
560 : // @Path: ../AP_InertialSensor/BatchSampler.cpp
561 : AP_SUBGROUPINFO(batchsampler, "_LOG_", 39, AP_InertialSensor, AP_InertialSensor::BatchSampler),
562 : #endif
563 :
564 : // @Param: _ENABLE_MASK
565 : // @DisplayName: IMU enable mask
566 : // @Description: Bitmask of IMUs to enable. It can be used to prevent startup of specific detected IMUs
567 : // @User: Advanced
568 : // @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU,3:FourthIMU,4:FifthIMU,5:SixthIMU,6:SeventhIMU
569 : AP_GROUPINFO("_ENABLE_MASK", 40, AP_InertialSensor, _enable_mask, 0x7F),
570 :
571 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
572 : // @Group: _HNTCH_
573 : // @Path: ../Filter/HarmonicNotchFilter.cpp
574 : AP_SUBGROUPINFO(harmonic_notches[0].params, "_HNTCH_", 41, AP_InertialSensor, HarmonicNotchFilterParams),
575 :
576 : #if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 1
577 : // @Group: _HNTC2_
578 : // @Path: ../Filter/HarmonicNotchFilter.cpp
579 : AP_SUBGROUPINFO(harmonic_notches[1].params, "_HNTC2_", 53, AP_InertialSensor, HarmonicNotchFilterParams),
580 : #endif
581 :
582 : #if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 2
583 : // @Group: _HNTC3_
584 : // @Path: ../Filter/HarmonicNotchFilter.cpp
585 : AP_SUBGROUPINFO(harmonic_notches[2].params, "_HNTC3_", 57, AP_InertialSensor, HarmonicNotchFilterParams),
586 : #endif
587 :
588 : #if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 3
589 : // @Group: _HNTC4_
590 : // @Path: ../Filter/HarmonicNotchFilter.cpp
591 : AP_SUBGROUPINFO(harmonic_notches[3].params, "_HNTC4_", 58, AP_InertialSensor, HarmonicNotchFilterParams),
592 : #endif
593 : #endif
594 :
595 : // @Param: _GYRO_RATE
596 : // @DisplayName: Gyro rate for IMUs with Fast Sampling enabled
597 : // @Description: Gyro rate for IMUs with fast sampling enabled. The gyro rate is the sample rate at which the IMU filters operate and needs to be at least double the maximum filter frequency. If the sensor does not support the selected rate the next highest supported rate will be used. For IMUs which do not support fast sampling this setting is ignored and the default gyro rate of 1Khz is used.
598 : // @User: Advanced
599 : // @Values: 0:1kHz,1:2kHz,2:4kHz,3:8kHz
600 : // @RebootRequired: True
601 : AP_GROUPINFO("_GYRO_RATE", 42, AP_InertialSensor, _fast_sampling_rate, MPU_FIFO_FASTSAMPLE_DEFAULT),
602 :
603 :
604 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
605 : // @Group: _TCAL1_
606 : // @Path: AP_InertialSensor_tempcal.cpp
607 : AP_SUBGROUPINFO(tcal_old_param[0], "_TCAL1_", 43, AP_InertialSensor, AP_InertialSensor_TCal),
608 :
609 : #if INS_MAX_INSTANCES > 1
610 : // @Group: _TCAL2_
611 : // @Path: AP_InertialSensor_tempcal.cpp
612 : AP_SUBGROUPINFO(tcal_old_param[1], "_TCAL2_", 44, AP_InertialSensor, AP_InertialSensor_TCal),
613 : #endif
614 :
615 : #if INS_MAX_INSTANCES > 2
616 : // @Group: _TCAL3_
617 : // @Path: AP_InertialSensor_tempcal.cpp
618 : AP_SUBGROUPINFO(tcal_old_param[2], "_TCAL3_", 45, AP_InertialSensor, AP_InertialSensor_TCal),
619 : #endif
620 :
621 : // @Param: _ACC1_CALTEMP
622 : // @DisplayName: Calibration temperature for 1st accelerometer
623 : // @Description: Temperature that the 1st accelerometer was calibrated at
624 : // @User: Advanced
625 : // @Units: degC
626 : // @Calibration: 1
627 : AP_GROUPINFO("_ACC1_CALTEMP", 46, AP_InertialSensor, caltemp_accel_old_param[0], -300),
628 :
629 : // @Param: _GYR1_CALTEMP
630 : // @DisplayName: Calibration temperature for 1st gyroscope
631 : // @Description: Temperature that the 1st gyroscope was calibrated at
632 : // @User: Advanced
633 : // @Units: degC
634 : // @Calibration: 1
635 : AP_GROUPINFO("_GYR1_CALTEMP", 47, AP_InertialSensor, caltemp_gyro_old_param[0], -300),
636 :
637 : #if INS_MAX_INSTANCES > 1
638 : // @Param: _ACC2_CALTEMP
639 : // @DisplayName: Calibration temperature for 2nd accelerometer
640 : // @Description: Temperature that the 2nd accelerometer was calibrated at
641 : // @User: Advanced
642 : // @Units: degC
643 : // @Calibration: 1
644 : AP_GROUPINFO("_ACC2_CALTEMP", 48, AP_InertialSensor, caltemp_accel_old_param[1], -300),
645 :
646 : // @Param: _GYR2_CALTEMP
647 : // @DisplayName: Calibration temperature for 2nd gyroscope
648 : // @Description: Temperature that the 2nd gyroscope was calibrated at
649 : // @User: Advanced
650 : // @Units: degC
651 : // @Calibration: 1
652 : AP_GROUPINFO("_GYR2_CALTEMP", 49, AP_InertialSensor, caltemp_gyro_old_param[1], -300),
653 : #endif
654 :
655 : #if INS_MAX_INSTANCES > 2
656 : // @Param: _ACC3_CALTEMP
657 : // @DisplayName: Calibration temperature for 3rd accelerometer
658 : // @Description: Temperature that the 3rd accelerometer was calibrated at
659 : // @User: Advanced
660 : // @Units: degC
661 : // @Calibration: 1
662 : AP_GROUPINFO("_ACC3_CALTEMP", 50, AP_InertialSensor, caltemp_accel_old_param[2], -300),
663 :
664 : // @Param: _GYR3_CALTEMP
665 : // @DisplayName: Calibration temperature for 3rd gyroscope
666 : // @Description: Temperature that the 3rd gyroscope was calibrated at
667 : // @User: Advanced
668 : // @Units: degC
669 : // @Calibration: 1
670 : AP_GROUPINFO("_GYR3_CALTEMP", 51, AP_InertialSensor, caltemp_gyro_old_param[2], -300),
671 : #endif
672 :
673 : // @Param: _TCAL_OPTIONS
674 : // @DisplayName: Options for temperature calibration
675 : // @Description: This enables optional temperature calibration features. Setting of the Persist bits will save the temperature and/or accelerometer calibration parameters in the bootloader sector on the next update of the bootloader.
676 : // @Bitmask: 0:PersistTemps, 1:PersistAccels
677 : // @User: Advanced
678 : AP_GROUPINFO("_TCAL_OPTIONS", 52, AP_InertialSensor, tcal_options, 0),
679 :
680 : #endif // HAL_INS_TEMPERATURE_CAL_ENABLE
681 :
682 :
683 : #if INS_MAX_INSTANCES > 3
684 : // @Group: 4_
685 : // @Path: AP_InertialSensor_Params.cpp
686 : AP_SUBGROUPINFO(params[0], "4_", 54, AP_InertialSensor, AP_InertialSensor_Params),
687 : #endif
688 :
689 : #if INS_MAX_INSTANCES > 4
690 : // @Group: 5_
691 : // @Path: AP_InertialSensor_Params.cpp
692 : AP_SUBGROUPINFO(params[1], "5_", 55, AP_InertialSensor, AP_InertialSensor_Params),
693 : #endif
694 :
695 : // @Param: _RAW_LOG_OPT
696 : // @DisplayName: Raw logging options
697 : // @Description: Raw logging options bitmask
698 : // @Bitmask: 0:Log primary gyro only, 1:Log all gyros, 2:Post filter, 3: Pre and post filter
699 : // @User: Advanced
700 : AP_GROUPINFO("_RAW_LOG_OPT", 56, AP_InertialSensor, raw_logging_options, 0),
701 :
702 : // indexes 57 and 58 used by INS_HNTC3 and INS_HNTC4
703 :
704 : /*
705 : NOTE: parameter indexes have gaps above. When adding new
706 : parameters check for conflicts carefully
707 : */
708 : AP_GROUPEND
709 : };
710 :
711 : AP_InertialSensor *AP_InertialSensor::_singleton = nullptr;
712 :
713 2098 : AP_InertialSensor::AP_InertialSensor() :
714 2098 : _board_orientation(ROTATION_NONE),
715 127948 : _log_raw_bit(-1)
716 : {
717 2098 : if (_singleton) {
718 0 : AP_HAL::panic("Too many inertial sensors");
719 : }
720 2098 : _singleton = this;
721 2098 : AP_Param::setup_object_defaults(this, var_info);
722 :
723 8392 : for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
724 6294 : _gyro_cal_ok[i] = true;
725 : }
726 8392 : for (uint8_t i=0; i<INS_VIBRATION_CHECK_INSTANCES; i++) {
727 6294 : _accel_vibe_floor_filter[i].set_cutoff_frequency(AP_INERTIAL_SENSOR_ACCEL_VIBE_FLOOR_FILT_HZ);
728 6294 : _accel_vibe_filter[i].set_cutoff_frequency(AP_INERTIAL_SENSOR_ACCEL_VIBE_FILT_HZ);
729 : }
730 : #if HAL_INS_ACCELCAL_ENABLED
731 2098 : AP_AccelCal::register_client(this);
732 : #endif
733 2098 : }
734 :
735 : /*
736 : * Get the AP_InertialSensor singleton
737 : */
738 829301397 : AP_InertialSensor *AP_InertialSensor::get_singleton()
739 : {
740 829301397 : if (!_singleton) {
741 0 : _singleton = NEW_NOTHROW AP_InertialSensor();
742 : }
743 829301397 : return _singleton;
744 : }
745 :
746 : /*
747 : register a new gyro instance
748 : */
749 4183 : bool AP_InertialSensor::register_gyro(uint8_t &instance, uint16_t raw_sample_rate_hz, uint32_t id)
750 : {
751 4183 : if (_gyro_count == INS_MAX_INSTANCES) {
752 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to register gyro id %u", unsigned(id));
753 0 : return false;
754 : }
755 :
756 : // Loop over the existing instances and check if the instance already exists
757 6285 : for (uint8_t instance_to_check = 0; instance_to_check < _gyro_count; instance_to_check++) {
758 2102 : if ((uint32_t)_gyro_id(instance_to_check) == id) {
759 : // if it does, then bail
760 0 : return false;
761 : }
762 : }
763 :
764 4183 : _gyro_raw_sample_rates[_gyro_count] = raw_sample_rate_hz;
765 4183 : _gyro_over_sampling[_gyro_count] = 1;
766 4183 : _gyro_raw_sampling_multiplier[_gyro_count] = INT16_MAX/radians(2000);
767 :
768 4183 : bool saved = _gyro_id(_gyro_count).load();
769 :
770 4183 : if (saved && (uint32_t)_gyro_id(_gyro_count) != id) {
771 : // inconsistent gyro id - mark it as needing calibration
772 26 : _gyro_cal_ok[_gyro_count] = false;
773 : }
774 :
775 4183 : _gyro_id(_gyro_count).set((int32_t) id);
776 :
777 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
778 4183 : if (!saved) {
779 : // assume this is the same sensor and save its ID to allow seamless
780 : // transition from when we didn't have the IDs.
781 595 : _gyro_id(_gyro_count).save();
782 : }
783 : #endif
784 :
785 4183 : instance = _gyro_count++;
786 :
787 4183 : return true;
788 : }
789 :
790 : /*
791 : get the accel instance number we will get from register_accel()
792 : */
793 0 : bool AP_InertialSensor::get_accel_instance(uint8_t &instance) const
794 : {
795 0 : if (_accel_count == INS_MAX_INSTANCES) {
796 0 : return false;
797 : }
798 0 : instance = _accel_count;
799 0 : return true;
800 : }
801 :
802 : /*
803 : get the gyro instance number we will get from register_accel()
804 : */
805 0 : bool AP_InertialSensor::get_gyro_instance(uint8_t &instance) const
806 : {
807 0 : if (_gyro_count == INS_MAX_INSTANCES) {
808 0 : return false;
809 : }
810 0 : instance = _gyro_count;
811 0 : return true;
812 : }
813 :
814 : /*
815 : register a new accel instance
816 : */
817 4183 : bool AP_InertialSensor::register_accel(uint8_t &instance, uint16_t raw_sample_rate_hz, uint32_t id)
818 : {
819 4183 : if (_accel_count == INS_MAX_INSTANCES) {
820 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to register accel id %u", unsigned(id));
821 0 : return false;
822 : }
823 :
824 : // Loop over the existing instances and check if the instance already exists
825 6285 : for (uint8_t instance_to_check = 0; instance_to_check < _accel_count; instance_to_check++) {
826 2102 : if ((uint32_t)_accel_id(instance_to_check) == id) {
827 : // if it does, then bail
828 0 : return false;
829 : }
830 : }
831 :
832 4183 : _accel_raw_sample_rates[_accel_count] = raw_sample_rate_hz;
833 4183 : _accel_over_sampling[_accel_count] = 1;
834 4183 : _accel_raw_sampling_multiplier[_accel_count] = INT16_MAX/(16*GRAVITY_MSS);
835 :
836 4183 : bool saved = _accel_id(_accel_count).load();
837 :
838 4183 : if (!saved) {
839 : // inconsistent accel id
840 597 : _accel_id_ok[_accel_count] = false;
841 3586 : } else if ((uint32_t)_accel_id(_accel_count) != id) {
842 : // inconsistent accel id
843 24 : _accel_id_ok[_accel_count] = false;
844 : } else {
845 3562 : _accel_id_ok[_accel_count] = true;
846 : }
847 :
848 4183 : _accel_id(_accel_count).set((int32_t) id);
849 :
850 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL || (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && AP_SIM_ENABLED)
851 : // assume this is the same sensor and save its ID to allow seamless
852 : // transition from when we didn't have the IDs.
853 4183 : _accel_id_ok[_accel_count] = true;
854 4183 : _accel_id(_accel_count).save();
855 : #endif
856 :
857 4183 : instance = _accel_count++;
858 4183 : return true;
859 : }
860 :
861 : /*
862 : * Start all backends for gyro and accel measurements. It automatically calls
863 : * detect_backends() if it has not been called already.
864 : */
865 2088 : void AP_InertialSensor::_start_backends()
866 :
867 : {
868 2088 : detect_backends();
869 :
870 6271 : for (uint8_t i = 0; i < _backend_count; i++) {
871 4183 : _backends[i]->start();
872 : }
873 :
874 : #if AP_INERTIALSENSOR_ALLOW_NO_SENSORS
875 : if (_gyro_count == 0 || _accel_count == 0) {
876 : AP_HAL::panic("INS needs at least 1 gyro and 1 accel");
877 : }
878 : #endif
879 :
880 : // clear IDs for unused sensor instances
881 4169 : for (uint8_t i=get_accel_count(); i<INS_MAX_INSTANCES; i++) {
882 2081 : _accel_id(i).set(0);
883 : }
884 4169 : for (uint8_t i=get_gyro_count(); i<INS_MAX_INSTANCES; i++) {
885 2081 : _gyro_id(i).set(0);
886 : }
887 2088 : }
888 :
889 : /* Find the N instance of the backend that has already been successfully detected */
890 0 : AP_InertialSensor_Backend *AP_InertialSensor::_find_backend(int16_t backend_id, uint8_t instance)
891 : {
892 0 : uint8_t found = 0;
893 :
894 0 : for (uint8_t i = 0; i < _backend_count; i++) {
895 0 : int16_t id = _backends[i]->get_id();
896 :
897 0 : if (id < 0 || id != backend_id) {
898 0 : continue;
899 : }
900 :
901 0 : if (instance == found) {
902 0 : return _backends[i];
903 : } else {
904 0 : found++;
905 : }
906 : }
907 :
908 0 : return nullptr;
909 : }
910 :
911 20 : bool AP_InertialSensor::set_gyro_window_size(uint16_t size) {
912 : #if HAL_GYROFFT_ENABLED
913 20 : _gyro_window_size = size;
914 :
915 : // allocate FFT gyro window
916 80 : for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
917 240 : for (uint8_t j = 0; j < XYZ_AXIS_COUNT; j++) {
918 180 : if (!_gyro_window[i][j].set_size(size)) {
919 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to allocate window for INS");
920 : // clean up whatever we have currently allocated
921 0 : for (uint8_t ii = 0; ii <= i; ii++) {
922 0 : for (uint8_t jj = 0; jj < j; jj++) {
923 0 : _gyro_window[ii][jj].set_size(0);
924 0 : _gyro_window_size = 0;
925 : }
926 : }
927 0 : return false;
928 : }
929 : }
930 : }
931 : #endif
932 20 : return true;
933 : }
934 :
935 : #if HAL_WITH_DSP && AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
936 324 : bool AP_InertialSensor::has_fft_notch() const
937 : {
938 1296 : for (auto ¬ch : harmonic_notches) {
939 972 : if (notch.params.enabled() && notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) {
940 0 : return true;
941 : }
942 : }
943 324 : return false;
944 : }
945 : #endif
946 :
947 : void
948 2088 : AP_InertialSensor::init(uint16_t loop_rate)
949 : {
950 : // remember the sample rate
951 2088 : _loop_rate = loop_rate;
952 2088 : _loop_delta_t = 1.0f / loop_rate;
953 :
954 : // we don't allow deltat values greater than 10x the normal loop
955 : // time to be exposed outside of INS. Large deltat values can
956 : // cause divergence of state estimators
957 2088 : _loop_delta_t_max = 10 * _loop_delta_t;
958 :
959 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
960 : // Initialize notch params
961 8352 : for (auto ¬ch : harmonic_notches) {
962 6264 : notch.params.init();
963 : }
964 : #endif
965 :
966 2088 : if (_gyro_count == 0 && _accel_count == 0) {
967 2088 : _start_backends();
968 : }
969 :
970 : // calibrate gyros unless gyro calibration has been disabled
971 2088 : if (gyro_calibration_timing() != GYRO_CAL_NEVER && _gyro_count > 0) {
972 1375 : init_gyro();
973 : }
974 :
975 2088 : _sample_period_usec = 1000*1000UL / _loop_rate;
976 :
977 : // establish the baseline time between samples
978 2088 : _delta_time = 0;
979 2088 : _next_sample_usec = 0;
980 2088 : _last_sample_usec = 0;
981 2088 : _have_sample = false;
982 :
983 : #if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
984 : // initialise IMU batch logging
985 2088 : batchsampler.init();
986 : #endif
987 :
988 : #if HAL_GYROFFT_ENABLED
989 2088 : AP_GyroFFT* fft = AP::fft();
990 2088 : bool fft_enabled = fft != nullptr && fft->enabled();
991 2088 : if (fft_enabled) {
992 20 : _post_filter_fft = fft->using_post_filter_samples();
993 : }
994 :
995 : // calculate the position that the FFT window needs to be applied
996 : // Use cases:
997 : // Gyro -> FFT window -> FFT Notch1/2 -> Non-FFT Notch2/1 -> LPF -> Filtered Gyro -- Phase 0
998 : // Gyro -> FFT window -> Non-FFT Notch1/2 -> LPF -> Filtered Gyro -- Phase 0
999 : // Gyro -> Non-FFT Notch1 -> Filtered FFT Window -> FFT Notch2 -> LPF -> Filtered Gyro -- Phase 1
1000 : // Gyro -> Non-FFT Notch1/2 -> Non-FFT Notch1/2 -> Filtered FFT Window -> LPF -> Filtered Gyro -- Phase 2
1001 : // Gyro -> Non-FFT Notch1/2 -> Filtered FFT Window -> LPF -> Filtered Gyro -- Phase 1
1002 : // Gyro -> Filtered FFT Window -> LPF -> Filtered Gyro -- Phase 0
1003 : // Gyro -> FFT window -> LPF -> Filtered Gyro -- Phase 0
1004 : // Gyro -> Notch1/2 -> LPF -> Filtered Gyro
1005 :
1006 2088 : if (_post_filter_fft) {
1007 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
1008 6 : for (auto ¬ch : harmonic_notches) {
1009 5 : if (!notch.params.enabled()) {
1010 1 : continue;
1011 : }
1012 : // window must always come before any FFT notch
1013 4 : if (notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) {
1014 1 : break;
1015 : }
1016 3 : _fft_window_phase++;
1017 : }
1018 : #endif
1019 : }
1020 : #else
1021 0 : bool fft_enabled = false;
1022 : (void)fft_enabled;
1023 : #endif
1024 :
1025 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
1026 :
1027 : #if APM_BUILD_TYPE(APM_BUILD_Heli)
1028 : // Throttle tracking does not make sense with heli because "throttle" is actually collective position in AP_MotorsHeli
1029 : for (auto ¬ch : harmonic_notches) {
1030 : if (notch.params.enabled() && notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateThrottle) {
1031 : AP_BoardConfig::config_error("Throttle notch unavailable with heli");
1032 : }
1033 : }
1034 : #endif
1035 :
1036 : // the center frequency of the harmonic notch is always taken from the calculated value so that it can be updated
1037 : // dynamically, the calculated value is always some multiple of the configured center frequency, so start with the
1038 : // configured value
1039 8352 : for (auto ¬ch : harmonic_notches) {
1040 6264 : if (!notch.params.enabled() && !fft_enabled) {
1041 6190 : continue;
1042 : }
1043 74 : notch.num_calculated_notch_frequencies = 1;
1044 74 : notch.num_dynamic_notches = 1;
1045 : #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1046 74 : if (notch.params.hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
1047 : #if HAL_GYROFFT_ENABLED
1048 12 : if (notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) {
1049 3 : notch.num_dynamic_notches = AP_HAL::DSP::MAX_TRACKED_PEAKS; // only 3 peaks supported currently
1050 : } else
1051 : #endif
1052 : {
1053 9 : AP_Motors *motors = AP::motors();
1054 9 : if (motors != nullptr) {
1055 : // Always have at least one notch, this allows the filter to alocate and then be expanded at runtime if the number of motors is changed
1056 : // Never have more than INS_MAX_NOTCHES
1057 9 : notch.num_dynamic_notches = MAX(MIN(__builtin_popcount(motors->get_motor_mask()), INS_MAX_NOTCHES), 1);
1058 : }
1059 : }
1060 : // avoid harmonics unless actually configured by the user
1061 12 : notch.params.set_default_harmonics(1);
1062 : }
1063 : #endif
1064 : }
1065 : #endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
1066 :
1067 : // count number of used sensors
1068 2088 : uint8_t sensors_used = 0;
1069 8352 : for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
1070 6264 : sensors_used += _use(i);
1071 : }
1072 :
1073 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
1074 2088 : uint16_t num_filters = 0;
1075 8352 : for (auto ¬ch : harmonic_notches) {
1076 : // calculate number of notches we might want to use for harmonic notch
1077 6264 : if (notch.params.enabled() || fft_enabled) {
1078 74 : const bool all_sensors = notch.params.hasOption(HarmonicNotchFilterParams::Options::EnableOnAllIMUs);
1079 74 : num_filters += __builtin_popcount(notch.params.harmonics())
1080 74 : * notch.num_dynamic_notches * notch.params.num_composite_notches()
1081 74 : * (all_sensors?sensors_used:1);
1082 : }
1083 : }
1084 :
1085 2088 : if (num_filters > HAL_HNF_MAX_FILTERS) {
1086 0 : AP_BoardConfig::config_error("Too many notches: %u > %u", num_filters, HAL_HNF_MAX_FILTERS);
1087 : }
1088 :
1089 : // allocate notches
1090 6271 : for (uint8_t i=0; i<get_gyro_count(); i++) {
1091 : // only allocate notches for IMUs in use
1092 4183 : if (_use(i)) {
1093 16732 : for (auto ¬ch : harmonic_notches) {
1094 12549 : if (notch.params.enabled() || fft_enabled) {
1095 148 : notch.filter[i].allocate_filters(notch.num_dynamic_notches,
1096 148 : notch.params.harmonics(), notch.params.num_composite_notches());
1097 : // initialise default settings, these will be subsequently changed in AP_InertialSensor_Backend::update_gyro()
1098 148 : notch.filter[i].init(_gyro_raw_sample_rates[i], notch.params);
1099 : }
1100 : }
1101 : }
1102 : }
1103 : #endif
1104 :
1105 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
1106 : /*
1107 : see if user has setup for on-boot enable of temperature learning
1108 : */
1109 2088 : if (temperature_cal_running()) {
1110 1 : tcal_learning = true;
1111 : }
1112 : #endif
1113 2088 : }
1114 :
1115 4183 : bool AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *backend)
1116 : {
1117 :
1118 4183 : if (!backend) {
1119 0 : return false;
1120 : }
1121 4183 : if (_backend_count == INS_MAX_BACKENDS) {
1122 0 : AP_HAL::panic("Too many INS backends");
1123 : }
1124 4183 : _backends[_backend_count++] = backend;
1125 4183 : return true;
1126 : }
1127 :
1128 : /*
1129 : detect available backends for this board
1130 : */
1131 : void
1132 2088 : AP_InertialSensor::detect_backends(void)
1133 : {
1134 2088 : if (_backends_detected) {
1135 0 : return;
1136 : }
1137 :
1138 2088 : _backends_detected = true;
1139 :
1140 : #if AP_INERTIALSENSOR_FORCE_ENABLE_NONISOLATED_INSTANCE
1141 : #if INS_MAX_INSTANCES < 3
1142 : #error AP_INERTIALSENSOR_FORCE_ENABLE_NONISOLATED_INSTANCE is not relevant for < 3 IMUs
1143 : #endif
1144 : // special case for Cubes, where the IMUs on the isolated
1145 : // board could fail on some boards. If the user has INS_USE=1,
1146 : // INS_USE2=1 and INS_USE3=0 then force INS_USE3 to 1. This is
1147 : // done as users loading past parameter files may end up with
1148 : // INS_USE3=0 unintentionally, which is unsafe on these
1149 : // boards. For users who really want limited IMUs they will need
1150 : // to either use the INS_ENABLE_MASK or set INS_USE2=0 which will
1151 : // enable the first IMU without triggering this check
1152 : if (_use(0) == 1 && _use(1) == 1 && _use(2) == 0) {
1153 : _use(2).set(1);
1154 : }
1155 : #endif
1156 :
1157 2088 : uint8_t probe_count __attribute__((unused)) = 0;
1158 2088 : uint8_t enable_mask __attribute__((unused)) = uint8_t(_enable_mask.get());
1159 2088 : uint8_t found_mask __attribute__((unused)) = 0;
1160 :
1161 : /*
1162 : use ADD_BACKEND() macro to allow for INS_ENABLE_MASK for enabling/disabling INS backends
1163 : */
1164 : #define ADD_BACKEND(x) do { \
1165 : if (((1U<<probe_count)&enable_mask) && _add_backend(x)) { \
1166 : found_mask |= (1U<<probe_count); \
1167 : } \
1168 : probe_count++; \
1169 : } while (0)
1170 :
1171 : // Can be used by adding INSTANCE:<num> keyword in hwdef.
1172 : // This keyword is used to denote the instance number of the sensor
1173 : // while probing. Probing is skipped if the instance number doesn't match the
1174 : // backend count. Its important the IMUs are listed in order of precedence globally
1175 : // (i.e. INSTANCE:0 IMUs are listed before INSTANCE:1 IMUs) and locally (i.e. IMUs
1176 : // on the same bus are listed in order of detection precedence)
1177 :
1178 : #define ADD_BACKEND_INSTANCE(x, instance) if (instance == _backend_count) { ADD_BACKEND(x); }
1179 :
1180 : // Can be used by adding AUX:<devid> keyword in hwdef.
1181 : // AUX:<devid> keyword is used to check for the presence of the sensor
1182 : // in the detected IMUs list. If the IMU with the given devid is found
1183 : // then we skip the probe for the sensor the second time. This is useful
1184 : // if you have multiple choices for IMU over same instance number, and still
1185 : // want to instantiate the sensor after main IMUs are detected.
1186 :
1187 : #define ADD_BACKEND_AUX(x, devid) do { \
1188 : bool init_aux = true; \
1189 : for (uint8_t i=0; i<_backend_count; i++) { \
1190 : if (((uint32_t)_accel_id(i) == devid) || ((uint32_t)_gyro_id(i) == devid)) { \
1191 : init_aux = false; \
1192 : } \
1193 : } \
1194 : if (init_aux) { \
1195 : ADD_BACKEND(x); \
1196 : } \
1197 : } while (0)
1198 :
1199 : // support for adding IMUs conditioned on board type
1200 : #define BOARD_MATCH(board_type) AP_BoardConfig::get_board_type()==AP_BoardConfig::board_type
1201 : #define ADD_BACKEND_BOARD_MATCH(board_match, x) do { if (board_match) { ADD_BACKEND(x); } } while(0)
1202 :
1203 : // macro for use by HAL_INS_PROBE_LIST
1204 : #define GET_I2C_DEVICE(bus, address) hal.i2c_mgr->get_device(bus, address)
1205 :
1206 : #if AP_EXTERNAL_AHRS_ENABLED
1207 : // if enabled, make the first IMU the external AHRS
1208 2088 : const int8_t serial_port = AP::externalAHRS().get_port(AP_ExternalAHRS::AvailableSensor::IMU);
1209 2088 : if (serial_port >= 0) {
1210 6 : const uint8_t count_before = _backend_count;
1211 6 : ADD_BACKEND(NEW_NOTHROW AP_InertialSensor_ExternalAHRS(*this, serial_port));
1212 6 : _first_onboard_imu_instance = _backend_count - count_before; // Sets to 1 only if it actually registered.
1213 : }
1214 : #endif
1215 :
1216 : #if AP_SIM_INS_ENABLED
1217 6265 : for (uint8_t i=0; i<AP::sitl()->imu_count; i++) {
1218 4177 : ADD_BACKEND(AP_InertialSensor_SITL::detect(*this, i==1?INS_SITL_SENSOR_B:INS_SITL_SENSOR_A));
1219 : }
1220 2088 : return;
1221 : #endif
1222 :
1223 : #if defined(HAL_INS_PROBE_LIST)
1224 : // IMUs defined by IMU lines in hwdef.dat
1225 : HAL_INS_PROBE_LIST;
1226 : #if defined(HAL_SITL_INVENSENSEV3)
1227 : ADD_BACKEND(AP_InertialSensor_Invensensev3::probe(*this, hal.i2c_mgr->get_device(1, 1), ROTATION_NONE));
1228 : #endif
1229 : #elif AP_FEATURE_BOARD_DETECT
1230 : switch (AP_BoardConfig::get_board_type()) {
1231 : case AP_BoardConfig::PX4_BOARD_PX4V1:
1232 : ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_NONE));
1233 : break;
1234 :
1235 : case AP_BoardConfig::PX4_BOARD_PIXHAWK:
1236 : ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180));
1237 : ADD_BACKEND(AP_InertialSensor_LSM9DS0::probe(*this,
1238 : hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
1239 : hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME),
1240 : ROTATION_ROLL_180,
1241 : ROTATION_ROLL_180_YAW_270,
1242 : ROTATION_PITCH_180));
1243 : break;
1244 :
1245 : case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
1246 : // older Pixhawk2 boards have the MPU6000 instead of MPU9250
1247 : _fast_sampling_mask.set_default(1);
1248 : ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), ROTATION_PITCH_180));
1249 : ADD_BACKEND(AP_InertialSensor_LSM9DS0::probe(*this,
1250 : hal.spi->get_device(HAL_INS_LSM9DS0_EXT_G_NAME),
1251 : hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME),
1252 : ROTATION_ROLL_180_YAW_270,
1253 : ROTATION_ROLL_180_YAW_90,
1254 : ROTATION_ROLL_180_YAW_90));
1255 : ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
1256 : // new cubes have ICM20602, ICM20948, ICM20649
1257 : ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device("icm20602_ext"), ROTATION_ROLL_180_YAW_270));
1258 : ADD_BACKEND(AP_InertialSensor_Invensensev2::probe(*this, hal.spi->get_device("icm20948_ext"), ROTATION_PITCH_180));
1259 : ADD_BACKEND(AP_InertialSensor_Invensensev2::probe(*this, hal.spi->get_device("icm20948"), ROTATION_YAW_270));
1260 : break;
1261 :
1262 : case AP_BoardConfig::PX4_BOARD_FMUV6:
1263 : _fast_sampling_mask.set_default(1);
1264 : ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device("icm20689"), ROTATION_NONE));
1265 : ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device("icm20602"), ROTATION_NONE));
1266 : // allow for either BMI055 or BMI088
1267 : ADD_BACKEND(AP_InertialSensor_BMI055::probe(*this,
1268 : hal.spi->get_device("bmi055_a"),
1269 : hal.spi->get_device("bmi055_g"),
1270 : ROTATION_ROLL_180_YAW_90));
1271 : ADD_BACKEND(AP_InertialSensor_BMI088::probe(*this,
1272 : hal.spi->get_device("bmi055_a"),
1273 : hal.spi->get_device("bmi055_g"),
1274 : ROTATION_ROLL_180_YAW_90));
1275 : break;
1276 :
1277 : case AP_BoardConfig::PX4_BOARD_PHMINI:
1278 : // PHMINI uses ICM20608 on the ACCEL_MAG device and a MPU9250 on the old MPU6000 CS line
1279 : _fast_sampling_mask.set_default(3);
1280 : ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_ROLL_180));
1281 : ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180));
1282 : break;
1283 :
1284 : case AP_BoardConfig::PX4_BOARD_AUAV21:
1285 : // AUAV2.1 uses ICM20608 on the ACCEL_MAG device and a MPU9250 on the old MPU6000 CS line
1286 : _fast_sampling_mask.set_default(3);
1287 : ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_ROLL_180_YAW_90));
1288 : ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180_YAW_90));
1289 : break;
1290 :
1291 : case AP_BoardConfig::PX4_BOARD_PH2SLIM:
1292 : _fast_sampling_mask.set_default(1);
1293 : ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
1294 : break;
1295 :
1296 : case AP_BoardConfig::PX4_BOARD_AEROFC:
1297 : _fast_sampling_mask.set_default(1);
1298 : ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU6500_NAME), ROTATION_YAW_270));
1299 : break;
1300 :
1301 : default:
1302 : break;
1303 : }
1304 : #elif HAL_INS_DEFAULT == HAL_INS_NONE
1305 : // no INS device
1306 : #else
1307 : #error Unrecognised HAL_INS_TYPE setting
1308 : #endif
1309 :
1310 0 : if (_backend_count == 0) {
1311 :
1312 : // no real INS backends avail, lets use an empty substitute to boot ok and get to mavlink
1313 : #if CONFIG_HAL_BOARD == HAL_BOARD_ESP32
1314 : ADD_BACKEND(AP_InertialSensor_NONE::detect(*this, INS_NONE_SENSOR_A));
1315 : #else
1316 0 : DEV_PRINTF("INS: unable to initialise driver\n");
1317 0 : GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "INS: unable to initialise driver");
1318 : #if !AP_INERTIALSENSOR_ALLOW_NO_SENSORS
1319 0 : AP_BoardConfig::config_error("INS: unable to initialise driver");
1320 : #endif
1321 : #endif
1322 : }
1323 : }
1324 :
1325 : // Armed, Copter, PixHawk:
1326 : // ins_periodic: 57500 events, 0 overruns, 208754us elapsed, 3us avg, min 1us max 218us 40.662us rms
1327 36043796 : void AP_InertialSensor::periodic()
1328 : {
1329 : #if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
1330 36043796 : batchsampler.periodic();
1331 : #endif
1332 36043796 : }
1333 :
1334 :
1335 : /*
1336 : _calculate_trim - calculates the x and y trim angles. The
1337 : accel_sample must be correctly scaled, offset and oriented for the
1338 : board
1339 :
1340 : Note that this only changes 2 axes of the trim vector. When in
1341 : ROTATION_NONE view we can calculate the x and y trim. When in
1342 : ROTATION_PITCH_90 for tailsitters we can calculate y and z. This
1343 : allows users to trim for both flight orientations by doing two trim
1344 : operations, one at each orientation
1345 :
1346 : When doing a full accel cal we pass in a trim vector that has been
1347 : zeroed so the 3rd non-observable axis is reset
1348 : */
1349 4 : bool AP_InertialSensor::_calculate_trim(const Vector3f &accel_sample, Vector3f &trim)
1350 : {
1351 4 : Rotation rotation = ROTATION_NONE;
1352 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
1353 3 : AP_AHRS_View *view = AP::ahrs().get_view();
1354 3 : if (view != nullptr) {
1355 : // Use pitch to guess which axis the user is trying to trim
1356 : // 5 deg buffer to favor normal AHRS and avoid floating point funny business
1357 0 : if (fabsf(view->pitch) < (fabsf(AP::ahrs().get_pitch_rad())+radians(5)) ) {
1358 : // user is trying to calibrate view
1359 0 : rotation = view->get_rotation();
1360 0 : if (!is_zero(view->get_pitch_trim())) {
1361 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Cannot calibrate with Q_TRIM_PITCH set");
1362 0 : return false;
1363 : }
1364 : }
1365 : }
1366 : #endif
1367 :
1368 4 : Vector3f newtrim = trim;
1369 4 : switch (rotation) {
1370 4 : case ROTATION_NONE:
1371 4 : newtrim.y = atan2f(accel_sample.x, norm(accel_sample.y, accel_sample.z));
1372 4 : newtrim.x = atan2f(-accel_sample.y, -accel_sample.z);
1373 4 : break;
1374 :
1375 0 : case ROTATION_PITCH_90: {
1376 0 : newtrim.y = atan2f(accel_sample.z, norm(accel_sample.y, -accel_sample.x));
1377 0 : newtrim.z = atan2f(-accel_sample.y, accel_sample.x);
1378 0 : break;
1379 : }
1380 0 : default:
1381 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "unsupported trim rotation");
1382 0 : return false;
1383 : }
1384 8 : if (fabsf(newtrim.x) <= radians(HAL_INS_TRIM_LIMIT_DEG) &&
1385 8 : fabsf(newtrim.y) <= radians(HAL_INS_TRIM_LIMIT_DEG) &&
1386 4 : fabsf(newtrim.z) <= radians(HAL_INS_TRIM_LIMIT_DEG)) {
1387 4 : trim = newtrim;
1388 4 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Trim OK: roll=%.2f pitch=%.2f yaw=%.2f",
1389 : (double)degrees(trim.x),
1390 : (double)degrees(trim.y),
1391 : (double)degrees(trim.z));
1392 4 : return true;
1393 : }
1394 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "trim over maximum of 10 degrees");
1395 0 : return false;
1396 : }
1397 :
1398 : void
1399 1382 : AP_InertialSensor::init_gyro()
1400 : {
1401 1382 : _init_gyro();
1402 :
1403 : // save calibration
1404 1382 : _save_gyro_calibration();
1405 1382 : }
1406 :
1407 : // output GCS startup messages
1408 6024 : bool AP_InertialSensor::get_output_banner(uint8_t backend_id, char* banner, uint8_t banner_len)
1409 : {
1410 6024 : if (backend_id >= INS_MAX_BACKENDS || _backends[backend_id] == nullptr) {
1411 4016 : return false;
1412 : }
1413 :
1414 2008 : return _backends[backend_id]->get_output_banner(banner, banner_len);
1415 : }
1416 :
1417 : // accelerometer clipping reporting
1418 7784750 : uint32_t AP_InertialSensor::get_accel_clip_count(uint8_t instance) const
1419 : {
1420 7784750 : if (instance >= get_accel_count()) {
1421 1109838 : return 0;
1422 : }
1423 6674912 : return _accel_clip_count[instance];
1424 : }
1425 :
1426 : // get_gyro_health_all - return true if all gyros are healthy
1427 1253677 : bool AP_InertialSensor::get_gyro_health_all(void) const
1428 : {
1429 3834372 : for (uint8_t i=0; i<get_gyro_count(); i++) {
1430 2580695 : if (!get_gyro_health(i)) {
1431 0 : return false;
1432 : }
1433 : }
1434 : // return true if we have at least one gyro
1435 1253677 : return (get_gyro_count() > 0);
1436 : }
1437 :
1438 : // threshold in degrees/s to be consistent, consistent_time_sec duration for which
1439 : // gyros need to be consistent to be considered consistent
1440 64769 : bool AP_InertialSensor::gyros_consistent(uint8_t threshold) const
1441 : {
1442 :
1443 64769 : const uint8_t gyro_count = get_gyro_count();
1444 64769 : if (gyro_count <= 1) {
1445 0 : return true;
1446 : }
1447 :
1448 64769 : const Vector3f &prime_gyro_vec = get_gyro();
1449 194171 : for(uint8_t i=0; i<gyro_count; i++) {
1450 129758 : if (!use_gyro(i)) {
1451 0 : continue;
1452 : }
1453 : // get next gyro vector
1454 129758 : const Vector3f &gyro_vec = get_gyro(i);
1455 129758 : const Vector3f vec_diff = gyro_vec - prime_gyro_vec;
1456 : // allow for up to threshold degrees/s difference
1457 129758 : if (vec_diff.length() > radians(threshold)) {
1458 : // this sensor disagrees with the primary sensor, so
1459 : // gyros are inconsistent:
1460 356 : return false;
1461 : }
1462 : }
1463 :
1464 64413 : return true;
1465 : }
1466 :
1467 : // gyro_calibration_ok_all - returns true if all gyros were calibrated successfully
1468 1253642 : bool AP_InertialSensor::gyro_calibrated_ok_all() const
1469 : {
1470 3833951 : for (uint8_t i=0; i<get_gyro_count(); i++) {
1471 2580509 : if (!gyro_calibrated_ok(i)) {
1472 200 : return false;
1473 : }
1474 : }
1475 2433459 : for (uint8_t i=get_gyro_count(); i<INS_MAX_INSTANCES; i++) {
1476 1180017 : if (_gyro_id(i) != 0) {
1477 : // missing gyro
1478 0 : return false;
1479 : }
1480 : }
1481 1253442 : return (get_gyro_count() > 0);
1482 : }
1483 :
1484 : // Prearm check to verify that we have sane sched loop rates set based on Gyro backend rates
1485 45825 : bool AP_InertialSensor::pre_arm_check_gyro_backend_rate_hz(char* fail_msg, uint16_t fail_msg_len) const
1486 : {
1487 : #if AP_SCHEDULER_ENABLED
1488 45825 : const auto gyro_count = get_gyro_count();
1489 45825 : const auto threshold = 1.8 * _loop_rate;
1490 137593 : for (uint8_t i=0; i<gyro_count; i++) {
1491 91768 : if (!_use(i) || _backends[i] == nullptr) {
1492 0 : continue;
1493 : }
1494 91768 : const auto rate_hz = _backends[i]->get_gyro_backend_rate_hz();
1495 91768 : if (rate_hz < threshold && (AP_HAL::Device::devid_get_devtype(_gyro_id(i)) != AP_InertialSensor_Backend::DEVTYPE_SERIAL)) {
1496 0 : hal.util->snprintf(fail_msg, fail_msg_len, "Gyro %d rate %dHz < loop rate*1.8 %dHz",
1497 : i, int(rate_hz), int(threshold));
1498 0 : return false;
1499 : }
1500 : }
1501 : #endif
1502 45825 : return true;
1503 : }
1504 :
1505 : // return true if gyro instance should be used (must be healthy and have it's use parameter set to 1)
1506 123762289 : bool AP_InertialSensor::use_gyro(uint8_t instance) const
1507 : {
1508 123762289 : if (instance >= INS_MAX_INSTANCES) {
1509 0 : return false;
1510 : }
1511 :
1512 123762289 : return (get_gyro_health(instance) && _use(instance));
1513 : }
1514 :
1515 : // look up the backend that owns the given gyro instance, or nullptr
1516 246937710 : const AP_InertialSensor_Backend *AP_InertialSensor::_find_gyro_backend(uint8_t instance) const
1517 : {
1518 493875420 : for (uint8_t i = 0; i < _backend_count; i++) {
1519 411714042 : if (_backends[i] != nullptr && _backends[i]->get_gyro_instance() == instance) {
1520 164776332 : return _backends[i];
1521 : }
1522 : }
1523 82161378 : return nullptr;
1524 : }
1525 :
1526 123468855 : float AP_InertialSensor::get_gyro_bias_limit_rads(uint8_t instance) const
1527 : {
1528 123468855 : const auto *backend = _find_gyro_backend(instance);
1529 123468855 : if (backend == nullptr) {
1530 : // fall back to the legacy default if no backend has claimed this instance yet
1531 41080689 : return 0.5f;
1532 : }
1533 82388166 : return backend->gyro_bias_limit_rads();
1534 : }
1535 :
1536 123468855 : float AP_InertialSensor::get_gyro_bias_init_dps(uint8_t instance) const
1537 : {
1538 123468855 : const auto *backend = _find_gyro_backend(instance);
1539 123468855 : if (backend == nullptr) {
1540 41080689 : return 2.5f;
1541 : }
1542 82388166 : return backend->gyro_bias_init_dps();
1543 : }
1544 :
1545 : // get_accel_health_all - return true if all accels are healthy
1546 1253647 : bool AP_InertialSensor::get_accel_health_all(void) const
1547 : {
1548 3834282 : for (uint8_t i=0; i<get_accel_count(); i++) {
1549 2580635 : if (!get_accel_health(i)) {
1550 0 : return false;
1551 : }
1552 : }
1553 : // return true if we have at least one accel
1554 1253647 : return (get_accel_count() > 0);
1555 : }
1556 :
1557 : // accel_error_threshold in m/s/s to be consistent
1558 64769 : bool AP_InertialSensor::accels_consistent(float accel_error_threshold) const
1559 : {
1560 64769 : const uint8_t accel_count = get_accel_count();
1561 64769 : if (accel_count <= 1) {
1562 0 : return true;
1563 : }
1564 :
1565 64769 : const Vector3f &prime_accel_vec = get_accel();
1566 193962 : for(uint8_t i=0; i<accel_count; i++) {
1567 129758 : if (!use_accel(i)) {
1568 0 : continue;
1569 : }
1570 : // get next accel vector
1571 129758 : const Vector3f &accel_vec = get_accel(i);
1572 129758 : Vector3f vec_diff = accel_vec - prime_accel_vec;
1573 : // allow for user-defined difference, threshold m/s/s. Has to pass in last consistent_time_sec seconds
1574 129758 : float threshold = accel_error_threshold;
1575 129758 : if (i >= 2) {
1576 : /*
1577 : we allow for a higher threshold for IMU3 as it
1578 : runs at a different temperature to IMU1/IMU2,
1579 : and is not used for accel data in the EKF
1580 : */
1581 220 : threshold *= 3;
1582 : }
1583 :
1584 : // EKF is less sensitive to Z-axis error
1585 129758 : vec_diff.z *= 0.5f;
1586 :
1587 129758 : if (vec_diff.length() > threshold) {
1588 : // this sensor disagrees with the primary sensor, so
1589 : // accels are inconsistent:
1590 565 : return false;
1591 : }
1592 : }
1593 64204 : return true;
1594 : }
1595 :
1596 : #if HAL_GCS_ENABLED && AP_AHRS_ENABLED
1597 : /*
1598 : calculate the trim_roll and trim_pitch. This is used for redoing the
1599 : trim without needing a full accel cal
1600 : */
1601 3 : MAV_RESULT AP_InertialSensor::calibrate_trim()
1602 : {
1603 : // exit immediately if calibration is already in progress
1604 3 : if (calibrating()) {
1605 0 : return MAV_RESULT_TEMPORARILY_REJECTED;
1606 : }
1607 :
1608 : // reject any time we've done a calibration recently
1609 3 : const uint32_t now = AP_HAL::millis();
1610 3 : if ((now - last_accel_cal_ms) < 5000) {
1611 0 : return MAV_RESULT_TEMPORARILY_REJECTED;
1612 : }
1613 :
1614 3 : if (!calibrate_gyros()) {
1615 0 : return MAV_RESULT_FAILED;
1616 : }
1617 :
1618 3 : AP_AHRS &ahrs = AP::ahrs();
1619 3 : Vector3f trim_rad = ahrs.get_trim();
1620 :
1621 3 : const uint8_t update_dt_milliseconds = (uint8_t)(1000.0f/get_loop_rate_hz()+0.5f);
1622 3 : Vector3f level_sample;
1623 3 : uint32_t num_samples = 0;
1624 :
1625 3 : _trimming_accel = true;
1626 :
1627 :
1628 : // wait 100ms for ins filter to rise
1629 18 : for (uint8_t k=0; k<100/update_dt_milliseconds; k++) {
1630 15 : wait_for_sample();
1631 15 : update();
1632 15 : hal.scheduler->delay(update_dt_milliseconds);
1633 : }
1634 :
1635 63 : while (num_samples < 400/update_dt_milliseconds) {
1636 60 : wait_for_sample();
1637 : // read samples from ins
1638 60 : update();
1639 : // capture sample
1640 60 : Vector3f samp;
1641 60 : samp = get_accel(0);
1642 60 : level_sample += samp;
1643 60 : if (!get_accel_health(0)) {
1644 0 : goto failed;
1645 : }
1646 60 : hal.scheduler->delay(update_dt_milliseconds);
1647 60 : num_samples++;
1648 : }
1649 3 : level_sample /= num_samples;
1650 :
1651 3 : if (!_calculate_trim(level_sample, trim_rad)) {
1652 0 : goto failed;
1653 : }
1654 :
1655 : // reset ahrs's trim to suggested values from calibration routine
1656 3 : ahrs.set_trim(trim_rad);
1657 :
1658 3 : last_accel_cal_ms = AP_HAL::millis();
1659 3 : _trimming_accel = false;
1660 3 : return MAV_RESULT_ACCEPTED;
1661 :
1662 0 : failed:
1663 0 : last_accel_cal_ms = AP_HAL::millis();
1664 0 : _trimming_accel = false;
1665 0 : return MAV_RESULT_FAILED;
1666 : }
1667 : #endif // HAL_GCS_ENABLED && AP_AHRS_ENABLED
1668 :
1669 : /*
1670 : check if the accelerometers are calibrated in 3D and that current number of accels matched number when calibrated
1671 : */
1672 1095628 : bool AP_InertialSensor::accel_calibrated_ok_all() const
1673 : {
1674 : // check each accelerometer has offsets saved
1675 3286796 : for (uint8_t i=0; i<get_accel_count(); i++) {
1676 2261237 : if (!_accel_id_ok[i]) {
1677 0 : return false;
1678 : }
1679 : // exactly 0.0 offset is extremely unlikely
1680 2261237 : if (_accel_offset(i).get().is_zero()) {
1681 70069 : return false;
1682 : }
1683 : // zero scaling also indicates not calibrated
1684 2191168 : if (_accel_scale(i).get().is_zero()) {
1685 0 : return false;
1686 : }
1687 : }
1688 2045070 : for (uint8_t i=get_accel_count(); i<INS_MAX_INSTANCES; i++) {
1689 1019511 : if (_accel_id(i) != 0) {
1690 : // missing accel
1691 0 : return false;
1692 : }
1693 : }
1694 :
1695 : // check calibrated accels matches number of accels (no unused accels should have offsets or scaling)
1696 1025559 : if (get_accel_count() < INS_MAX_INSTANCES) {
1697 2039021 : for (uint8_t i=get_accel_count(); i<INS_MAX_INSTANCES; i++) {
1698 1019511 : const Vector3f &scaling = _accel_scale(i).get();
1699 1019511 : bool have_scaling = (!is_zero(scaling.x) && !is_equal(scaling.x,1.0f)) || (!is_zero(scaling.y) && !is_equal(scaling.y,1.0f)) || (!is_zero(scaling.z) && !is_equal(scaling.z,1.0f));
1700 1019511 : bool have_offsets = !_accel_offset(i).get().is_zero();
1701 1019511 : if (have_scaling || have_offsets) {
1702 1 : return false;
1703 : }
1704 : }
1705 : }
1706 :
1707 : // if we got this far the accelerometers must have been calibrated
1708 1025558 : return true;
1709 : }
1710 :
1711 : // return true if accel instance should be used (must be healthy and have it's use parameter set to 1)
1712 213682031 : bool AP_InertialSensor::use_accel(uint8_t instance) const
1713 : {
1714 213682031 : if (instance >= INS_MAX_INSTANCES) {
1715 0 : return false;
1716 : }
1717 :
1718 213682031 : return (get_accel_health(instance) && _use(instance));
1719 : }
1720 :
1721 : void
1722 1382 : AP_InertialSensor::_init_gyro()
1723 : {
1724 1382 : uint8_t num_gyros = MIN(get_gyro_count(), INS_MAX_INSTANCES);
1725 1382 : Vector3f last_average[INS_MAX_INSTANCES], best_avg[INS_MAX_INSTANCES];
1726 1382 : Vector3f new_gyro_offset[INS_MAX_INSTANCES];
1727 : float best_diff[INS_MAX_INSTANCES];
1728 : bool converged[INS_MAX_INSTANCES];
1729 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
1730 1382 : float start_temperature[INS_MAX_INSTANCES] {};
1731 : #endif
1732 :
1733 : // exit immediately if calibration is already in progress
1734 1382 : if (calibrating()) {
1735 0 : return;
1736 : }
1737 :
1738 : // record we are calibrating
1739 1382 : _calibrating_gyro = true;
1740 :
1741 : // flash leds to tell user to keep the IMU still
1742 1382 : AP_Notify::flags.initialising = true;
1743 :
1744 : // cold start
1745 1382 : DEV_PRINTF("Init Gyro");
1746 :
1747 : // the gyro backend leaves the board rotation off while _calibrating_gyro
1748 : // is set, so the samples below are already in board frame
1749 :
1750 : // remove existing gyro offsets
1751 4153 : for (uint8_t k=0; k<num_gyros; k++) {
1752 2771 : _gyro_offset(k).set(Vector3f());
1753 2771 : new_gyro_offset[k].zero();
1754 2771 : best_diff[k] = -1.f;
1755 2771 : last_average[k].zero();
1756 2771 : converged[k] = false;
1757 : }
1758 :
1759 8292 : for(int8_t c = 0; c < 5; c++) {
1760 6910 : hal.scheduler->delay(5);
1761 6910 : update();
1762 : }
1763 :
1764 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
1765 : // get start temperature. gyro cal usually happens when the board
1766 : // has just been powered on, so the temperature may be changing
1767 : // rapidly. We use the average between start and end temperature
1768 : // as the calibration temperature to minimise errors
1769 4153 : for (uint8_t k=0; k<num_gyros; k++) {
1770 2771 : start_temperature[k] = get_temperature(k);
1771 : }
1772 : #endif
1773 :
1774 : // the strategy is to average 50 points over 0.5 seconds, then do it
1775 : // again and see if the 2nd average is within a small margin of
1776 : // the first
1777 :
1778 1382 : uint8_t num_converged = 0;
1779 :
1780 : // we try to get a good calibration estimate for up to 30 seconds
1781 : // if the gyros are stable, we should get it in 1 second
1782 4281 : for (int16_t j = 0; j <= 30*4 && num_converged < num_gyros; j++) {
1783 2899 : Vector3f gyro_sum[INS_MAX_INSTANCES], gyro_avg[INS_MAX_INSTANCES], gyro_diff[INS_MAX_INSTANCES];
1784 2899 : Vector3f accel_start;
1785 : float diff_norm[INS_MAX_INSTANCES];
1786 : uint8_t i;
1787 :
1788 2899 : EXPECT_DELAY_MS(1000);
1789 :
1790 2899 : memset(diff_norm, 0, sizeof(diff_norm));
1791 :
1792 2899 : DEV_PRINTF("*");
1793 :
1794 8750 : for (uint8_t k=0; k<num_gyros; k++) {
1795 5851 : gyro_sum[k].zero();
1796 : }
1797 2899 : accel_start = get_accel(0);
1798 147849 : for (i=0; i<50; i++) {
1799 144950 : update();
1800 437500 : for (uint8_t k=0; k<num_gyros; k++) {
1801 292550 : gyro_sum[k] += get_gyro(k);
1802 : }
1803 144950 : hal.scheduler->delay(5);
1804 : }
1805 :
1806 2899 : Vector3f accel_diff = get_accel(0) - accel_start;
1807 2899 : if (accel_diff.length() > 0.2f) {
1808 : // the accelerometers changed during the gyro sum. Skip
1809 : // this sample. This copes with doing gyro cal on a
1810 : // steadily moving platform. The value 0.2 corresponds
1811 : // with around 5 degrees/second of rotation.
1812 135 : continue;
1813 : }
1814 :
1815 8306 : for (uint8_t k=0; k<num_gyros; k++) {
1816 5542 : gyro_avg[k] = gyro_sum[k] / i;
1817 5542 : gyro_diff[k] = last_average[k] - gyro_avg[k];
1818 5542 : diff_norm[k] = gyro_diff[k].length();
1819 : }
1820 :
1821 8306 : for (uint8_t k=0; k<num_gyros; k++) {
1822 5542 : if (best_diff[k] < 0) {
1823 2771 : best_diff[k] = diff_norm[k];
1824 2771 : best_avg[k] = gyro_avg[k];
1825 2771 : } else if (gyro_diff[k].length() < radians(GYRO_INIT_MAX_DIFF_DPS)) {
1826 : // we want the average to be within 0.1 bit, which is 0.04 degrees/s
1827 2771 : last_average[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
1828 2771 : if (!converged[k] || last_average[k].length() < new_gyro_offset[k].length()) {
1829 2771 : new_gyro_offset[k] = last_average[k];
1830 : }
1831 2771 : if (!converged[k]) {
1832 2771 : converged[k] = true;
1833 2771 : num_converged++;
1834 : }
1835 0 : } else if (diff_norm[k] < best_diff[k]) {
1836 0 : best_diff[k] = diff_norm[k];
1837 0 : best_avg[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
1838 : }
1839 5542 : last_average[k] = gyro_avg[k];
1840 : }
1841 2899 : }
1842 :
1843 : // we've kept the user waiting long enough - use the best pair we
1844 : // found so far
1845 1382 : DEV_PRINTF("\n");
1846 4153 : for (uint8_t k=0; k<num_gyros; k++) {
1847 2771 : if (!converged[k]) {
1848 0 : DEV_PRINTF("gyro[%u] did not converge: diff=%f dps (expected < %f)\n",
1849 : (unsigned)k,
1850 : (double)degrees(best_diff[k]),
1851 : (double)GYRO_INIT_MAX_DIFF_DPS);
1852 0 : _gyro_offset(k).set(best_avg[k]);
1853 : // flag calibration as failed for this gyro
1854 0 : _gyro_cal_ok[k] = false;
1855 : } else {
1856 2771 : _gyro_cal_ok[k] = true;
1857 2771 : _gyro_offset(k).set(new_gyro_offset[k]);
1858 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
1859 2771 : caltemp_gyro(k).set(0.5 * (get_temperature(k) + start_temperature[k]));
1860 : #endif
1861 : }
1862 : }
1863 :
1864 : // record calibration complete
1865 1382 : _calibrating_gyro = false;
1866 :
1867 : // stop flashing leds
1868 1382 : AP_Notify::flags.initialising = false;
1869 1382 : AP_Notify::flags.gyro_calibrated = true;
1870 : }
1871 :
1872 : // save parameters to eeprom
1873 1382 : void AP_InertialSensor::_save_gyro_calibration()
1874 : {
1875 4153 : for (uint8_t i=0; i<_gyro_count; i++) {
1876 2771 : _gyro_offset(i).save();
1877 2771 : _gyro_id(i).save();
1878 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
1879 2771 : caltemp_gyro(i).save();
1880 : #endif
1881 : }
1882 2757 : for (uint8_t i=_gyro_count; i<INS_MAX_INSTANCES; i++) {
1883 1375 : _gyro_offset(i).set_and_save(Vector3f());
1884 1375 : _gyro_id(i).set_and_save(0);
1885 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
1886 1375 : caltemp_gyro(i).set_and_save_ifchanged(-300);
1887 : #endif
1888 : }
1889 1382 : }
1890 :
1891 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
1892 : /*
1893 : update harmonic notch parameters
1894 : */
1895 2103528 : void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool converging, float gyro_rate)
1896 : {
1897 2103528 : if (!is_equal(last_bandwidth_hz[instance], params.bandwidth_hz()) ||
1898 2103441 : !is_equal(last_attenuation_dB[instance], params.attenuation_dB()) ||
1899 4206983 : !is_equal(last_center_freq_hz[instance], params.center_freq_hz()) ||
1900 : converging) {
1901 253779 : filter[instance].init(gyro_rate, params);
1902 253772 : last_center_freq_hz[instance] = params.center_freq_hz();
1903 253772 : last_bandwidth_hz[instance] = params.bandwidth_hz();
1904 253771 : last_attenuation_dB[instance] = params.attenuation_dB();
1905 : }
1906 :
1907 2103532 : if (params.tracking_mode() != HarmonicNotchDynamicMode::Fixed) {
1908 1873038 : if (num_calculated_notch_frequencies > 1) {
1909 998386 : filter[instance].update(num_calculated_notch_frequencies, calculated_notch_freq_hz);
1910 : } else {
1911 874652 : filter[instance].update(calculated_notch_freq_hz[0]);
1912 : }
1913 : }
1914 2103538 : }
1915 : #endif
1916 :
1917 : // notify IMUs of the new primary
1918 61 : void AP_InertialSensor::set_primary(uint8_t instance)
1919 : {
1920 61 : _primary = instance;
1921 61 : }
1922 :
1923 : /*
1924 : update gyro and accel values from backends
1925 : */
1926 41843605 : void AP_InertialSensor::update(void)
1927 : {
1928 : // during initialisation update() may be called without
1929 : // wait_for_sample(), and a wait is implied
1930 41843605 : wait_for_sample();
1931 :
1932 167374420 : for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
1933 : // health flags are deliberately not cleared here: they are read
1934 : // from other threads, and clearing before the backends republish
1935 : // leaves a window in which a healthy sensor reads unhealthy
1936 125530815 : _delta_velocity_valid[i] = false;
1937 125530815 : _delta_angle_valid[i] = false;
1938 : }
1939 125610556 : for (uint8_t i=0; i<_backend_count; i++) {
1940 83766951 : _backends[i]->update();
1941 : }
1942 :
1943 41843605 : if (!_startup_error_counts_set) {
1944 3355572 : for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
1945 2516679 : _accel_startup_error_count[i] = _accel_error_count[i];
1946 2516679 : _gyro_startup_error_count[i] = _gyro_error_count[i];
1947 : }
1948 :
1949 838893 : if (_startup_ms == 0) {
1950 2088 : _startup_ms = AP_HAL::millis();
1951 836805 : } else if (AP_HAL::millis()-_startup_ms > 2000) {
1952 1970 : _startup_error_counts_set = true;
1953 : }
1954 : }
1955 :
1956 167374420 : for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
1957 125530815 : if (_accel_error_count[i] < _accel_startup_error_count[i]) {
1958 0 : _accel_startup_error_count[i] = _accel_error_count[i];
1959 : }
1960 125530815 : if (_gyro_error_count[i] < _gyro_startup_error_count[i]) {
1961 0 : _gyro_startup_error_count[i] = _gyro_error_count[i];
1962 : }
1963 : }
1964 :
1965 : // adjust health status if a sensor has a non-zero error count
1966 : // but another sensor doesn't.
1967 41843605 : bool have_zero_accel_error_count = false;
1968 41843605 : bool have_zero_gyro_error_count = false;
1969 167374420 : for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
1970 125530815 : if (_accel_healthy[i] && _accel_error_count[i] <= _accel_startup_error_count[i]) {
1971 83766540 : have_zero_accel_error_count = true;
1972 : }
1973 125530815 : if (_gyro_healthy[i] && _gyro_error_count[i] <= _gyro_startup_error_count[i]) {
1974 83766540 : have_zero_gyro_error_count = true;
1975 : }
1976 : }
1977 :
1978 167374420 : for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
1979 125530815 : if (_gyro_healthy[i] && _gyro_error_count[i] > _gyro_startup_error_count[i] && have_zero_gyro_error_count) {
1980 : // we prefer not to use a gyro that has had errors
1981 0 : _gyro_healthy[i] = false;
1982 : }
1983 125530815 : if (_accel_healthy[i] && _accel_error_count[i] > _accel_startup_error_count[i] && have_zero_accel_error_count) {
1984 : // we prefer not to use a accel that has had errors
1985 0 : _accel_healthy[i] = false;
1986 : }
1987 : }
1988 :
1989 : // set primary to first healthy accel and gyro
1990 41844016 : for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
1991 41844016 : if (_gyro_healthy[i] && _use(i)) {
1992 41843605 : _first_usable_gyro = i;
1993 : #if !AP_AHRS_ENABLED
1994 : _primary = _first_usable_gyro;
1995 : #endif
1996 41843605 : break;
1997 : }
1998 : }
1999 41844016 : for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
2000 41844016 : if (_accel_healthy[i] && _use(i)) {
2001 41843605 : _first_usable_accel = i;
2002 41843605 : break;
2003 : }
2004 : }
2005 :
2006 41843605 : _last_update_usec = AP_HAL::micros();
2007 :
2008 41843605 : _have_sample = false;
2009 :
2010 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
2011 41843605 : if (tcal_learning && !temperature_cal_running()) {
2012 1 : AP_Notify::flags.temp_cal_running = false;
2013 1 : AP_Notify::events.temp_cal_saved = 1;
2014 1 : tcal_learning = false;
2015 1 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL finished all IMUs");
2016 : }
2017 : #endif
2018 : #if AP_SERIALMANAGER_IMUOUT_ENABLED
2019 41843605 : if (uart.imu_out_uart) {
2020 0 : send_uart_data();
2021 : }
2022 : #endif
2023 41843605 : }
2024 :
2025 : /*
2026 : wait for a sample to be available. This is the function that
2027 : determines the timing of the main loop in ardupilot.
2028 :
2029 : Ideally this function would return at exactly the rate given by the
2030 : sample_rate argument given to AP_InertialSensor::init().
2031 :
2032 : The key output of this function is _delta_time, which is the time
2033 : over which the gyro and accel integration will happen for this
2034 : sample. We want that to be a constant time if possible, but if
2035 : delays occur we need to cope with them. The long term sum of
2036 : _delta_time should be exactly equal to the wall clock elapsed time
2037 : */
2038 83530666 : void AP_InertialSensor::wait_for_sample(void)
2039 : {
2040 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
2041 83530666 : auto *sitl = AP::sitl();
2042 83530666 : if (sitl == nullptr) {
2043 0 : hal.scheduler->delay_microseconds(1000);
2044 0 : return;
2045 : }
2046 : #endif
2047 83530666 : if (_have_sample) {
2048 : // the user has called wait_for_sample() again without
2049 : // consuming the sample with update()
2050 41687060 : return;
2051 : }
2052 :
2053 41843606 : uint32_t now = AP_HAL::micros();
2054 :
2055 41843606 : if (_next_sample_usec == 0 && _delta_time <= 0) {
2056 : // this is the first call to wait_for_sample()
2057 3462 : _last_sample_usec = now - _sample_period_usec;
2058 3462 : _next_sample_usec = now + _sample_period_usec;
2059 3462 : goto check_sample;
2060 : }
2061 :
2062 : // see how long it is till the next sample is due
2063 41840144 : if (_next_sample_usec - now <=_sample_period_usec) {
2064 : // we're ahead on time, schedule next sample at expected period
2065 41666743 : uint32_t wait_usec = _next_sample_usec - now;
2066 41666743 : hal.scheduler->delay_microseconds_boost(wait_usec);
2067 41666742 : uint32_t now2 = AP_HAL::micros();
2068 41666742 : if (now2+100 < _next_sample_usec) {
2069 : timing_printf("shortsleep %u\n", (unsigned)(_next_sample_usec-now2));
2070 : }
2071 41666742 : if (now2 > _next_sample_usec+400) {
2072 : timing_printf("longsleep %u wait_usec=%u\n",
2073 : (unsigned)(now2-_next_sample_usec),
2074 : (unsigned)wait_usec);
2075 : }
2076 41666742 : _next_sample_usec += _sample_period_usec;
2077 173401 : } else if (now - _next_sample_usec < _sample_period_usec/8) {
2078 : // we've overshot, but only by a small amount, keep on
2079 : // schedule with no delay
2080 : timing_printf("overshoot1 %u\n", (unsigned)(now-_next_sample_usec));
2081 8892 : _next_sample_usec += _sample_period_usec;
2082 : } else {
2083 : // we've overshot by a larger amount, re-zero scheduling with
2084 : // no delay
2085 : timing_printf("overshoot2 %u\n", (unsigned)(now-_next_sample_usec));
2086 164509 : _next_sample_usec = now + _sample_period_usec;
2087 : }
2088 :
2089 41843605 : check_sample:
2090 : // now we wait until we have the gyro and accel samples we need
2091 41843605 : uint8_t gyro_available_mask = 0;
2092 41843605 : uint8_t accel_available_mask = 0;
2093 41843605 : uint32_t wait_counter = 0;
2094 : // allow to wait for up to 1/3 of the loop time for samples from all
2095 : // IMUs to come in
2096 41843605 : const uint8_t wait_per_loop = 100;
2097 41843605 : const uint8_t wait_counter_limit = uint32_t(_loop_delta_t * 1.0e6) / (3*wait_per_loop);
2098 :
2099 : while (true) {
2100 127718331 : for (uint8_t i=0; i<_backend_count; i++) {
2101 : // this is normally a nop, but can be used by backends
2102 : // that don't accumulate samples on a timer
2103 85346920 : _backends[i]->accumulate();
2104 : }
2105 :
2106 127718331 : for (uint8_t i=0; i<_gyro_count; i++) {
2107 85346920 : if (_new_gyro_data[i]) {
2108 84804856 : const uint8_t imask = (1U<<i);
2109 84804856 : gyro_available_mask |= imask;
2110 84804856 : if (_use(i)) {
2111 84804856 : _gyro_wait_mask |= imask;
2112 : } else {
2113 0 : _gyro_wait_mask &= ~imask;
2114 : }
2115 : }
2116 : }
2117 127718331 : for (uint8_t i=0; i<_accel_count; i++) {
2118 85346920 : if (_new_accel_data[i]) {
2119 84803789 : const uint8_t imask = (1U<<i);
2120 84803789 : accel_available_mask |= imask;
2121 84803789 : if (_use(i)) {
2122 84803789 : _accel_wait_mask |= imask;
2123 : } else {
2124 0 : _accel_wait_mask &= ~imask;
2125 : }
2126 : }
2127 : }
2128 :
2129 : // we wait for up to 1/3 of the loop time to get all of the required
2130 : // accel and gyro samples. After that we accept at least
2131 : // one of each
2132 42371411 : if (wait_counter < wait_counter_limit) {
2133 42371411 : if (gyro_available_mask &&
2134 42368144 : ((gyro_available_mask & _gyro_wait_mask) == _gyro_wait_mask) &&
2135 41843605 : accel_available_mask &&
2136 41843605 : ((accel_available_mask & _accel_wait_mask) == _accel_wait_mask)) {
2137 41843605 : break;
2138 : }
2139 : } else {
2140 0 : if (gyro_available_mask && accel_available_mask) {
2141 : // reset the wait mask so we don't keep delaying
2142 : // for a dead IMU on the next loop. As soon as it
2143 : // comes back we will start waiting on it again
2144 0 : _gyro_wait_mask &= gyro_available_mask;
2145 0 : _accel_wait_mask &= accel_available_mask;
2146 0 : break;
2147 : }
2148 : }
2149 :
2150 527806 : hal.scheduler->delay_microseconds_boost(wait_per_loop);
2151 527806 : wait_counter++;
2152 527806 : }
2153 :
2154 41843605 : now = AP_HAL::micros();
2155 41843605 : _delta_time = (now - _last_sample_usec) * 1.0e-6f;
2156 41843605 : _last_sample_usec = now;
2157 :
2158 : #if 0
2159 : {
2160 : static uint64_t delta_time_sum;
2161 : static uint16_t counter;
2162 : if (delta_time_sum == 0) {
2163 : delta_time_sum = _sample_period_usec;
2164 : }
2165 : delta_time_sum += _delta_time * 1.0e6f;
2166 : if (counter++ == 400) {
2167 : counter = 0;
2168 : hal.console->printf("now=%lu _delta_time_sum=%lu diff=%ld\n",
2169 : (unsigned long)now,
2170 : (unsigned long)delta_time_sum,
2171 : (long)(now - delta_time_sum));
2172 : }
2173 : }
2174 : #endif
2175 :
2176 41843605 : _have_sample = true;
2177 : }
2178 :
2179 :
2180 : /*
2181 : get delta angles
2182 : */
2183 124238558 : bool AP_InertialSensor::get_delta_angle(uint8_t i, Vector3f &delta_angle, float &delta_angle_dt) const
2184 : {
2185 124238558 : if (_delta_angle_valid[i] && _delta_angle_dt[i] > 0) {
2186 124238557 : delta_angle_dt = _delta_angle_dt[i];
2187 : } else {
2188 1 : delta_angle_dt = get_delta_time();
2189 : }
2190 124238558 : delta_angle_dt = MIN(delta_angle_dt, _loop_delta_t_max);
2191 :
2192 124238558 : if (_delta_angle_valid[i]) {
2193 124238558 : delta_angle = _delta_angle[i];
2194 124238558 : return true;
2195 0 : } else if (get_gyro_health(i)) {
2196 : // provide delta angle from raw gyro, so we use the same code
2197 : // at higher level
2198 0 : delta_angle = get_gyro(i) * get_delta_time();
2199 0 : return true;
2200 : }
2201 0 : return false;
2202 : }
2203 :
2204 : /*
2205 : get delta velocity if available
2206 : */
2207 207572648 : bool AP_InertialSensor::get_delta_velocity(uint8_t i, Vector3f &delta_velocity, float &delta_velocity_dt) const
2208 : {
2209 207572648 : if (_delta_velocity_valid[i]) {
2210 207572648 : delta_velocity_dt = _delta_velocity_dt[i];
2211 : } else {
2212 0 : delta_velocity_dt = get_delta_time();
2213 : }
2214 207572648 : delta_velocity_dt = MIN(delta_velocity_dt, _loop_delta_t_max);
2215 :
2216 207572648 : if (_delta_velocity_valid[i]) {
2217 207572648 : delta_velocity = _delta_velocity[i];
2218 207572648 : return true;
2219 0 : } else if (get_accel_health(i)) {
2220 0 : delta_velocity = get_accel(i) * get_delta_time();
2221 0 : return true;
2222 : }
2223 0 : return false;
2224 : }
2225 :
2226 : /*
2227 : * Get an AuxiliaryBus of N @instance of backend identified by @backend_id
2228 : */
2229 0 : AuxiliaryBus *AP_InertialSensor::get_auxiliary_bus(int16_t backend_id, uint8_t instance)
2230 : {
2231 0 : detect_backends();
2232 :
2233 0 : AP_InertialSensor_Backend *backend = _find_backend(backend_id, instance);
2234 0 : if (backend == nullptr) {
2235 0 : return nullptr;
2236 : }
2237 :
2238 0 : return backend->get_auxiliary_bus();
2239 : }
2240 :
2241 : // calculate vibration levels and check for accelerometer clipping (called by a backends)
2242 275741651 : void AP_InertialSensor::calc_vibration_and_clipping(uint8_t instance, const Vector3f &accel, float dt)
2243 : {
2244 : // check for clipping
2245 275741651 : if (_backends[instance] == nullptr) {
2246 0 : return;
2247 : }
2248 551483167 : if (fabsf(accel.x) > _backends[instance]->get_clip_limit() ||
2249 551483177 : fabsf(accel.y) > _backends[instance]->get_clip_limit() ||
2250 275741511 : fabsf(accel.z) > _backends[instance]->get_clip_limit()) {
2251 146 : _accel_clip_count[instance]++;
2252 : }
2253 :
2254 : // calculate vibration levels
2255 275741681 : if (instance < INS_VIBRATION_CHECK_INSTANCES) {
2256 : // filter accel at 5hz
2257 275741641 : Vector3f accel_filt = _accel_vibe_floor_filter[instance].apply(accel, dt);
2258 :
2259 : // calc difference from this sample and 5hz filtered value, square and filter at 2hz
2260 275741666 : Vector3f accel_diff = (accel - accel_filt);
2261 275741753 : accel_diff.x *= accel_diff.x;
2262 275741753 : accel_diff.y *= accel_diff.y;
2263 275741753 : accel_diff.z *= accel_diff.z;
2264 275741753 : _accel_vibe_filter[instance].apply(accel_diff, dt);
2265 : }
2266 : }
2267 :
2268 : // peak hold detector for slower mechanisms to detect spikes
2269 275741522 : void AP_InertialSensor::set_accel_peak_hold(uint8_t instance, const Vector3f &accel)
2270 : {
2271 275741522 : if (instance != _first_usable_accel) {
2272 : // we only record for primary accel
2273 123294726 : return;
2274 : }
2275 152446796 : uint32_t now = AP_HAL::millis();
2276 :
2277 : // negative x peak(min) hold detector
2278 152447164 : if (accel.x < _peak_hold_state.accel_peak_hold_neg_x ||
2279 138565275 : _peak_hold_state.accel_peak_hold_neg_x_age <= now) {
2280 14045337 : _peak_hold_state.accel_peak_hold_neg_x = accel.x;
2281 14045337 : _peak_hold_state.accel_peak_hold_neg_x_age = now + AP_INERTIAL_SENSOR_ACCEL_PEAK_DETECT_TIMEOUT_MS;
2282 : }
2283 : }
2284 :
2285 : // retrieve latest calculated vibration levels
2286 5990416 : Vector3f AP_InertialSensor::get_vibration_levels(uint8_t instance) const
2287 : {
2288 5990416 : Vector3f vibe;
2289 5990416 : if (instance < INS_VIBRATION_CHECK_INSTANCES) {
2290 5990416 : vibe = _accel_vibe_filter[instance].get();
2291 5990416 : vibe.x = safe_sqrt(vibe.x);
2292 5990416 : vibe.y = safe_sqrt(vibe.y);
2293 5990416 : vibe.z = safe_sqrt(vibe.z);
2294 : }
2295 5990416 : return vibe;
2296 : }
2297 :
2298 : // check for vibration movement. Return true if all axis show nearly zero movement
2299 437656 : bool AP_InertialSensor::is_still()
2300 : {
2301 437656 : Vector3f vibe = get_vibration_levels();
2302 437656 : return (vibe.x < _still_threshold) &&
2303 591937 : (vibe.y < _still_threshold) &&
2304 591937 : (vibe.z < _still_threshold);
2305 : }
2306 :
2307 : // return true if we are in a calibration
2308 1122532 : bool AP_InertialSensor::calibrating() const
2309 : {
2310 1122532 : if (_calibrating_accel || _calibrating_gyro || _trimming_accel) {
2311 10 : return true;
2312 : }
2313 : #if HAL_INS_ACCELCAL_ENABLED
2314 1122522 : if (_acal && _acal->running()) {
2315 513 : return true;
2316 : }
2317 : #endif
2318 1122009 : return false;
2319 : }
2320 :
2321 : /// calibrating - returns true if a temperature calibration is running
2322 58433 : bool AP_InertialSensor::temperature_cal_running() const
2323 : {
2324 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
2325 202196 : for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
2326 154276 : if (tcal(i).enable == AP_InertialSensor_TCal::Enable::LearnCalibration) {
2327 10513 : return true;
2328 : }
2329 : }
2330 : #endif
2331 47920 : return false;
2332 : }
2333 :
2334 : #if HAL_INS_ACCELCAL_ENABLED
2335 : // initialise and register accel calibrator
2336 : // called during the startup of accel cal
2337 2 : void AP_InertialSensor::acal_init()
2338 : {
2339 : // NOTE: these objects are never deallocated because the pre-arm checks force a reboot
2340 2 : if (_acal == nullptr) {
2341 2 : _acal = NEW_NOTHROW AP_AccelCal;
2342 : }
2343 2 : if (_accel_calibrator == nullptr) {
2344 8 : _accel_calibrator = NEW_NOTHROW AccelCalibrator[INS_MAX_INSTANCES];
2345 : }
2346 2 : }
2347 :
2348 : // update accel calibrator
2349 499279 : void AP_InertialSensor::acal_update()
2350 : {
2351 499279 : if(_acal == nullptr) {
2352 498588 : return;
2353 : }
2354 :
2355 691 : EXPECT_DELAY_MS(20000);
2356 691 : _acal->update();
2357 :
2358 691 : if (hal.util->get_soft_armed() && _acal->get_status() != ACCEL_CAL_NOT_STARTED) {
2359 0 : _acal->cancel();
2360 : }
2361 691 : }
2362 : #endif
2363 :
2364 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
2365 : /*
2366 : Update the harmonic notch frequency
2367 :
2368 : Note that zero is a valid value and will disable the notch unless
2369 : the TreatLowAsMin option is set
2370 : */
2371 192491 : void AP_InertialSensor::HarmonicNotch::update_freq_hz(float scaled_freq)
2372 : {
2373 192491 : calculated_notch_freq_hz[0] = fabsf(scaled_freq);
2374 192491 : num_calculated_notch_frequencies = 1;
2375 192491 : }
2376 :
2377 : // Update the harmonic notch frequency
2378 215971 : void AP_InertialSensor::HarmonicNotch::update_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]) {
2379 : // note that we allow zero through, which will disable the notch
2380 1103729 : for (uint8_t i = 0; i < num_freqs; i++) {
2381 887758 : calculated_notch_freq_hz[i] = fabsf(scaled_freq[i]);
2382 : }
2383 : // any uncalculated frequencies will float at the previous value or the initialized freq if none
2384 215971 : num_calculated_notch_frequencies = num_freqs;
2385 215971 : }
2386 :
2387 : // setup the notch for throttle based tracking, called from FFT based tuning
2388 2 : bool AP_InertialSensor::setup_throttle_gyro_harmonic_notch(float center_freq_hz, float lower_freq_hz, float ref, uint8_t harmonics)
2389 : {
2390 2 : for (auto ¬ch : harmonic_notches) {
2391 2 : if (notch.params.tracking_mode() != HarmonicNotchDynamicMode::UpdateThrottle) {
2392 0 : continue;
2393 : }
2394 2 : notch.params.enable();
2395 2 : notch.params.set_center_freq_hz(center_freq_hz);
2396 2 : notch.params.set_reference(ref);
2397 2 : notch.params.set_bandwidth_hz(center_freq_hz / 2.0f);
2398 2 : notch.params.set_freq_min_ratio(lower_freq_hz / center_freq_hz);
2399 2 : notch.params.set_harmonics(harmonics);
2400 2 : notch.params.save_params();
2401 : // only enable the first notch
2402 2 : return true;
2403 : }
2404 :
2405 0 : return false;
2406 : }
2407 : #endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
2408 :
2409 : /*
2410 : set and save accelerometer bias along with trim calculation
2411 : */
2412 1 : void AP_InertialSensor::_acal_save_calibrations()
2413 : {
2414 1 : Vector3f bias, gain;
2415 3 : for (uint8_t i=0; i<_accel_count; i++) {
2416 2 : if (_accel_calibrator[i].get_status() == ACCEL_CAL_SUCCESS) {
2417 2 : _accel_calibrator[i].get_calibration(bias, gain);
2418 2 : _accel_offset(i).set_and_save(bias);
2419 2 : _accel_scale(i).set_and_save(gain);
2420 2 : _accel_id(i).save();
2421 2 : _accel_id_ok[i] = true;
2422 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
2423 2 : caltemp_accel(i).set_and_save(get_temperature(i));
2424 : #endif
2425 : } else {
2426 0 : _accel_offset(i).set_and_save(Vector3f());
2427 0 : _accel_scale(i).set_and_save(Vector3f());
2428 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
2429 0 : caltemp_accel(i).set_and_save(-300);
2430 : #endif
2431 : }
2432 : }
2433 :
2434 : // clear any unused accels
2435 2 : for (uint8_t i=_accel_count; i<INS_MAX_INSTANCES; i++) {
2436 1 : _accel_id(i).set_and_save(0);
2437 1 : _accel_offset(i).set_and_save(Vector3f());
2438 1 : _accel_scale(i).set_and_save(Vector3f());
2439 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
2440 1 : caltemp_accel(i).set_and_save_ifchanged(-300);
2441 : #endif
2442 : }
2443 :
2444 1 : Vector3f aligned_sample;
2445 1 : Vector3f misaligned_sample;
2446 1 : switch(_trim_option) {
2447 0 : case 0:
2448 0 : break;
2449 1 : case 1:
2450 : // The first level step of accel cal will be taken as gnd truth,
2451 : // i.e. trim will be set as per the output of primary accel from the level step
2452 1 : get_first_usable_accel_cal_sample_avg(0,aligned_sample);
2453 1 : _trim_rad.zero();
2454 1 : _calculate_trim(aligned_sample, _trim_rad);
2455 1 : _new_trim = true;
2456 1 : break;
2457 0 : case 2:
2458 : // Reference accel is truth, in this scenario there is a reference accel
2459 : // as mentioned in ACC_BODY_ALIGNED
2460 0 : if (get_first_usable_accel_cal_sample_avg(0,misaligned_sample) && get_fixed_mount_accel_cal_sample(0,aligned_sample)) {
2461 : // determine trim from aligned sample vs misaligned sample
2462 0 : Vector3f cross = (misaligned_sample%aligned_sample);
2463 0 : float dot = (misaligned_sample*aligned_sample);
2464 0 : Quaternion q(safe_sqrt(sq(misaligned_sample.length())*sq(aligned_sample.length()))+dot, cross.x, cross.y, cross.z);
2465 0 : q.normalize();
2466 0 : _trim_rad.x = q.get_euler_roll();
2467 0 : _trim_rad.y = q.get_euler_pitch();
2468 0 : _trim_rad.z = 0;
2469 0 : _new_trim = true;
2470 : }
2471 0 : break;
2472 0 : default:
2473 0 : _new_trim = false;
2474 : /* no break */
2475 : }
2476 :
2477 2 : if (fabsf(_trim_rad.x) > radians(HAL_INS_TRIM_LIMIT_DEG) ||
2478 2 : fabsf(_trim_rad.y) > radians(HAL_INS_TRIM_LIMIT_DEG) ||
2479 1 : fabsf(_trim_rad.z) > radians(HAL_INS_TRIM_LIMIT_DEG)) {
2480 0 : DEV_PRINTF("ERR: Trim over maximum of %.1f degrees!!", float(HAL_INS_TRIM_LIMIT_DEG));
2481 0 : _new_trim = false; //we have either got faulty level during acal or highly misaligned accelerometers
2482 : }
2483 :
2484 1 : _accel_cal_requires_reboot = true;
2485 1 : }
2486 :
2487 0 : void AP_InertialSensor::_acal_event_failure()
2488 : {
2489 0 : for (uint8_t i=0; i<_accel_count; i++) {
2490 0 : _accel_offset(i).set_and_notify(Vector3f(0,0,0));
2491 0 : _accel_scale(i).set_and_notify(Vector3f(1,1,1));
2492 : }
2493 0 : }
2494 :
2495 : /*
2496 : Returns true if new valid trim values are available and passes them to reference vars
2497 : */
2498 499279 : bool AP_InertialSensor::get_new_trim(Vector3f &trim_rad)
2499 : {
2500 499279 : if (_new_trim) {
2501 1 : trim_rad = _trim_rad;
2502 1 : _new_trim = false;
2503 1 : return true;
2504 : }
2505 499278 : return false;
2506 : }
2507 :
2508 : /*
2509 : Returns body fixed accelerometer level data averaged during accel calibration's first step
2510 : */
2511 0 : bool AP_InertialSensor::get_fixed_mount_accel_cal_sample(uint8_t sample_num, Vector3f& ret) const
2512 : {
2513 0 : if (_accel_count <= (_acc_body_aligned-1) || _accel_calibrator[2].get_status() != ACCEL_CAL_SUCCESS || sample_num>=_accel_calibrator[2].get_num_samples_collected()) {
2514 0 : return false;
2515 : }
2516 0 : _accel_calibrator[_acc_body_aligned-1].get_sample_corrected(sample_num, ret);
2517 0 : ret.rotate(_board_orientation);
2518 0 : return true;
2519 : }
2520 :
2521 : /*
2522 : Returns Primary accelerometer level data averaged during accel calibration's first step
2523 : */
2524 1 : bool AP_InertialSensor::get_first_usable_accel_cal_sample_avg(uint8_t sample_num, Vector3f& ret) const
2525 : {
2526 1 : uint8_t count = 0;
2527 1 : Vector3f avg = Vector3f(0,0,0);
2528 3 : for (uint8_t i=0; i<MIN(_accel_count,2); i++) {
2529 2 : if (_accel_calibrator[i].get_status() != ACCEL_CAL_SUCCESS || sample_num>=_accel_calibrator[i].get_num_samples_collected()) {
2530 0 : continue;
2531 : }
2532 2 : Vector3f sample;
2533 2 : _accel_calibrator[i].get_sample_corrected(sample_num, sample);
2534 2 : avg += sample;
2535 2 : count++;
2536 : }
2537 1 : if (count == 0) {
2538 0 : return false;
2539 : }
2540 1 : avg /= count;
2541 1 : ret = avg;
2542 1 : ret.rotate(_board_orientation);
2543 1 : return true;
2544 : }
2545 :
2546 : #if HAL_GCS_ENABLED
2547 7 : bool AP_InertialSensor::calibrate_gyros()
2548 : {
2549 7 : init_gyro();
2550 7 : if (!gyro_calibrated_ok_all()) {
2551 0 : return false;
2552 : }
2553 : #if AP_AHRS_ENABLED
2554 7 : AP::ahrs().reset_gyro_drift();
2555 : #endif
2556 7 : return true;
2557 : }
2558 :
2559 : /*
2560 : perform a simple 1D accel calibration, returning mavlink result code
2561 : */
2562 27 : MAV_RESULT AP_InertialSensor::simple_accel_cal()
2563 : {
2564 27 : const uint32_t now = AP_HAL::millis();
2565 27 : if ((now - last_accel_cal_ms) < 5000) {
2566 0 : return MAV_RESULT_TEMPORARILY_REJECTED;
2567 : }
2568 27 : last_accel_cal_ms = now;
2569 :
2570 27 : uint8_t num_accels = MIN(get_accel_count(), INS_MAX_INSTANCES);
2571 27 : Vector3f last_average[INS_MAX_INSTANCES];
2572 27 : Vector3f new_accel_offset[INS_MAX_INSTANCES];
2573 27 : Vector3f saved_offsets[INS_MAX_INSTANCES];
2574 27 : Vector3f saved_scaling[INS_MAX_INSTANCES];
2575 : bool converged[INS_MAX_INSTANCES];
2576 27 : const float accel_convergence_limit = 0.05;
2577 27 : Vector3f rotated_gravity(0, 0, -GRAVITY_MSS);
2578 :
2579 : // exit immediately if calibration is already in progress
2580 27 : if (calibrating()) {
2581 0 : return MAV_RESULT_TEMPORARILY_REJECTED;
2582 : }
2583 :
2584 27 : EXPECT_DELAY_MS(20000);
2585 : // record we are calibrating
2586 27 : _calibrating_accel = true;
2587 :
2588 : // flash leds to tell user to keep the IMU still
2589 27 : AP_Notify::flags.initialising = true;
2590 :
2591 : /*
2592 : we do the accel calibration with no board rotation. This avoids
2593 : having to rotate readings during the calibration
2594 : */
2595 27 : enum Rotation saved_orientation = _board_orientation;
2596 27 : _board_orientation = ROTATION_NONE;
2597 :
2598 : // get the rotated gravity vector which will need to be applied to the offsets
2599 27 : rotated_gravity.rotate_inverse(saved_orientation);
2600 :
2601 : // save existing accel offsets
2602 86 : for (uint8_t k=0; k<num_accels; k++) {
2603 59 : saved_offsets[k] = _accel_offset(k);
2604 59 : saved_scaling[k] = _accel_scale(k);
2605 : }
2606 :
2607 : // remove existing accel offsets and scaling
2608 108 : for (uint8_t k=0; k<INS_MAX_INSTANCES; k++) {
2609 81 : _accel_offset(k).set(Vector3f());
2610 81 : _accel_scale(k).set(Vector3f(1,1,1));
2611 81 : new_accel_offset[k].zero();
2612 81 : last_average[k].zero();
2613 81 : converged[k] = false;
2614 : }
2615 :
2616 162 : for (uint8_t c = 0; c < 5; c++) {
2617 135 : hal.scheduler->delay(5);
2618 135 : update();
2619 : }
2620 :
2621 : // the strategy is to average 50 points over 0.5 seconds, then do it
2622 : // again and see if the 2nd average is within a small margin of
2623 : // the first
2624 :
2625 27 : uint8_t num_converged = 0;
2626 :
2627 : // we try to get a good calibration estimate for up to 10 seconds
2628 : // if the accels are stable, we should get it in 1 second
2629 81 : for (int16_t j = 0; j <= 10*4 && num_converged < num_accels; j++) {
2630 54 : Vector3f accel_sum[INS_MAX_INSTANCES], accel_avg[INS_MAX_INSTANCES], accel_diff[INS_MAX_INSTANCES];
2631 : float diff_norm[INS_MAX_INSTANCES];
2632 : uint8_t i;
2633 :
2634 54 : memset(diff_norm, 0, sizeof(diff_norm));
2635 :
2636 54 : DEV_PRINTF("*");
2637 :
2638 172 : for (uint8_t k=0; k<num_accels; k++) {
2639 118 : accel_sum[k].zero();
2640 : }
2641 2754 : for (i=0; i<50; i++) {
2642 2700 : update();
2643 8600 : for (uint8_t k=0; k<num_accels; k++) {
2644 5900 : accel_sum[k] += get_accel(k);
2645 : }
2646 2700 : hal.scheduler->delay(5);
2647 : }
2648 :
2649 172 : for (uint8_t k=0; k<num_accels; k++) {
2650 118 : accel_avg[k] = accel_sum[k] / i;
2651 118 : accel_diff[k] = last_average[k] - accel_avg[k];
2652 118 : diff_norm[k] = accel_diff[k].length();
2653 : }
2654 :
2655 172 : for (uint8_t k=0; k<num_accels; k++) {
2656 118 : if (j > 0 && diff_norm[k] < accel_convergence_limit) {
2657 59 : last_average[k] = (accel_avg[k] * 0.5f) + (last_average[k] * 0.5f);
2658 59 : if (!converged[k] || last_average[k].length() < new_accel_offset[k].length()) {
2659 59 : new_accel_offset[k] = last_average[k];
2660 : }
2661 59 : if (!converged[k]) {
2662 59 : converged[k] = true;
2663 59 : num_converged++;
2664 : }
2665 : } else {
2666 59 : last_average[k] = accel_avg[k];
2667 : }
2668 : }
2669 : }
2670 :
2671 27 : MAV_RESULT result = MAV_RESULT_ACCEPTED;
2672 :
2673 : // see if we've passed
2674 86 : for (uint8_t k=0; k<num_accels; k++) {
2675 59 : if (!converged[k]) {
2676 0 : result = MAV_RESULT_FAILED;
2677 : }
2678 : }
2679 :
2680 : // restore orientation
2681 27 : _board_orientation = saved_orientation;
2682 :
2683 27 : if (result == MAV_RESULT_ACCEPTED) {
2684 27 : DEV_PRINTF("\nPASSED\n");
2685 86 : for (uint8_t k=0; k<num_accels; k++) {
2686 : // remove rotated gravity
2687 59 : new_accel_offset[k] -= rotated_gravity;
2688 59 : _accel_offset(k).set_and_save(new_accel_offset[k]);
2689 59 : _accel_scale(k).save();
2690 59 : _accel_id(k).save();
2691 59 : _accel_id_ok[k] = true;
2692 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
2693 59 : caltemp_accel(k).set_and_save(get_temperature(k));
2694 : #endif
2695 : }
2696 49 : for (uint8_t k=num_accels; k<INS_MAX_INSTANCES; k++) {
2697 22 : _accel_offset(k).set_and_save(Vector3f());
2698 22 : _accel_scale(k).set_and_save(Vector3f());
2699 22 : _gyro_offset(k).set_and_save(Vector3f());
2700 22 : _accel_id(k).set_and_save(0);
2701 : }
2702 :
2703 : #if AP_AHRS_ENABLED
2704 : // force trim to zero
2705 27 : AP::ahrs().set_trim(Vector3f(0, 0, 0));
2706 : #endif
2707 : } else {
2708 0 : DEV_PRINTF("\nFAILED\n");
2709 : // restore old values
2710 0 : for (uint8_t k=0; k<num_accels; k++) {
2711 0 : _accel_offset(k).set(saved_offsets[k]);
2712 0 : _accel_scale(k).set(saved_scaling[k]);
2713 : }
2714 : }
2715 :
2716 : // record calibration complete
2717 27 : _calibrating_accel = false;
2718 :
2719 : // throw away any existing samples that may have the wrong
2720 : // orientation. We do this by throwing samples away for 0.5s,
2721 : // which is enough time for the filters to settle
2722 27 : uint32_t start_ms = AP_HAL::millis();
2723 1414 : while (AP_HAL::millis() - start_ms < 500) {
2724 1387 : update();
2725 : }
2726 :
2727 : #if AP_AHRS_ENABLED
2728 : // and reset state estimators
2729 27 : AP::ahrs().reset();
2730 : #endif
2731 :
2732 : // stop flashing leds
2733 27 : AP_Notify::flags.initialising = false;
2734 27 : last_accel_cal_ms = AP_HAL::millis ();
2735 :
2736 27 : return result;
2737 27 : }
2738 : #endif
2739 :
2740 : /*
2741 : see if gyro calibration should be performed
2742 : */
2743 2780 : AP_InertialSensor::Gyro_Calibration_Timing AP_InertialSensor::gyro_calibration_timing()
2744 : {
2745 2780 : if (hal.util->was_watchdog_reset()) {
2746 2 : return GYRO_CAL_NEVER;
2747 : }
2748 2778 : return (Gyro_Calibration_Timing)_gyro_cal_timing.get();
2749 : }
2750 :
2751 : #if AP_INERTIALSENSOR_KILL_IMU_ENABLED
2752 : /*
2753 : update IMU kill mask, used for testing IMU failover
2754 : */
2755 0 : void AP_InertialSensor::kill_imu(uint8_t imu_idx, bool kill_it)
2756 : {
2757 0 : if (kill_it) {
2758 0 : uint8_t new_kill_mask = imu_kill_mask | (1U<<imu_idx);
2759 : // don't allow the last IMU to be killed
2760 0 : bool all_dead = true;
2761 0 : for (uint8_t i=0; i<MIN(_gyro_count, _accel_count); i++) {
2762 0 : if (use_gyro(i) && use_accel(i) && !(new_kill_mask & (1U<<i))) {
2763 : // we have at least one healthy IMU left
2764 0 : all_dead = false;
2765 : }
2766 : }
2767 0 : if (!all_dead) {
2768 0 : imu_kill_mask = new_kill_mask;
2769 : }
2770 : } else {
2771 0 : imu_kill_mask &= ~(1U<<imu_idx);
2772 : }
2773 0 : }
2774 : #endif // AP_INERTIALSENSOR_KILL_IMU_ENABLED
2775 :
2776 : #if AP_SERIALMANAGER_IMUOUT_ENABLED
2777 : /*
2778 : setup a UART for sending external data
2779 : */
2780 0 : void AP_InertialSensor::set_imu_out_uart(AP_HAL::UARTDriver *_uart)
2781 : {
2782 0 : uart.imu_out_uart = _uart;
2783 0 : uart.counter = 0;
2784 0 : }
2785 :
2786 : /*
2787 : send IMU delta-angle and delta-velocity to a UART
2788 : */
2789 0 : void AP_InertialSensor::send_uart_data(void)
2790 : {
2791 : struct {
2792 : uint16_t magic = 0x29c4;
2793 : uint16_t length;
2794 : uint32_t timestamp_us;
2795 : Vector3f delta_velocity;
2796 : Vector3f delta_angle;
2797 : float delta_velocity_dt;
2798 : float delta_angle_dt;
2799 : uint16_t counter;
2800 : uint16_t crc;
2801 0 : } data;
2802 :
2803 0 : if (uart.imu_out_uart->txspace() < sizeof(data)) {
2804 : // not enough space
2805 0 : return;
2806 : }
2807 :
2808 0 : data.length = sizeof(data);
2809 0 : data.timestamp_us = AP_HAL::micros();
2810 :
2811 0 : get_delta_angle(get_first_usable_gyro(), data.delta_angle, data.delta_angle_dt);
2812 0 : get_delta_velocity(get_first_usable_accel(), data.delta_velocity, data.delta_velocity_dt);
2813 :
2814 0 : data.counter = uart.counter++;
2815 0 : data.crc = crc_xmodem((const uint8_t *)&data, sizeof(data)-sizeof(uint16_t));
2816 :
2817 0 : uart.imu_out_uart->write((const uint8_t *)&data, sizeof(data));
2818 : }
2819 : #endif // AP_SERIALMANAGER_IMUOUT_ENABLED
2820 :
2821 : #if AP_EXTERNAL_AHRS_ENABLED
2822 137584 : void AP_InertialSensor::handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt)
2823 : {
2824 480591 : for (uint8_t i = 0; i < _backend_count; i++) {
2825 343007 : _backends[i]->handle_external(pkt);
2826 : }
2827 137584 : }
2828 : #endif // AP_EXTERNAL_AHRS_ENABLED
2829 :
2830 : // force save of current calibration as valid
2831 3 : void AP_InertialSensor::force_save_calibration(void)
2832 : {
2833 9 : for (uint8_t i=0; i<_accel_count; i++) {
2834 6 : if (_accel_id(i) != 0) {
2835 6 : _accel_id(i).save();
2836 : // we also save the scale as the default of 1.0 may be
2837 : // over a stored value of 0.0
2838 6 : _accel_scale(i).save();
2839 6 : _accel_id_ok[i] = true;
2840 : }
2841 : }
2842 3 : }
2843 :
2844 : namespace AP {
2845 :
2846 829301395 : AP_InertialSensor &ins()
2847 : {
2848 829301395 : return *AP_InertialSensor::get_singleton();
2849 : }
2850 :
2851 : };
2852 :
2853 : #endif // AP_INERTIALSENSOR_ENABLED
|
