Line data Source code
1 : #include "AP_Compass_config.h"
2 :
3 : #if AP_COMPASS_ENABLED
4 :
5 : #include <AP_HAL/AP_HAL.h>
6 : #include <AP_HAL/I2CDevice.h>
7 : #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
8 : #include <AP_HAL_Linux/I2CDevice.h>
9 : #endif
10 : #include <AP_Vehicle/AP_Vehicle_Type.h>
11 : #include <AP_BoardConfig/AP_BoardConfig.h>
12 : #include <AP_Logger/AP_Logger.h>
13 : #include <AP_Vehicle/AP_Vehicle_Type.h>
14 : #include <AP_ExternalAHRS/AP_ExternalAHRS.h>
15 : #include <GCS_MAVLink/GCS.h>
16 : #include <AP_AHRS/AP_AHRS.h>
17 :
18 : #include "AP_Compass_config.h"
19 :
20 : #include "AP_Compass_SITL.h"
21 : #include "AP_Compass_AF9838.h"
22 : #include "AP_Compass_AK8963.h"
23 : #include "AP_Compass_Backend.h"
24 : #include "AP_Compass_BMM150.h"
25 : #include "AP_Compass_BMM350.h"
26 : #include "AP_Compass_HMC5843.h"
27 : #include "AP_Compass_IIS2MDC.h"
28 : #include "AP_Compass_IST8308.h"
29 : #include "AP_Compass_IST8310.h"
30 : #include "AP_Compass_LSM303D.h"
31 : #include "AP_Compass_LSM9DS1.h"
32 : #include "AP_Compass_LIS3MDL.h"
33 : #include "AP_Compass_AK09916.h"
34 : #include "AP_Compass_QMC5883L.h"
35 : #if AP_COMPASS_DRONECAN_ENABLED
36 : #include "AP_Compass_DroneCAN.h"
37 : #endif
38 : #include "AP_Compass_QMC5883P.h"
39 : #include "AP_Compass_MMC3416.h"
40 : #include "AP_Compass_MMC5xx3.h"
41 : #include "AP_Compass_MAG3110.h"
42 : #include "AP_Compass_RM3100.h"
43 : #if AP_COMPASS_MSP_ENABLED
44 : #include "AP_Compass_MSP.h"
45 : #endif
46 : #if AP_COMPASS_EXTERNALAHRS_ENABLED
47 : #include "AP_Compass_ExternalAHRS.h"
48 : #endif
49 : #include "AP_Compass.h"
50 : #include "Compass_learn.h"
51 : #include <stdio.h>
52 :
53 : extern const AP_HAL::HAL& hal;
54 :
55 : #ifndef AP_COMPASS_ENABLE_DEFAULT
56 : #define AP_COMPASS_ENABLE_DEFAULT 1
57 : #endif
58 :
59 : #ifndef COMPASS_LEARN_DEFAULT
60 : #define COMPASS_LEARN_DEFAULT Compass::LearnType::NONE
61 : #endif
62 :
63 : #ifndef AP_COMPASS_OFFSETS_MAX_DEFAULT
64 : #define AP_COMPASS_OFFSETS_MAX_DEFAULT 1800
65 : #endif
66 :
67 : #ifndef HAL_COMPASS_FILTER_DEFAULT
68 : #define HAL_COMPASS_FILTER_DEFAULT 0 // turned off by default
69 : #endif
70 :
71 : #ifndef HAL_COMPASS_AUTO_ROT_DEFAULT
72 : #define HAL_COMPASS_AUTO_ROT_DEFAULT 2
73 : #endif
74 :
75 : const AP_Param::GroupInfo Compass::var_info[] = {
76 : // index 0 was used for the old orientation matrix
77 :
78 : #ifndef HAL_BUILD_AP_PERIPH
79 : // @Param: OFS_X
80 : // @DisplayName: Compass offsets in milligauss on the X axis
81 : // @Description: Offset to be added to the compass x-axis values to compensate for metal in the frame
82 : // @Range: -400 400
83 : // @Units: mGauss
84 : // @Increment: 1
85 : // @User: Advanced
86 : // @Calibration: 1
87 :
88 : // @Param: OFS_Y
89 : // @DisplayName: Compass offsets in milligauss on the Y axis
90 : // @Description: Offset to be added to the compass y-axis values to compensate for metal in the frame
91 : // @Range: -400 400
92 : // @Units: mGauss
93 : // @Increment: 1
94 : // @User: Advanced
95 : // @Calibration: 1
96 :
97 : // @Param: OFS_Z
98 : // @DisplayName: Compass offsets in milligauss on the Z axis
99 : // @Description: Offset to be added to the compass z-axis values to compensate for metal in the frame
100 : // @Range: -400 400
101 : // @Units: mGauss
102 : // @Increment: 1
103 : // @User: Advanced
104 : // @Calibration: 1
105 : AP_GROUPINFO("OFS", 1, Compass, _state._priv_instance[0].offset, 0),
106 :
107 : // @Param: DEC
108 : // @DisplayName: Compass declination
109 : // @Description: An angle to compensate between the true north and magnetic north
110 : // @Range: -3.142 3.142
111 : // @Units: rad
112 : // @Increment: 0.01
113 : // @User: Standard
114 : AP_GROUPINFO("DEC", 2, Compass, _declination, 0),
115 : #endif // HAL_BUILD_AP_PERIPH
116 :
117 : #if COMPASS_LEARN_ENABLED
118 : // @Param: LEARN
119 : // @DisplayName: Learn compass offsets automatically
120 : // @Description: Enable or disable the automatic learning of compass offsets. EKF-Learning uses the offsets learnt by the active EKF state estimator; those offsets are saved when you disarm the vehicle, and it does nothing on firmware built without the CompassLearnCopyFromEKF feature. If InFlight learning is enabled then the compass will automatically start learning once a flight starts (must be armed). While InFlight learning is running you cannot use position control modes.
121 : // @Values: 0:Disabled,2:EKF-Learning,3:InFlight-Learning
122 : // @User: Advanced
123 : AP_GROUPINFO("LEARN", 3, Compass, _learn, float(COMPASS_LEARN_DEFAULT)),
124 : #endif
125 :
126 : #ifndef HAL_BUILD_AP_PERIPH
127 : // @Param: USE
128 : // @DisplayName: Use compass for yaw
129 : // @Description: Enable or disable the use of the compass (instead of the GPS) for determining heading
130 : // @Values: 0:Disabled,1:Enabled
131 : // @User: Advanced
132 : AP_GROUPINFO("USE", 4, Compass, _use_for_yaw._priv_instance[0], 1), // true if used for DCM yaw
133 :
134 : // @Param: AUTODEC
135 : // @DisplayName: Auto Declination
136 : // @Description: Enable or disable the automatic calculation of the declination based on gps location
137 : // @Values: 0:Disabled,1:Enabled
138 : // @User: Advanced
139 : AP_GROUPINFO("AUTODEC",5, Compass, _auto_declination, 1),
140 : #endif
141 :
142 : #if COMPASS_MOT_ENABLED
143 : // @Param: MOTCT
144 : // @DisplayName: Motor interference compensation type
145 : // @Description: Set motor interference compensation type to disabled, throttle or current. Do not change manually.
146 : // @Values: 0:Disabled,1:Use Throttle,2:Use Current
147 : // @User: Advanced
148 : // @Calibration: 1
149 : AP_GROUPINFO("MOTCT", 6, Compass, _motor_comp_type, AP_COMPASS_MOT_COMP_DISABLED),
150 :
151 : // @Param: MOT_X
152 : // @DisplayName: Motor interference compensation for body frame X axis
153 : // @Description: Multiplied by the current throttle and added to the compass's x-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
154 : // @Range: -1000 1000
155 : // @Units: mGauss/A
156 : // @Increment: 1
157 : // @User: Advanced
158 : // @Calibration: 1
159 :
160 : // @Param: MOT_Y
161 : // @DisplayName: Motor interference compensation for body frame Y axis
162 : // @Description: Multiplied by the current throttle and added to the compass's y-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
163 : // @Range: -1000 1000
164 : // @Units: mGauss/A
165 : // @Increment: 1
166 : // @User: Advanced
167 : // @Calibration: 1
168 :
169 : // @Param: MOT_Z
170 : // @DisplayName: Motor interference compensation for body frame Z axis
171 : // @Description: Multiplied by the current throttle and added to the compass's z-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
172 : // @Range: -1000 1000
173 : // @Units: mGauss/A
174 : // @Increment: 1
175 : // @User: Advanced
176 : // @Calibration: 1
177 : AP_GROUPINFO("MOT", 7, Compass, _state._priv_instance[0].motor_compensation, 0),
178 : #endif
179 :
180 : #ifndef HAL_BUILD_AP_PERIPH
181 : // @Param: ORIENT
182 : // @DisplayName: Compass orientation
183 : // @Description: The orientation of the first external compass relative to the vehicle frame. This value will be ignored unless this compass is set as an external compass. When set correctly in the northern hemisphere, pointing the nose and right side down should increase the MagX and MagY values respectively. Rolling the vehicle upside down should decrease the MagZ value. For southern hemisphere, switch increase and decrease. NOTE: For internal compasses, AHRS_ORIENT is used. The label for each option is specified in the order of rotations for that orientation. Firmware versions 4.2 and prior can use a CUSTOM (100) rotation to set the COMPASS_CUS_ROLL/PIT/YAW angles for Compass orientation. Later versions provide two general custom rotations which can be used, Custom 1 and Custom 2, with CUST_1_ROLL/PIT/YAW or CUST_2_ROLL/PIT/YAW angles.
184 : // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Yaw45Roll180,10:Yaw90Roll180,11:Yaw135Roll180,12:Pitch180,13:Yaw225Roll180,14:Yaw270Roll180,15:Yaw315Roll180,16:Roll90,17:Yaw45Roll90,18:Yaw90Roll90,19:Yaw135Roll90,20:Roll270,21:Yaw45Roll270,22:Yaw90Roll270,23:Yaw135Roll270,24:Pitch90,25:Pitch270,26:Yaw90Pitch180,27:Yaw270Pitch180,28:Pitch90Roll90,29:Pitch90Roll180,30:Pitch90Roll270,31:Pitch180Roll90,32:Pitch180Roll270,33:Pitch270Roll90,34:Pitch270Roll180,35:Pitch270Roll270,36:Yaw90Pitch180Roll90,37:Yaw270Roll90,38:Yaw293Pitch68Roll180,39:Pitch315,40:Pitch315Roll90,42:Roll45,43:Roll315,100:Custom 4.1 and older,101:Custom 1,102:Custom 2
185 : // @User: Advanced
186 : AP_GROUPINFO("ORIENT", 8, Compass, _state._priv_instance[0].orientation, ROTATION_NONE),
187 :
188 : // @Param: EXTERNAL
189 : // @DisplayName: Compass is attached via an external cable
190 : // @Description: Configure compass so it is attached externally. This is auto-detected on most boards. Set to 1 if the compass is externally connected. When externally connected the COMPASS_ORIENT option operates independently of the AHRS_ORIENTATION board orientation option. If set to 0 or 1 then auto-detection by bus connection can override the value. If set to 2 then auto-detection will be disabled.
191 : // @Values: 0:Internal,1:External,2:ForcedExternal
192 : // @User: Advanced
193 : AP_GROUPINFO("EXTERNAL", 9, Compass, _state._priv_instance[0].external, 0),
194 : #endif
195 :
196 : #if COMPASS_MAX_INSTANCES > 1
197 : // @Param: OFS2_X
198 : // @DisplayName: Compass2 offsets in milligauss on the X axis
199 : // @Description: Offset to be added to compass2's x-axis values to compensate for metal in the frame
200 : // @Range: -400 400
201 : // @Units: mGauss
202 : // @Increment: 1
203 : // @User: Advanced
204 : // @Calibration: 1
205 :
206 : // @Param: OFS2_Y
207 : // @DisplayName: Compass2 offsets in milligauss on the Y axis
208 : // @Description: Offset to be added to compass2's y-axis values to compensate for metal in the frame
209 : // @Range: -400 400
210 : // @Units: mGauss
211 : // @Increment: 1
212 : // @User: Advanced
213 : // @Calibration: 1
214 :
215 : // @Param: OFS2_Z
216 : // @DisplayName: Compass2 offsets in milligauss on the Z axis
217 : // @Description: Offset to be added to compass2's z-axis values to compensate for metal in the frame
218 : // @Range: -400 400
219 : // @Units: mGauss
220 : // @Increment: 1
221 : // @User: Advanced
222 : // @Calibration: 1
223 : AP_GROUPINFO("OFS2", 10, Compass, _state._priv_instance[1].offset, 0),
224 :
225 : // @Param: MOT2_X
226 : // @DisplayName: Motor interference compensation to compass2 for body frame X axis
227 : // @Description: Multiplied by the current throttle and added to compass2's x-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
228 : // @Range: -1000 1000
229 : // @Units: mGauss/A
230 : // @Increment: 1
231 : // @User: Advanced
232 : // @Calibration: 1
233 :
234 : // @Param: MOT2_Y
235 : // @DisplayName: Motor interference compensation to compass2 for body frame Y axis
236 : // @Description: Multiplied by the current throttle and added to compass2's y-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
237 : // @Range: -1000 1000
238 : // @Units: mGauss/A
239 : // @Increment: 1
240 : // @User: Advanced
241 : // @Calibration: 1
242 :
243 : // @Param: MOT2_Z
244 : // @DisplayName: Motor interference compensation to compass2 for body frame Z axis
245 : // @Description: Multiplied by the current throttle and added to compass2's z-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
246 : // @Range: -1000 1000
247 : // @Units: mGauss/A
248 : // @Increment: 1
249 : // @User: Advanced
250 : // @Calibration: 1
251 : AP_GROUPINFO("MOT2", 11, Compass, _state._priv_instance[1].motor_compensation, 0),
252 :
253 : #endif // COMPASS_MAX_INSTANCES
254 :
255 : #if COMPASS_MAX_INSTANCES > 2
256 : // @Param: OFS3_X
257 : // @DisplayName: Compass3 offsets in milligauss on the X axis
258 : // @Description: Offset to be added to compass3's x-axis values to compensate for metal in the frame
259 : // @Range: -400 400
260 : // @Units: mGauss
261 : // @Increment: 1
262 : // @User: Advanced
263 : // @Calibration: 1
264 :
265 : // @Param: OFS3_Y
266 : // @DisplayName: Compass3 offsets in milligauss on the Y axis
267 : // @Description: Offset to be added to compass3's y-axis values to compensate for metal in the frame
268 : // @Range: -400 400
269 : // @Units: mGauss
270 : // @Increment: 1
271 : // @User: Advanced
272 : // @Calibration: 1
273 :
274 : // @Param: OFS3_Z
275 : // @DisplayName: Compass3 offsets in milligauss on the Z axis
276 : // @Description: Offset to be added to compass3's z-axis values to compensate for metal in the frame
277 : // @Range: -400 400
278 : // @Units: mGauss
279 : // @Increment: 1
280 : // @User: Advanced
281 : // @Calibration: 1
282 : AP_GROUPINFO("OFS3", 13, Compass, _state._priv_instance[2].offset, 0),
283 :
284 : // @Param: MOT3_X
285 : // @DisplayName: Motor interference compensation to compass3 for body frame X axis
286 : // @Description: Multiplied by the current throttle and added to compass3's x-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
287 : // @Range: -1000 1000
288 : // @Units: mGauss/A
289 : // @Increment: 1
290 : // @User: Advanced
291 : // @Calibration: 1
292 :
293 : // @Param: MOT3_Y
294 : // @DisplayName: Motor interference compensation to compass3 for body frame Y axis
295 : // @Description: Multiplied by the current throttle and added to compass3's y-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
296 : // @Range: -1000 1000
297 : // @Units: mGauss/A
298 : // @Increment: 1
299 : // @User: Advanced
300 : // @Calibration: 1
301 :
302 : // @Param: MOT3_Z
303 : // @DisplayName: Motor interference compensation to compass3 for body frame Z axis
304 : // @Description: Multiplied by the current throttle and added to compass3's z-axis values to compensate for motor interference (Offset per Amp or at Full Throttle)
305 : // @Range: -1000 1000
306 : // @Units: mGauss/A
307 : // @Increment: 1
308 : // @User: Advanced
309 : // @Calibration: 1
310 : AP_GROUPINFO("MOT3", 14, Compass, _state._priv_instance[2].motor_compensation, 0),
311 : #endif // COMPASS_MAX_INSTANCES
312 :
313 : // @Param: DEV_ID
314 : // @DisplayName: Compass device id
315 : // @Description: Compass device id. Automatically detected, do not set manually
316 : // @ReadOnly: True
317 : // @User: Advanced
318 : AP_GROUPINFO("DEV_ID", 15, Compass, _state._priv_instance[0].dev_id, 0),
319 :
320 : #if COMPASS_MAX_INSTANCES > 1
321 : // @Param: DEV_ID2
322 : // @DisplayName: Compass2 device id
323 : // @Description: Second compass's device id. Automatically detected, do not set manually
324 : // @ReadOnly: True
325 : // @User: Advanced
326 : AP_GROUPINFO("DEV_ID2", 16, Compass, _state._priv_instance[1].dev_id, 0),
327 : #endif // COMPASS_MAX_INSTANCES
328 :
329 : #if COMPASS_MAX_INSTANCES > 2
330 : // @Param: DEV_ID3
331 : // @DisplayName: Compass3 device id
332 : // @Description: Third compass's device id. Automatically detected, do not set manually
333 : // @ReadOnly: True
334 : // @User: Advanced
335 : AP_GROUPINFO("DEV_ID3", 17, Compass, _state._priv_instance[2].dev_id, 0),
336 : #endif // COMPASS_MAX_INSTANCES
337 :
338 : #if COMPASS_MAX_INSTANCES > 1
339 : // @Param: USE2
340 : // @DisplayName: Compass2 used for yaw
341 : // @Description: Enable or disable the secondary compass for determining heading.
342 : // @Values: 0:Disabled,1:Enabled
343 : // @User: Advanced
344 : AP_GROUPINFO("USE2", 18, Compass, _use_for_yaw._priv_instance[1], 1),
345 :
346 : // @Param: ORIENT2
347 : // @DisplayName: Compass2 orientation
348 : // @Description: The orientation of a second external compass relative to the vehicle frame. This value will be ignored unless this compass is set as an external compass. When set correctly in the northern hemisphere, pointing the nose and right side down should increase the MagX and MagY values respectively. Rolling the vehicle upside down should decrease the MagZ value. For southern hemisphere, switch increase and decrease. NOTE: For internal compasses, AHRS_ORIENT is used. The label for each option is specified in the order of rotations for that orientation. Firmware versions 4.2 and prior can use a CUSTOM (100) rotation to set the COMPASS_CUS_ROLL/PIT/YAW angles for Compass orientation. Later versions provide two general custom rotations which can be used, Custom 1 and Custom 2, with CUST_1_ROLL/PIT/YAW or CUST_2_ROLL/PIT/YAW angles.
349 : // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Yaw45Roll180,10:Yaw90Roll180,11:Yaw135Roll180,12:Pitch180,13:Yaw225Roll180,14:Yaw270Roll180,15:Yaw315Roll180,16:Roll90,17:Yaw45Roll90,18:Yaw90Roll90,19:Yaw135Roll90,20:Roll270,21:Yaw45Roll270,22:Yaw90Roll270,23:Yaw135Roll270,24:Pitch90,25:Pitch270,26:Yaw90Pitch180,27:Yaw270Pitch180,28:Pitch90Roll90,29:Pitch90Roll180,30:Pitch90Roll270,31:Pitch180Roll90,32:Pitch180Roll270,33:Pitch270Roll90,34:Pitch270Roll180,35:Pitch270Roll270,36:Yaw90Pitch180Roll90,37:Yaw270Roll90,38:Yaw293Pitch68Roll180,39:Pitch315,40:Pitch315Roll90,42:Roll45,43:Roll315,100:Custom 4.1 and older,101:Custom 1,102:Custom 2
350 : // @User: Advanced
351 : AP_GROUPINFO("ORIENT2", 19, Compass, _state._priv_instance[1].orientation, ROTATION_NONE),
352 :
353 : // @Param: EXTERN2
354 : // @DisplayName: Compass2 is attached via an external cable
355 : // @Description: Configure second compass so it is attached externally. This is auto-detected on most boards. If set to 0 or 1 then auto-detection by bus connection can override the value. If set to 2 then auto-detection will be disabled.
356 : // @Values: 0:Internal,1:External,2:ForcedExternal
357 : // @User: Advanced
358 : AP_GROUPINFO("EXTERN2",20, Compass, _state._priv_instance[1].external, 0),
359 : #endif // COMPASS_MAX_INSTANCES
360 :
361 : #if COMPASS_MAX_INSTANCES > 2
362 : // @Param: USE3
363 : // @DisplayName: Compass3 used for yaw
364 : // @Description: Enable or disable the tertiary compass for determining heading.
365 : // @Values: 0:Disabled,1:Enabled
366 : // @User: Advanced
367 : AP_GROUPINFO("USE3", 21, Compass, _use_for_yaw._priv_instance[2], 1),
368 :
369 : // @Param: ORIENT3
370 : // @DisplayName: Compass3 orientation
371 : // @Description: The orientation of a third external compass relative to the vehicle frame. This value will be ignored unless this compass is set as an external compass. When set correctly in the northern hemisphere, pointing the nose and right side down should increase the MagX and MagY values respectively. Rolling the vehicle upside down should decrease the MagZ value. For southern hemisphere, switch increase and decrease. NOTE: For internal compasses, AHRS_ORIENT is used. The label for each option is specified in the order of rotations for that orientation. Firmware versions 4.2 and prior can use a CUSTOM (100) rotation to set the COMPASS_CUS_ROLL/PIT/YAW angles for Compass orientation. Later versions provide two general custom rotations which can be used, Custom 1 and Custom 2, with CUST_1_ROLL/PIT/YAW or CUST_2_ROLL/PIT/YAW angles.
372 : // @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Yaw45Roll180,10:Yaw90Roll180,11:Yaw135Roll180,12:Pitch180,13:Yaw225Roll180,14:Yaw270Roll180,15:Yaw315Roll180,16:Roll90,17:Yaw45Roll90,18:Yaw90Roll90,19:Yaw135Roll90,20:Roll270,21:Yaw45Roll270,22:Yaw90Roll270,23:Yaw135Roll270,24:Pitch90,25:Pitch270,26:Yaw90Pitch180,27:Yaw270Pitch180,28:Pitch90Roll90,29:Pitch90Roll180,30:Pitch90Roll270,31:Pitch180Roll90,32:Pitch180Roll270,33:Pitch270Roll90,34:Pitch270Roll180,35:Pitch270Roll270,36:Yaw90Pitch180Roll90,37:Yaw270Roll90,38:Yaw293Pitch68Roll180,39:Pitch315,40:Pitch315Roll90,42:Roll45,43:Roll315,100:Custom 4.1 and older,101:Custom 1,102:Custom 2
373 : // @User: Advanced
374 : AP_GROUPINFO("ORIENT3", 22, Compass, _state._priv_instance[2].orientation, ROTATION_NONE),
375 :
376 : // @Param: EXTERN3
377 : // @DisplayName: Compass3 is attached via an external cable
378 : // @Description: Configure third compass so it is attached externally. This is auto-detected on most boards. If set to 0 or 1 then auto-detection by bus connection can override the value. If set to 2 then auto-detection will be disabled.
379 : // @Values: 0:Internal,1:External,2:ForcedExternal
380 : // @User: Advanced
381 : AP_GROUPINFO("EXTERN3",23, Compass, _state._priv_instance[2].external, 0),
382 : #endif // COMPASS_MAX_INSTANCES
383 :
384 : #if AP_COMPASS_DIAGONALS_ENABLED
385 : // @Param: DIA_X
386 : // @DisplayName: Compass soft-iron diagonal X component
387 : // @Description: DIA_X in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
388 : // @User: Advanced
389 : // @Calibration: 1
390 :
391 : // @Param: DIA_Y
392 : // @DisplayName: Compass soft-iron diagonal Y component
393 : // @Description: DIA_Y in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
394 : // @User: Advanced
395 : // @Calibration: 1
396 :
397 : // @Param: DIA_Z
398 : // @DisplayName: Compass soft-iron diagonal Z component
399 : // @Description: DIA_Z in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
400 : // @User: Advanced
401 : // @Calibration: 1
402 : AP_GROUPINFO("DIA", 24, Compass, _state._priv_instance[0].diagonals, 1.0),
403 :
404 : // @Param: ODI_X
405 : // @DisplayName: Compass soft-iron off-diagonal X component
406 : // @Description: ODI_X in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
407 : // @User: Advanced
408 : // @Calibration: 1
409 :
410 : // @Param: ODI_Y
411 : // @DisplayName: Compass soft-iron off-diagonal Y component
412 : // @Description: ODI_Y in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
413 : // @User: Advanced
414 : // @Calibration: 1
415 :
416 : // @Param: ODI_Z
417 : // @DisplayName: Compass soft-iron off-diagonal Z component
418 : // @Description: ODI_Z in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
419 : // @User: Advanced
420 : // @Calibration: 1
421 : AP_GROUPINFO("ODI", 25, Compass, _state._priv_instance[0].offdiagonals, 0),
422 :
423 : #if COMPASS_MAX_INSTANCES > 1
424 : // @Param: DIA2_X
425 : // @DisplayName: Compass2 soft-iron diagonal X component
426 : // @Description: DIA_X in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
427 : // @User: Advanced
428 : // @Calibration: 1
429 :
430 : // @Param: DIA2_Y
431 : // @DisplayName: Compass2 soft-iron diagonal Y component
432 : // @Description: DIA_Y in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
433 : // @User: Advanced
434 : // @Calibration: 1
435 :
436 : // @Param: DIA2_Z
437 : // @DisplayName: Compass2 soft-iron diagonal Z component
438 : // @Description: DIA_Z in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
439 : // @User: Advanced
440 : // @Calibration: 1
441 : AP_GROUPINFO("DIA2", 26, Compass, _state._priv_instance[1].diagonals, 1.0),
442 :
443 : // @Param: ODI2_X
444 : // @DisplayName: Compass2 soft-iron off-diagonal X component
445 : // @Description: ODI_X in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
446 : // @User: Advanced
447 : // @Calibration: 1
448 :
449 : // @Param: ODI2_Y
450 : // @DisplayName: Compass2 soft-iron off-diagonal Y component
451 : // @Description: ODI_Y in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
452 : // @User: Advanced
453 : // @Calibration: 1
454 :
455 : // @Param: ODI2_Z
456 : // @DisplayName: Compass2 soft-iron off-diagonal Z component
457 : // @Description: ODI_Z in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
458 : // @User: Advanced
459 : // @Calibration: 1
460 : AP_GROUPINFO("ODI2", 27, Compass, _state._priv_instance[1].offdiagonals, 0),
461 : #endif // COMPASS_MAX_INSTANCES
462 :
463 : #if COMPASS_MAX_INSTANCES > 2
464 : // @Param: DIA3_X
465 : // @DisplayName: Compass3 soft-iron diagonal X component
466 : // @Description: DIA_X in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
467 : // @User: Advanced
468 : // @Calibration: 1
469 :
470 : // @Param: DIA3_Y
471 : // @DisplayName: Compass3 soft-iron diagonal Y component
472 : // @Description: DIA_Y in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
473 : // @User: Advanced
474 : // @Calibration: 1
475 :
476 : // @Param: DIA3_Z
477 : // @DisplayName: Compass3 soft-iron diagonal Z component
478 : // @Description: DIA_Z in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
479 : // @User: Advanced
480 : // @Calibration: 1
481 : AP_GROUPINFO("DIA3", 28, Compass, _state._priv_instance[2].diagonals, 1.0),
482 :
483 : // @Param: ODI3_X
484 : // @DisplayName: Compass3 soft-iron off-diagonal X component
485 : // @Description: ODI_X in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
486 : // @User: Advanced
487 : // @Calibration: 1
488 :
489 : // @Param: ODI3_Y
490 : // @DisplayName: Compass3 soft-iron off-diagonal Y component
491 : // @Description: ODI_Y in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
492 : // @User: Advanced
493 : // @Calibration: 1
494 :
495 : // @Param: ODI3_Z
496 : // @DisplayName: Compass3 soft-iron off-diagonal Z component
497 : // @Description: ODI_Z in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]]
498 : // @User: Advanced
499 : // @Calibration: 1
500 : AP_GROUPINFO("ODI3", 29, Compass, _state._priv_instance[2].offdiagonals, 0),
501 : #endif // COMPASS_MAX_INSTANCES
502 : #endif // AP_COMPASS_DIAGONALS_ENABLED
503 :
504 : #if COMPASS_CAL_ENABLED
505 : // @Param: CAL_FIT
506 : // @DisplayName: Compass calibration fitness
507 : // @Description: This controls the fitness level required for a successful compass calibration. A lower value makes for a stricter fit (less likely to pass). This is the value used for the primary magnetometer. Other magnetometers get double the value.
508 : // @Range: 4 32
509 : // @Values: 4:Very Strict,8:Strict,16:Default,32:Relaxed
510 : // @Increment: 0.1
511 : // @User: Advanced
512 : AP_GROUPINFO("CAL_FIT", 30, Compass, _calibration_threshold, AP_COMPASS_CALIBRATION_FITNESS_DEFAULT),
513 : #endif
514 :
515 : #ifndef HAL_BUILD_AP_PERIPH
516 : // @Param: OFFS_MAX
517 : // @DisplayName: Compass maximum offset
518 : // @Description: This sets the maximum allowed compass offset in calibration and arming checks
519 : // @Range: 500 3000
520 : // @Increment: 1
521 : // @User: Advanced
522 : AP_GROUPINFO("OFFS_MAX", 31, Compass, _offset_max, AP_COMPASS_OFFSETS_MAX_DEFAULT),
523 : #endif
524 :
525 : #if COMPASS_MOT_ENABLED
526 : // @Group: PMOT
527 : // @Path: Compass_PerMotor.cpp
528 : AP_SUBGROUPINFO(_per_motor, "PMOT", 32, Compass, Compass_PerMotor),
529 : #endif
530 :
531 : // @Param: DISBLMSK
532 : // @DisplayName: Compass disable driver type mask
533 : // @Description: This is a bitmask of driver types to disable. If a driver type is set in this mask then that driver will not try to find a sensor at startup
534 : // @Bitmask: 0:HMC5883,1:LSM303D,2:AK8963,3:BMM150,4:LSM9DS1,5:LIS3MDL,6:AK0991x,7:IST8310,8:ICM20948,9:MMC3416,11:DroneCAN,12:QMC5883,14:MAG3110,15:IST8308,16:RM3100,17:MSP,18:ExternalAHRS,19:MMC5XX3,20:QMC5883P,21:BMM350,22:IIS2MDC or LIS2MDL,24:AF9838
535 : // @User: Advanced
536 : AP_GROUPINFO("DISBLMSK", 33, Compass, _driver_type_mask, 0),
537 :
538 : // @Param: FLTR_RNG
539 : // @DisplayName: Range in which sample is accepted
540 : // @Description: This sets the range around the average value that new samples must be within to be accepted. This can help reduce the impact of noise on sensors that are on long I2C cables. The value is a percentage from the average value. A value of zero disables this filter.
541 : // @Units: %
542 : // @Range: 0 100
543 : // @Increment: 1
544 : AP_GROUPINFO("FLTR_RNG", 34, Compass, _filter_range, HAL_COMPASS_FILTER_DEFAULT),
545 :
546 : #if COMPASS_CAL_ENABLED
547 : // @Param: AUTO_ROT
548 : // @DisplayName: Automatically check orientation
549 : // @Description: When enabled this will automatically check the orientation of compasses on successful completion of compass calibration. If set to 2 then external compasses will have their orientation automatically corrected.
550 : // @Values: 0:Disabled,1:CheckOnly,2:CheckAndFix,3:use same tolerance to auto rotate 45 deg rotations
551 : AP_GROUPINFO("AUTO_ROT", 35, Compass, _rotate_auto, HAL_COMPASS_AUTO_ROT_DEFAULT),
552 : #endif
553 :
554 : #if COMPASS_MAX_INSTANCES > 1
555 : // @Param: PRIO1_ID
556 : // @DisplayName: Compass device id with 1st order priority
557 : // @Description: Compass device id with 1st order priority, set automatically if 0. Reboot required after change.
558 : // @RebootRequired: True
559 : // @User: Advanced
560 : AP_GROUPINFO("PRIO1_ID", 36, Compass, _priority_did_stored_list._priv_instance[0], 0),
561 :
562 : // @Param: PRIO2_ID
563 : // @DisplayName: Compass device id with 2nd order priority
564 : // @Description: Compass device id with 2nd order priority, set automatically if 0. Reboot required after change.
565 : // @RebootRequired: True
566 : // @User: Advanced
567 : AP_GROUPINFO("PRIO2_ID", 37, Compass, _priority_did_stored_list._priv_instance[1], 0),
568 : #endif // COMPASS_MAX_INSTANCES
569 :
570 : #if COMPASS_MAX_INSTANCES > 2
571 : // @Param: PRIO3_ID
572 : // @DisplayName: Compass device id with 3rd order priority
573 : // @Description: Compass device id with 3rd order priority, set automatically if 0. Reboot required after change.
574 : // @RebootRequired: True
575 : // @User: Advanced
576 : AP_GROUPINFO("PRIO3_ID", 38, Compass, _priority_did_stored_list._priv_instance[2], 0),
577 : #endif // COMPASS_MAX_INSTANCES
578 :
579 : // @Param: ENABLE
580 : // @DisplayName: Enable Compass
581 : // @Description: Setting this to Enabled(1) will enable the compass. Setting this to Disabled(0) will disable the compass. Note that this is separate from COMPASS_USE. This will enable the low level senor, and will enable logging of magnetometer data. To use the compass for navigation you must also set COMPASS_USE to 1.
582 : // @User: Standard
583 : // @RebootRequired: True
584 : // @Values: 0:Disabled,1:Enabled
585 : AP_GROUPINFO("ENABLE", 39, Compass, _enabled, AP_COMPASS_ENABLE_DEFAULT),
586 :
587 : #ifndef HAL_BUILD_AP_PERIPH
588 : // @Param: SCALE
589 : // @DisplayName: Compass1 scale factor
590 : // @Description: Scaling factor for first compass to compensate for sensor scaling errors. If this is 0 then no scaling is done
591 : // @User: Standard
592 : // @Range: 0 1.3
593 : AP_GROUPINFO("SCALE", 40, Compass, _state._priv_instance[0].scale_factor, 0),
594 :
595 : #if COMPASS_MAX_INSTANCES > 1
596 : // @Param: SCALE2
597 : // @DisplayName: Compass2 scale factor
598 : // @Description: Scaling factor for 2nd compass to compensate for sensor scaling errors. If this is 0 then no scaling is done
599 : // @User: Standard
600 : // @Range: 0 1.3
601 : AP_GROUPINFO("SCALE2", 41, Compass, _state._priv_instance[1].scale_factor, 0),
602 : #endif
603 :
604 : #if COMPASS_MAX_INSTANCES > 2
605 : // @Param: SCALE3
606 : // @DisplayName: Compass3 scale factor
607 : // @Description: Scaling factor for 3rd compass to compensate for sensor scaling errors. If this is 0 then no scaling is done
608 : // @User: Standard
609 : // @Range: 0 1.3
610 : AP_GROUPINFO("SCALE3", 42, Compass, _state._priv_instance[2].scale_factor, 0),
611 : #endif
612 : #endif // HAL_BUILD_AP_PERIPH
613 :
614 : #ifndef HAL_BUILD_AP_PERIPH
615 : // @Param: OPTIONS
616 : // @DisplayName: Compass options
617 : // @Description: This sets options to change the behaviour of the compass
618 : // @Bitmask: 0:CalRequireGPS
619 : // @Bitmask: 1: Allow missing DroneCAN compasses to be automaticaly replaced (calibration still required)
620 : // @User: Advanced
621 : AP_GROUPINFO("OPTIONS", 43, Compass, _options, 0),
622 : #endif
623 :
624 : #if COMPASS_MAX_UNREG_DEV > 0
625 : // @Param: DEV_ID4
626 : // @DisplayName: Compass4 device id
627 : // @Description: Extra 4th compass's device id. Automatically detected, do not set manually
628 : // @ReadOnly: True
629 : // @User: Advanced
630 : AP_GROUPINFO("DEV_ID4", 44, Compass, extra_dev_id[0], 0),
631 : #endif // COMPASS_MAX_UNREG_DEV
632 :
633 : #if COMPASS_MAX_UNREG_DEV > 1
634 : // @Param: DEV_ID5
635 : // @DisplayName: Compass5 device id
636 : // @Description: Extra 5th compass's device id. Automatically detected, do not set manually
637 : // @ReadOnly: True
638 : // @User: Advanced
639 : AP_GROUPINFO("DEV_ID5", 45, Compass, extra_dev_id[1], 0),
640 : #endif // COMPASS_MAX_UNREG_DEV
641 :
642 : #if COMPASS_MAX_UNREG_DEV > 2
643 : // @Param: DEV_ID6
644 : // @DisplayName: Compass6 device id
645 : // @Description: Extra 6th compass's device id. Automatically detected, do not set manually
646 : // @ReadOnly: True
647 : // @User: Advanced
648 : AP_GROUPINFO("DEV_ID6", 46, Compass, extra_dev_id[2], 0),
649 : #endif // COMPASS_MAX_UNREG_DEV
650 :
651 : #if COMPASS_MAX_UNREG_DEV > 3
652 : // @Param: DEV_ID7
653 : // @DisplayName: Compass7 device id
654 : // @Description: Extra 7th compass's device id. Automatically detected, do not set manually
655 : // @ReadOnly: True
656 : // @User: Advanced
657 : AP_GROUPINFO("DEV_ID7", 47, Compass, extra_dev_id[3], 0),
658 : #endif // COMPASS_MAX_UNREG_DEV
659 :
660 : #if COMPASS_MAX_UNREG_DEV > 4
661 : // @Param: DEV_ID8
662 : // @DisplayName: Compass8 device id
663 : // @Description: Extra 8th compass's device id. Automatically detected, do not set manually
664 : // @ReadOnly: True
665 : // @User: Advanced
666 : AP_GROUPINFO("DEV_ID8", 48, Compass, extra_dev_id[4], 0),
667 : #endif // COMPASS_MAX_UNREG_DEV
668 :
669 : // index 49 was CUS_ROLL
670 :
671 : // index 50 was CUS_PIT
672 :
673 : // index 51 was CUS_YAW
674 :
675 : AP_GROUPEND
676 : };
677 :
678 : // Default constructor.
679 : // Note that the Vector/Matrix constructors already implicitly zero
680 : // their values.
681 : //
682 79692 : Compass::Compass(void)
683 : {
684 2098 : if (_singleton != nullptr) {
685 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
686 0 : AP_HAL::panic("Compass must be singleton");
687 : #endif
688 0 : return;
689 : }
690 2098 : _singleton = this;
691 2098 : AP_Param::setup_object_defaults(this, var_info);
692 : }
693 :
694 : // Default init method
695 : //
696 2094 : void Compass::init()
697 : {
698 2094 : if (!_enabled) {
699 1 : return;
700 : }
701 :
702 : /*
703 : on init() if any devid is set then we set suppress_devid_save to
704 : false. This is used to determine if we save device ids during
705 : the init process.
706 : */
707 2093 : suppress_devid_save = true;
708 2978 : for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
709 2683 : if (_state._priv_instance[i].dev_id != 0) {
710 1798 : suppress_devid_save = false;
711 1798 : break;
712 : }
713 : #if COMPASS_MAX_INSTANCES > 1
714 885 : if (_priority_did_stored_list._priv_instance[i] != 0) {
715 0 : suppress_devid_save = false;
716 0 : break;
717 : }
718 : #endif
719 : }
720 :
721 : #if COMPASS_MAX_INSTANCES > 1
722 : // Load priority list from storage, the changes to priority list
723 : // by user only take effect post reboot, after this
724 2093 : if (!suppress_devid_save) {
725 7192 : for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
726 5394 : if (_priority_did_stored_list[i] != 0) {
727 4785 : _priority_did_list[i] = _priority_did_stored_list[i];
728 : } else {
729 : // Maintain a list without gaps and duplicates
730 1218 : for (Priority j(i+1); j<COMPASS_MAX_INSTANCES; j++) {
731 : int32_t temp;
732 609 : if (_priority_did_stored_list[j] == _priority_did_stored_list[i]) {
733 609 : _priority_did_stored_list[j].set_and_save_ifchanged(0);
734 : }
735 609 : if (_priority_did_stored_list[j] == 0) {
736 609 : continue;
737 : }
738 0 : temp = _priority_did_stored_list[j];
739 0 : _priority_did_stored_list[j].set_and_save_ifchanged(0);
740 0 : _priority_did_list[i] = temp;
741 0 : _priority_did_stored_list[i].set_and_save_ifchanged(temp);
742 0 : break;
743 : }
744 : }
745 : }
746 : }
747 : #endif // COMPASS_MAX_INSTANCES
748 :
749 : // cache expected dev ids for use during runtime detection
750 8372 : for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
751 6279 : _state[i].expected_dev_id = _state[i].dev_id;
752 : }
753 :
754 : #if COMPASS_MAX_UNREG_DEV
755 : // set the dev_id to 0 for undetected compasses. extra_dev_id is just an
756 : // interface for users to see unreg compasses, we actually never store it
757 : // in storage.
758 12558 : for (uint8_t i=_unreg_compass_count; i<COMPASS_MAX_UNREG_DEV; i++) {
759 : // cache the extra devices detected in last boot
760 : // for detecting replacement mag
761 10465 : _previously_unreg_mag[i] = extra_dev_id[i];
762 10465 : extra_dev_id[i].set(0);
763 : }
764 : #endif
765 :
766 : #if COMPASS_MAX_INSTANCES > 1
767 : // This method calls set_and_save_ifchanged on parameters
768 : // which are set() but not saved() during normal runtime,
769 : // do not move this call without ensuring that is not happening
770 : // read comments under set_and_save_ifchanged for details
771 2093 : if (!suppress_devid_save) {
772 1798 : _reorder_compass_params();
773 : }
774 : #endif
775 :
776 2093 : if (_compass_count == 0) {
777 : // detect available backends. Only called once
778 2093 : _detect_backends();
779 : }
780 :
781 2093 : if (_compass_count != 0) {
782 : // get initial health status
783 2093 : hal.scheduler->delay(100);
784 2093 : read();
785 : }
786 : // set the dev_id to 0 for undetected compasses, to make it easier
787 : // for users to see how many compasses are detected. We don't do a
788 : // set_and_save() as the user may have temporarily removed the
789 : // compass, and we don't want to force a re-cal if they plug it
790 : // back in again
791 8372 : for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
792 6279 : if (!_state[i].registered) {
793 0 : _state[i].dev_id.set(0);
794 : }
795 : }
796 :
797 : #ifndef HAL_BUILD_AP_PERIPH
798 : // updating the AHRS orientation updates our own orientation:
799 2093 : AP::ahrs().update_orientation();
800 : #endif
801 :
802 2093 : init_done = true;
803 2093 : suppress_devid_save = false;
804 : }
805 :
806 : #if COMPASS_MAX_INSTANCES > 1 || COMPASS_MAX_UNREG_DEV
807 : // Update Priority List for Mags, by default, we just
808 : // load them as they come up the first time
809 78642 : Compass::Priority Compass::_update_priority_list(int32_t dev_id)
810 : {
811 : // Check if already in priority list
812 291280 : for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
813 226376 : if (_priority_did_list[i] == dev_id) {
814 13738 : if (i >= _compass_count) {
815 4780 : _compass_count = uint8_t(i)+1;
816 : }
817 13738 : return i;
818 : }
819 : }
820 :
821 : // We are not in priority list, let's add at first empty
822 256628 : for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
823 193218 : if (_priority_did_stored_list[i] == 0) {
824 1494 : if (suppress_devid_save) {
825 885 : _priority_did_stored_list[i].set(dev_id);
826 : } else {
827 609 : _priority_did_stored_list[i].set_and_save(dev_id);
828 : }
829 1494 : _priority_did_list[i] = dev_id;
830 1494 : if (i >= _compass_count) {
831 1494 : _compass_count = uint8_t(i)+1;
832 : }
833 1494 : return i;
834 : }
835 : }
836 63410 : return Priority(COMPASS_MAX_INSTANCES);
837 : }
838 : #endif
839 :
840 :
841 : #if COMPASS_MAX_INSTANCES > 1
842 : // This method reorganises devid list to match
843 : // priority list, only call before detection at boot
844 1798 : void Compass::_reorder_compass_params()
845 : {
846 1798 : mag_state swap_state;
847 1798 : StateIndex curr_state_id;
848 7192 : for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
849 5394 : if (_priority_did_list[i] == 0) {
850 609 : continue;
851 : }
852 4785 : curr_state_id = COMPASS_MAX_INSTANCES;
853 9586 : for (StateIndex j(0); j<COMPASS_MAX_INSTANCES; j++) {
854 9582 : if (_priority_did_list[i] == _state[j].dev_id) {
855 4781 : curr_state_id = j;
856 4781 : break;
857 : }
858 : }
859 4785 : if (curr_state_id != COMPASS_MAX_INSTANCES && uint8_t(curr_state_id) != uint8_t(i)) {
860 : //let's swap
861 6 : swap_state.copy_from(_state[curr_state_id]);
862 6 : _state[curr_state_id].copy_from(_state[StateIndex(uint8_t(i))]);
863 6 : _state[StateIndex(uint8_t(i))].copy_from(swap_state);
864 : }
865 : }
866 1798 : }
867 : #endif
868 :
869 18 : void Compass::mag_state::copy_from(const Compass::mag_state& state)
870 : {
871 18 : external.set_and_save_ifchanged(state.external);
872 18 : orientation.set_and_save_ifchanged(state.orientation);
873 18 : offset.set_and_save_ifchanged(state.offset);
874 : #if AP_COMPASS_DIAGONALS_ENABLED
875 18 : diagonals.set_and_save_ifchanged(state.diagonals);
876 18 : offdiagonals.set_and_save_ifchanged(state.offdiagonals);
877 : #endif
878 18 : scale_factor.set_and_save_ifchanged(state.scale_factor);
879 18 : dev_id.set_and_save_ifchanged(state.dev_id);
880 18 : motor_compensation.set_and_save_ifchanged(state.motor_compensation);
881 18 : expected_dev_id = state.expected_dev_id;
882 18 : detected_dev_id = state.detected_dev_id;
883 18 : }
884 : // Register a new compass instance
885 : //
886 12620 : bool Compass::register_compass(int32_t dev_id, uint8_t& instance)
887 : {
888 :
889 : #if COMPASS_MAX_INSTANCES == 1 && !COMPASS_MAX_UNREG_DEV
890 : // simple single compass setup for AP_Periph
891 : Priority priority(0);
892 : StateIndex i(0);
893 : if (_state[i].registered) {
894 : return false;
895 : }
896 : _state[i].registered = true;
897 : _state[i].priority = priority;
898 : instance = uint8_t(i);
899 : _compass_count = 1;
900 : return true;
901 : #else
902 12620 : Priority priority;
903 : // Check if we already have this dev_id registered
904 39699 : for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
905 32469 : priority = _update_priority_list(dev_id);
906 32469 : if (_state[i].expected_dev_id == dev_id && priority < COMPASS_MAX_INSTANCES) {
907 5390 : _state[i].registered = true;
908 5390 : _state[i].priority = priority;
909 5390 : instance = uint8_t(i);
910 5390 : return true;
911 : }
912 : }
913 :
914 : // This is an unregistered compass, check if any free slot is available
915 27150 : for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
916 20805 : priority = _update_priority_list(dev_id);
917 20805 : if (_state[i].dev_id == 0 && priority < COMPASS_MAX_INSTANCES) {
918 885 : _state[i].registered = true;
919 885 : _state[i].priority = priority;
920 885 : instance = uint8_t(i);
921 885 : return true;
922 : }
923 : }
924 :
925 : // This might be a replacement compass module, find any unregistered compass
926 : // instance and replace that
927 6345 : priority = _update_priority_list(dev_id);
928 : // try to match priority and state index if possible, this ensure that compass order
929 : // to state order while detection is preserved, this ensures that if compasses in priority
930 : // list show up out of order during detection, it does not replace the state.
931 6345 : StateIndex priority_index = StateIndex(uint8_t(priority));
932 6345 : if (!_state[priority_index].registered && priority < COMPASS_MAX_INSTANCES) {
933 4 : _state[priority_index].registered = true;
934 4 : _state[priority_index].priority = priority;
935 4 : instance = uint8_t(priority_index);
936 4 : return true;
937 : }
938 25364 : for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
939 19023 : priority = _update_priority_list(dev_id);
940 19023 : if (!_state[i].registered && priority < COMPASS_MAX_INSTANCES) {
941 0 : _state[i].registered = true;
942 0 : _state[i].priority = priority;
943 0 : instance = uint8_t(i);
944 0 : return true;
945 : }
946 : }
947 : #endif
948 :
949 : #if COMPASS_MAX_UNREG_DEV
950 : // Set extra dev id
951 6341 : if (_unreg_compass_count >= ARRAY_SIZE(extra_dev_id)) {
952 0 : return false;
953 : }
954 :
955 12806 : for (uint8_t i=0; i<COMPASS_MAX_UNREG_DEV; i++) {
956 12806 : if (extra_dev_id[i] == dev_id) {
957 0 : if (i >= _unreg_compass_count) {
958 0 : _unreg_compass_count = i+1;
959 : }
960 0 : instance = i+COMPASS_MAX_INSTANCES;
961 0 : return false;
962 12806 : } else if (extra_dev_id[i] == 0) {
963 6341 : extra_dev_id[_unreg_compass_count++].set(dev_id);
964 6341 : instance = i+COMPASS_MAX_INSTANCES;
965 6341 : return false;
966 : }
967 : }
968 : #endif // COMPASS_MAX_UNREG_DEV
969 :
970 0 : return false;
971 : }
972 :
973 953951840 : Compass::StateIndex Compass::_get_state_id(Compass::Priority priority) const
974 : {
975 : #if COMPASS_MAX_INSTANCES > 1
976 953951840 : if (_priority_did_list[priority] == 0) {
977 32632 : return StateIndex(COMPASS_MAX_INSTANCES);
978 : }
979 1763759927 : for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
980 1763759925 : if (_priority_did_list[priority] == _state[i].detected_dev_id) {
981 953919210 : return i;
982 : }
983 : }
984 0 : return StateIndex(COMPASS_MAX_INSTANCES);
985 : #else
986 : return StateIndex(0);
987 : #endif
988 : }
989 :
990 : /*
991 : return true if a driver type is enabled
992 : */
993 206696 : bool Compass::_driver_enabled(enum DriverType driver_type)
994 : {
995 206696 : uint32_t mask = (1U<<uint8_t(driver_type));
996 206696 : return (mask & uint32_t(_driver_type_mask.get())) == 0;
997 : }
998 :
999 : /*
1000 : wrapper around hal.i2c_mgr->get_device() that prevents duplicate devices being opened
1001 : */
1002 0 : bool Compass::_i2c_sensor_is_registered(uint8_t bus, uint8_t address) const
1003 : {
1004 0 : for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
1005 0 : if (!_state[i].registered) {
1006 0 : continue;
1007 : }
1008 0 : if (AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_I2C, bus, address, 0) ==
1009 0 : AP_HAL::Device::change_bus_id(uint32_t(_state[i].dev_id.get()), 0)) {
1010 : // we are already using this device
1011 0 : return true;
1012 : }
1013 : }
1014 0 : return false;
1015 : }
1016 :
1017 : #if COMPASS_MAX_UNREG_DEV > 0
1018 : #define RETURN_IF_NO_SPACE if (_unreg_compass_count == COMPASS_MAX_UNREG_DEV) return
1019 : #else
1020 : #define RETURN_IF_NO_SPACE
1021 : #endif
1022 :
1023 : /*
1024 : macro to add a backend with check for too many backends or compass
1025 : instances. We don't try to start more than the maximum allowed
1026 : */
1027 16806 : void Compass::add_backend(Compass::DriverType driver_type, AP_Compass_Backend *backend)
1028 : {
1029 16806 : if (!_driver_enabled(driver_type)) {
1030 0 : return;
1031 : }
1032 :
1033 16806 : if (!backend) {
1034 62 : return;
1035 : }
1036 :
1037 16744 : if (_backend_count == COMPASS_MAX_BACKEND) {
1038 10465 : return;
1039 : }
1040 :
1041 6279 : _backends[_backend_count++] = backend;
1042 : }
1043 :
1044 : #define GET_I2C_DEVICE(bus, address) _i2c_sensor_is_registered(bus, address)?nullptr:hal.i2c_mgr->get_device(bus, address)
1045 :
1046 : /*
1047 : look for compasses on external i2c buses
1048 : */
1049 0 : void Compass::_probe_external_i2c_compasses(void)
1050 : {
1051 : #if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1052 : bool all_external = (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2);
1053 : (void)all_external; // in case all backends using this are compiled out
1054 : #endif // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1055 :
1056 : #if AP_COMPASS_HMC5843_ENABLED
1057 : // external i2c bus
1058 0 : FOREACH_I2C_EXTERNAL(i) {
1059 0 : probe_i2c_dev(DRIVER_HMC5843, AP_Compass_HMC5843::probe, i, HAL_COMPASS_HMC5843_I2C_ADDR, true, ROTATION_ROLL_180);
1060 0 : RETURN_IF_NO_SPACE;
1061 : }
1062 :
1063 : #if AP_COMPASS_HMC5843_INTERNAL_BUS_PROBING_ENABLED
1064 : if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_AEROFC) {
1065 : // internal i2c bus
1066 : FOREACH_I2C_INTERNAL(i) {
1067 : probe_i2c_dev(DRIVER_HMC5843, AP_Compass_HMC5843::probe, i, HAL_COMPASS_HMC5843_I2C_ADDR, all_external, all_external?ROTATION_ROLL_180:ROTATION_YAW_270);
1068 : RETURN_IF_NO_SPACE;
1069 : }
1070 : }
1071 : #endif // AP_COMPASS_HMC5843_INTERNAL_BUS_PROBING_ENABLED
1072 : #endif // AP_COMPASS_HMC5843_ENABLED
1073 :
1074 : #if AP_COMPASS_QMC5883L_ENABLED
1075 : //external i2c bus
1076 0 : FOREACH_I2C_EXTERNAL(i) {
1077 0 : probe_i2c_dev(DRIVER_QMC5883L, AP_Compass_QMC5883L::probe, i, HAL_COMPASS_QMC5883L_I2C_ADDR, true, HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL);
1078 0 : RETURN_IF_NO_SPACE;
1079 : }
1080 :
1081 : // internal i2c bus
1082 : #if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1083 : if (all_external) {
1084 : // only probe QMC5883L on internal if we are treating internals as externals
1085 : FOREACH_I2C_INTERNAL(i) {
1086 : probe_i2c_dev(DRIVER_QMC5883L, AP_Compass_QMC5883L::probe, i, HAL_COMPASS_QMC5883L_I2C_ADDR, all_external, all_external?HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL:HAL_COMPASS_QMC5883L_ORIENTATION_INTERNAL);
1087 : RETURN_IF_NO_SPACE;
1088 : }
1089 : }
1090 : #endif // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1091 : #endif // AP_COMPASS_QMC5883L_ENABLED
1092 :
1093 : #if AP_COMPASS_QMC5883P_ENABLED
1094 : //external i2c bus
1095 0 : FOREACH_I2C_EXTERNAL(i) {
1096 0 : probe_i2c_dev(DRIVER_QMC5883P, AP_Compass_QMC5883P::probe, i, HAL_COMPASS_QMC5883P_I2C_ADDR, true, HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL);
1097 0 : RETURN_IF_NO_SPACE;
1098 : }
1099 :
1100 : // internal i2c bus
1101 : #if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1102 : if (all_external) {
1103 : // only probe QMC5883P on internal if we are treating internals as externals
1104 : FOREACH_I2C_INTERNAL(i) {
1105 : probe_i2c_dev(DRIVER_QMC5883P, AP_Compass_QMC5883P::probe, i, HAL_COMPASS_QMC5883P_I2C_ADDR, all_external, all_external?HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL:HAL_COMPASS_QMC5883P_ORIENTATION_INTERNAL);
1106 : RETURN_IF_NO_SPACE;
1107 : }
1108 : }
1109 : #endif // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1110 : #endif // AP_COMPASS_QMC5883P_ENABLED
1111 :
1112 : #if AP_COMPASS_IIS2MDC_ENABLED
1113 : //external i2c bus
1114 : #if AP_COMPASS_IIS2MDC_EXTERNAL_BUS_PROBING_ENABLED
1115 : FOREACH_I2C_EXTERNAL(i) {
1116 : probe_i2c_dev(DRIVER_IIS2MDC, AP_Compass_IIS2MDC::probe, i, HAL_COMPASS_IIS2MDC_I2C_ADDR, true, HAL_COMPASS_IIS2MDC_ORIENTATION_EXTERNAL);
1117 : RETURN_IF_NO_SPACE;
1118 : }
1119 : #endif // AP_COMPASS_IIS2MDC_EXTERNAL_BUS_PROBING_ENABLED
1120 :
1121 : // internal i2c bus
1122 : #if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1123 : if (all_external) {
1124 : // only probe IIS2MDC on internal if we are treating internals as externals
1125 : FOREACH_I2C_INTERNAL(i) {
1126 : probe_i2c_dev(DRIVER_IIS2MDC, AP_Compass_IIS2MDC::probe, i, HAL_COMPASS_IIS2MDC_I2C_ADDR, all_external, all_external?HAL_COMPASS_IIS2MDC_ORIENTATION_EXTERNAL:HAL_COMPASS_IIS2MDC_ORIENTATION_INTERNAL);
1127 : RETURN_IF_NO_SPACE;
1128 : }
1129 : }
1130 : #endif // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1131 : #endif // AP_COMPASS_IIS2MDC_ENABLED
1132 :
1133 : // AK09916 on ICM20948
1134 : #if AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED
1135 0 : FOREACH_I2C_EXTERNAL(i) {
1136 0 : probe_ak09916_via_icm20948(i, HAL_COMPASS_AK09916_I2C_ADDR, HAL_COMPASS_ICM20948_I2C_ADDR, true, ROTATION_PITCH_180_YAW_90);
1137 0 : RETURN_IF_NO_SPACE;
1138 0 : probe_ak09916_via_icm20948(i, HAL_COMPASS_AK09916_I2C_ADDR, HAL_COMPASS_ICM20948_I2C_ADDR2, true, ROTATION_PITCH_180_YAW_90);
1139 0 : RETURN_IF_NO_SPACE;
1140 : }
1141 :
1142 : #if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1143 : FOREACH_I2C_INTERNAL(i) {
1144 : probe_ak09916_via_icm20948(i, HAL_COMPASS_AK09916_I2C_ADDR, HAL_COMPASS_ICM20948_I2C_ADDR, all_external, ROTATION_PITCH_180_YAW_90);
1145 : RETURN_IF_NO_SPACE;
1146 : probe_ak09916_via_icm20948(i, HAL_COMPASS_AK09916_I2C_ADDR, HAL_COMPASS_ICM20948_I2C_ADDR2, all_external, ROTATION_PITCH_180_YAW_90);
1147 : RETURN_IF_NO_SPACE;
1148 : }
1149 : #endif // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1150 : #endif // AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED
1151 :
1152 : #if AP_COMPASS_LIS3MDL_ENABLED
1153 : // lis3mdl on bus 0 with default address
1154 : #if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1155 : FOREACH_I2C_INTERNAL(i) {
1156 : probe_i2c_dev(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe, i, HAL_COMPASS_LIS3MDL_I2C_ADDR, all_external, all_external?ROTATION_YAW_90:ROTATION_NONE);
1157 : RETURN_IF_NO_SPACE;
1158 : }
1159 :
1160 : // lis3mdl on bus 0 with alternate address
1161 : FOREACH_I2C_INTERNAL(i) {
1162 : probe_i2c_dev(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe, i, HAL_COMPASS_LIS3MDL_I2C_ADDR2, all_external, all_external?ROTATION_YAW_90:ROTATION_NONE);
1163 : RETURN_IF_NO_SPACE;
1164 : }
1165 : #endif
1166 : // external lis3mdl on bus 1 with default address
1167 0 : FOREACH_I2C_EXTERNAL(i) {
1168 0 : probe_i2c_dev(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe, i, HAL_COMPASS_LIS3MDL_I2C_ADDR, true, ROTATION_YAW_90);
1169 0 : RETURN_IF_NO_SPACE;
1170 : }
1171 :
1172 : // external lis3mdl on bus 1 with alternate address
1173 0 : FOREACH_I2C_EXTERNAL(i) {
1174 0 : probe_i2c_dev(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe, i, HAL_COMPASS_LIS3MDL_I2C_ADDR2, true, ROTATION_YAW_90);
1175 0 : RETURN_IF_NO_SPACE;
1176 : }
1177 : #endif // AP_COMPASS_LIS3MDL_ENABLED
1178 :
1179 : #if AP_COMPASS_AK09916_ENABLED
1180 : // AK09916. This can be found twice, due to the ICM20948 i2c bus pass-thru, so we need to be careful to avoid that
1181 0 : FOREACH_I2C_EXTERNAL(i) {
1182 0 : probe_i2c_dev(DRIVER_AK09916, AP_Compass_AK09916::probe, i, HAL_COMPASS_AK09916_I2C_ADDR, true, ROTATION_YAW_270);
1183 0 : RETURN_IF_NO_SPACE;
1184 : }
1185 : #if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1186 : FOREACH_I2C_INTERNAL(i) {
1187 : probe_i2c_dev(DRIVER_AK09916, AP_Compass_AK09916::probe, i, HAL_COMPASS_AK09916_I2C_ADDR, all_external, all_external?ROTATION_YAW_270:ROTATION_NONE);
1188 : RETURN_IF_NO_SPACE;
1189 : }
1190 : #endif // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1191 : #endif // AP_COMPASS_AK09916_ENABLED
1192 :
1193 : #if AP_COMPASS_IST8310_EXTERNAL_BUS_PROBING_ENABLED || AP_COMPASS_IST8310_INTERNAL_BUS_PROBING_ENABLED
1194 : // IST8310 on external and internal bus
1195 : if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_FMUV6) {
1196 : enum Rotation default_rotation = AP_COMPASS_IST8310_DEFAULT_ROTATION;
1197 :
1198 : if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_AEROFC) {
1199 : default_rotation = ROTATION_PITCH_180_YAW_90;
1200 : }
1201 : // probe all 4 possible addresses
1202 : const uint8_t ist8310_addr[] = { 0x0C, 0x0D, 0x0E, 0x0F };
1203 :
1204 : for (uint8_t a=0; a<ARRAY_SIZE(ist8310_addr); a++) {
1205 : #if AP_COMPASS_IST8310_EXTERNAL_BUS_PROBING_ENABLED
1206 : FOREACH_I2C_EXTERNAL(i) {
1207 : probe_i2c_dev(DRIVER_IST8310, AP_Compass_IST8310::probe, i, ist8310_addr[a], true, default_rotation);
1208 : RETURN_IF_NO_SPACE;
1209 : }
1210 : #endif // AP_COMPASS_IST8310_EXTERNAL_BUS_PROBING_ENABLED
1211 : #if AP_COMPASS_IST8310_INTERNAL_BUS_PROBING_ENABLED
1212 : FOREACH_I2C_INTERNAL(i) {
1213 : probe_i2c_dev(DRIVER_IST8310, AP_Compass_IST8310::probe, i, ist8310_addr[a], all_external, default_rotation);
1214 : RETURN_IF_NO_SPACE;
1215 : }
1216 : #endif // AP_COMPASS_IST8310_INTERNAL_BUS_PROBING_ENABLED
1217 : }
1218 : }
1219 : #endif // AP_COMPASS_IST8310_EXTERNAL_BUS_PROBING_ENABLED || AP_COMPASS_IST8310_INTERNAL_BUS_PROBING_ENABLED
1220 :
1221 : #if AP_COMPASS_IST8308_ENABLED
1222 : // external i2c bus
1223 0 : FOREACH_I2C_EXTERNAL(i) {
1224 0 : probe_i2c_dev(DRIVER_IST8308, AP_Compass_IST8308::probe, i, HAL_COMPASS_IST8308_I2C_ADDR, true, ROTATION_NONE);
1225 0 : RETURN_IF_NO_SPACE;
1226 : }
1227 : #if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1228 : FOREACH_I2C_INTERNAL(i) {
1229 : probe_i2c_dev(DRIVER_IST8308, AP_Compass_IST8308::probe, i, HAL_COMPASS_IST8308_I2C_ADDR, all_external, ROTATION_NONE);
1230 : RETURN_IF_NO_SPACE;
1231 : }
1232 : #endif // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1233 : #endif // AP_COMPASS_IST8308_ENABLED
1234 :
1235 : #if AP_COMPASS_MMC3416_ENABLED
1236 : // external i2c bus
1237 0 : FOREACH_I2C_EXTERNAL(i) {
1238 0 : probe_i2c_dev(DRIVER_MMC3416, AP_Compass_MMC3416::probe, i, HAL_COMPASS_MMC3416_I2C_ADDR, true, ROTATION_NONE);
1239 0 : RETURN_IF_NO_SPACE;
1240 : }
1241 : #if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1242 : FOREACH_I2C_INTERNAL(i) {
1243 : probe_i2c_dev(DRIVER_MMC3416, AP_Compass_MMC3416::probe, i, HAL_COMPASS_MMC3416_I2C_ADDR, all_external, ROTATION_NONE);
1244 : RETURN_IF_NO_SPACE;
1245 : }
1246 : #endif // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1247 : #endif // AP_COMPASS_MMC3416_ENABLED
1248 :
1249 : #if AP_COMPASS_MMC5XX3_ENABLED
1250 : // external i2c bus
1251 0 : FOREACH_I2C_EXTERNAL(i) {
1252 0 : probe_i2c_dev(DRIVER_MMC5XX3, AP_Compass_MMC5XX3::probe, i, HAL_COMPASS_MMC5xx3_I2C_ADDR, true, ROTATION_NONE);
1253 0 : RETURN_IF_NO_SPACE;
1254 : }
1255 : #endif // AP_COMPASS_MMC5XX3_ENABLED (MMC5983MA)
1256 :
1257 : #if AP_COMPASS_AF9838_ENABLED
1258 : // AF9838 on external I2C buses
1259 0 : FOREACH_I2C_EXTERNAL(i) {
1260 0 : probe_i2c_dev(DRIVER_AF9838, AP_Compass_AF9838::probe, i,
1261 : HAL_COMPASS_AF9838_I2C_ADDR, true, ROTATION_NONE);
1262 0 : RETURN_IF_NO_SPACE;
1263 : }
1264 :
1265 : #if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1266 : // AF9838 on internal HAL I2C buses
1267 : FOREACH_I2C_INTERNAL(i) {
1268 : probe_i2c_dev(DRIVER_AF9838, AP_Compass_AF9838::probe, i,
1269 : HAL_COMPASS_AF9838_I2C_ADDR, all_external, ROTATION_NONE);
1270 : RETURN_IF_NO_SPACE;
1271 : }
1272 : #endif // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1273 : #endif // AP_COMPASS_AF9838_ENABLED
1274 :
1275 : #if AP_COMPASS_RM3100_ENABLED
1276 : #ifdef HAL_COMPASS_RM3100_I2C_ADDR
1277 : const uint8_t rm3100_addresses[] = { HAL_COMPASS_RM3100_I2C_ADDR };
1278 : #else
1279 : // RM3100 can be on 4 different addresses
1280 0 : const uint8_t rm3100_addresses[] = { HAL_COMPASS_RM3100_I2C_ADDR1,
1281 : HAL_COMPASS_RM3100_I2C_ADDR2,
1282 : HAL_COMPASS_RM3100_I2C_ADDR3,
1283 : HAL_COMPASS_RM3100_I2C_ADDR4 };
1284 : #endif
1285 : // external i2c bus
1286 0 : FOREACH_I2C_EXTERNAL(i) {
1287 0 : for (uint8_t j=0; j<ARRAY_SIZE(rm3100_addresses); j++) {
1288 0 : probe_i2c_dev(DRIVER_RM3100, AP_Compass_RM3100::probe, i, rm3100_addresses[j], true, ROTATION_NONE);
1289 0 : RETURN_IF_NO_SPACE;
1290 : }
1291 : }
1292 :
1293 : #if AP_COMPASS_RM3100_INTERNAL_BUS_PROBING_ENABLED
1294 : FOREACH_I2C_INTERNAL(i) {
1295 : for (uint8_t j=0; j<ARRAY_SIZE(rm3100_addresses); j++) {
1296 : probe_i2c_dev(DRIVER_RM3100, AP_Compass_RM3100::probe, i, rm3100_addresses[j], all_external, ROTATION_NONE);
1297 : RETURN_IF_NO_SPACE;
1298 : }
1299 : }
1300 : #endif // AP_COMPASS_RM3100_INTERNAL_BUS_PROBING_ENABLED
1301 : #endif // AP_COMPASS_RM3100_ENABLED
1302 :
1303 : #if AP_COMPASS_BMM150_ENABLED
1304 : // BMM150 on I2C
1305 0 : FOREACH_I2C_EXTERNAL(i) {
1306 0 : for (uint8_t addr=BMM150_I2C_ADDR_MIN; addr <= BMM150_I2C_ADDR_MAX; addr++) {
1307 0 : probe_i2c_dev(DRIVER_BMM150, AP_Compass_BMM150::probe, i, addr, true, ROTATION_NONE);
1308 0 : RETURN_IF_NO_SPACE;
1309 : }
1310 : }
1311 : #endif // AP_COMPASS_BMM150_ENABLED
1312 :
1313 : #if AP_COMPASS_BMM350_ENABLED
1314 : // BMM350 on I2C
1315 0 : FOREACH_I2C_EXTERNAL(i) {
1316 0 : for (uint8_t addr=BMM350_I2C_ADDR_MIN; addr <= BMM350_I2C_ADDR_MAX; addr++) {
1317 0 : probe_i2c_dev(DRIVER_BMM350, AP_Compass_BMM350::probe, i, addr, true, ROTATION_NONE);
1318 0 : RETURN_IF_NO_SPACE;
1319 : }
1320 : }
1321 : #if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1322 : FOREACH_I2C_INTERNAL(i) {
1323 : for (uint8_t addr=BMM350_I2C_ADDR_MIN; addr <= BMM350_I2C_ADDR_MAX; addr++) {
1324 : probe_i2c_dev(DRIVER_BMM350, AP_Compass_BMM350::probe, i, addr, all_external, ROTATION_NONE);
1325 : RETURN_IF_NO_SPACE;
1326 : }
1327 : }
1328 : #endif // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1329 : #endif // AP_COMPASS_BMM350_ENABLED
1330 : }
1331 :
1332 : /*
1333 : detect available backends for this board
1334 : */
1335 2093 : void Compass::_detect_backends(void)
1336 : {
1337 : #if AP_COMPASS_EXTERNALAHRS_ENABLED
1338 2093 : const int8_t serial_port = AP::externalAHRS().get_port(AP_ExternalAHRS::AvailableSensor::COMPASS);
1339 2093 : if (serial_port >= 0) {
1340 62 : add_backend(DRIVER_EXTERNALAHRS, AP_Compass_ExternalAHRS::probe(serial_port));
1341 62 : RETURN_IF_NO_SPACE;
1342 : }
1343 : #endif
1344 :
1345 : #if AP_FEATURE_BOARD_DETECT
1346 : if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2) {
1347 : // default to disabling LIS3MDL on pixhawk2 due to hardware issue
1348 : #if AP_COMPASS_LIS3MDL_ENABLED
1349 : _driver_type_mask.set_default(1U<<DRIVER_LIS3MDL);
1350 : #endif
1351 : }
1352 : #endif
1353 :
1354 : #if AP_COMPASS_SITL_ENABLED && !AP_TEST_DRONECAN_DRIVERS
1355 : // create several SITL compass backends:
1356 18837 : for (uint8_t i=0; i<MAX_CONNECTED_MAGS; i++) {
1357 16744 : add_backend(DRIVER_SITL, NEW_NOTHROW AP_Compass_SITL(i));
1358 16744 : RETURN_IF_NO_SPACE;
1359 : }
1360 : #endif
1361 :
1362 : #if AP_COMPASS_DRONECAN_ENABLED
1363 : // probe DroneCAN before I2C and SPI so that DroneCAN compasses
1364 : // default to first in the list for a new board
1365 2093 : probe_dronecan_compasses();
1366 2093 : RETURN_IF_NO_SPACE;
1367 : #endif
1368 :
1369 : #if AP_COMPASS_PROBING_ENABLED
1370 : // allow boards to ask for external probing of all i2c compass types in hwdef.dat
1371 : _probe_external_i2c_compasses();
1372 : RETURN_IF_NO_SPACE;
1373 : #endif // AP_COMPASS_PROBING_ENABLED
1374 :
1375 : #if AP_COMPASS_MSP_ENABLED
1376 18837 : for (uint8_t i=0; i<8; i++) {
1377 16744 : if (msp_instance_mask & (1U<<i)) {
1378 0 : auto *backend = AP_Compass_MSP::probe(i);
1379 0 : if (backend == nullptr) {
1380 0 : continue;
1381 : }
1382 0 : add_backend(DRIVER_MSP, backend);
1383 0 : RETURN_IF_NO_SPACE;
1384 : }
1385 : }
1386 : #endif
1387 :
1388 : // finally look for i2c and spi compasses not found yet
1389 2093 : RETURN_IF_NO_SPACE;
1390 2093 : probe_i2c_spi_compasses();
1391 :
1392 2093 : if (_backend_count == 0 ||
1393 2093 : _compass_count == 0) {
1394 0 : DEV_PRINTF("No Compass backends available\n");
1395 : }
1396 : }
1397 :
1398 : /*
1399 : probe i2c and SPI compasses
1400 : */
1401 2093 : void Compass::probe_i2c_spi_compasses(void)
1402 : {
1403 : #if defined(HAL_MAG_PROBE_LIST)
1404 : // driver probes defined by COMPASS lines in hwdef.dat
1405 : HAL_MAG_PROBE_LIST;
1406 : #elif AP_FEATURE_BOARD_DETECT
1407 : switch (AP_BoardConfig::get_board_type()) {
1408 : case AP_BoardConfig::PX4_BOARD_PX4V1:
1409 : case AP_BoardConfig::PX4_BOARD_PIXHAWK:
1410 : case AP_BoardConfig::PX4_BOARD_PHMINI:
1411 : case AP_BoardConfig::PX4_BOARD_AUAV21:
1412 : case AP_BoardConfig::PX4_BOARD_PH2SLIM:
1413 : case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
1414 : case AP_BoardConfig::PX4_BOARD_FMUV6:
1415 : case AP_BoardConfig::PX4_BOARD_AEROFC:
1416 : _probe_external_i2c_compasses();
1417 : RETURN_IF_NO_SPACE;
1418 : break;
1419 :
1420 : default:
1421 : break;
1422 : }
1423 : switch (AP_BoardConfig::get_board_type()) {
1424 : case AP_BoardConfig::PX4_BOARD_PIXHAWK:
1425 : #if AP_COMPASS_HMC5843_ENABLED
1426 : probe_spi_dev(DRIVER_HMC5843, AP_Compass_HMC5843::probe, HAL_COMPASS_HMC5843_NAME, false, ROTATION_PITCH_180);
1427 : RETURN_IF_NO_SPACE;
1428 : #endif
1429 : #if AP_COMPASS_LSM303D_ENABLED
1430 : probe_spi_dev(DRIVER_LSM303D, AP_Compass_LSM303D::probe, HAL_INS_LSM9DS0_A_NAME, ROTATION_NONE);
1431 : RETURN_IF_NO_SPACE;
1432 : #endif
1433 : break;
1434 :
1435 : case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
1436 : #if AP_COMPASS_LSM303D_ENABLED
1437 : probe_spi_dev(DRIVER_LSM303D, AP_Compass_LSM303D::probe, HAL_INS_LSM9DS0_EXT_A_NAME, ROTATION_YAW_270);
1438 : RETURN_IF_NO_SPACE;
1439 : #endif
1440 : #if AP_COMPASS_AK8963_ENABLED
1441 : // we run the AK8963 only on the 2nd MPU9250, which leaves the
1442 : // first MPU9250 to run without disturbance at high rate
1443 : probe_ak8963_via_mpu9250(1, ROTATION_YAW_270);
1444 : RETURN_IF_NO_SPACE;
1445 : #endif
1446 : #if AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED
1447 : probe_ak09916_via_icm20948(0, ROTATION_ROLL_180_YAW_90);
1448 : RETURN_IF_NO_SPACE;
1449 : #endif
1450 : break;
1451 :
1452 : case AP_BoardConfig::PX4_BOARD_FMUV6:
1453 : #if AP_COMPASS_IST8310_ENABLED
1454 : FOREACH_I2C_EXTERNAL(i) {
1455 : probe_i2c_dev(DRIVER_IST8310, AP_Compass_IST8310::probe, i, HAL_COMPASS_IST8310_I2C_ADDR, true, ROTATION_ROLL_180_YAW_90);
1456 : RETURN_IF_NO_SPACE;
1457 : }
1458 : FOREACH_I2C_INTERNAL(i) {
1459 : probe_i2c_dev(DRIVER_IST8310, AP_Compass_IST8310::probe, i, HAL_COMPASS_IST8310_I2C_ADDR, false, ROTATION_ROLL_180_YAW_90);
1460 : RETURN_IF_NO_SPACE;
1461 : }
1462 : #endif // AP_COMPASS_IST8310_ENABLED
1463 : break;
1464 :
1465 : case AP_BoardConfig::PX4_BOARD_PHMINI:
1466 : #if AP_COMPASS_AK8963_ENABLED
1467 : probe_ak8963_via_mpu9250(0, ROTATION_ROLL_180);
1468 : RETURN_IF_NO_SPACE;
1469 : #endif
1470 : break;
1471 :
1472 : case AP_BoardConfig::PX4_BOARD_AUAV21:
1473 : #if AP_COMPASS_AK8963_ENABLED
1474 : probe_ak8963_via_mpu9250(0, ROTATION_ROLL_180_YAW_90);
1475 : RETURN_IF_NO_SPACE;
1476 : #endif
1477 : break;
1478 :
1479 : case AP_BoardConfig::PX4_BOARD_PH2SLIM:
1480 : #if AP_COMPASS_AK8963_ENABLED
1481 : probe_ak8963_via_mpu9250(0, ROTATION_YAW_270);
1482 : RETURN_IF_NO_SPACE;
1483 : #endif
1484 : break;
1485 :
1486 : default:
1487 : break;
1488 : }
1489 : #endif
1490 2093 : }
1491 :
1492 : #if AP_COMPASS_AK8963_ENABLED
1493 0 : void Compass::probe_ak8963_via_mpu9250(uint8_t imu_instance, Rotation rotation)
1494 : {
1495 0 : if (!_driver_enabled(DRIVER_AK8963)) {
1496 0 : return;
1497 : }
1498 0 : auto *backend = AP_Compass_AK8963::probe_mpu9250(imu_instance, rotation);
1499 0 : add_backend(DRIVER_AK8963, backend); // add_backend does nullptr check
1500 : }
1501 : #endif // AP_COMPASS_AK8963_ENABLED
1502 :
1503 : #if AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED
1504 0 : void Compass::probe_ak09916_via_icm20948(uint8_t i2c_bus, uint8_t ak09916_addr, uint8_t icm20948_addr, bool external, Rotation rotation)
1505 : {
1506 0 : if (!_driver_enabled(DRIVER_ICM20948)) {
1507 0 : return;
1508 : }
1509 0 : auto *backend = AP_Compass_AK09916::probe_ICM20948(
1510 0 : GET_I2C_DEVICE(i2c_bus, ak09916_addr),
1511 0 : GET_I2C_DEVICE(i2c_bus, icm20948_addr),
1512 : external,
1513 : rotation
1514 : );
1515 :
1516 0 : add_backend(DRIVER_ICM20948, backend); // add_backend does nullptr check
1517 : }
1518 :
1519 0 : void Compass::probe_ak09916_via_icm20948(uint8_t ins_instance, Rotation rotation)
1520 : {
1521 0 : if (!_driver_enabled(DRIVER_AK09916)) {
1522 0 : return;
1523 : }
1524 :
1525 0 : auto *backend = AP_Compass_AK09916::probe_ICM20948(ins_instance, rotation);
1526 :
1527 0 : add_backend(DRIVER_AK09916, backend);
1528 : }
1529 :
1530 : #endif // #if AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED
1531 :
1532 0 : void Compass::probe_i2c_dev(DriverType driver_type, probe_i2c_dev_probefn_t probefn, uint8_t i2c_bus, uint8_t i2c_addr, bool external, Rotation rotation)
1533 : {
1534 0 : if (!_driver_enabled(driver_type)) {
1535 0 : return;
1536 : }
1537 0 : auto *backend = probefn(
1538 0 : GET_I2C_DEVICE(i2c_bus, i2c_addr),
1539 : external,
1540 : rotation
1541 : );
1542 :
1543 0 : add_backend(driver_type, backend); // add_backend does nullptr check
1544 : }
1545 :
1546 0 : void Compass::probe_spi_dev(DriverType driver_type, probe_spi_dev_probefn_t probefn, const char *name, bool external, Rotation rotation)
1547 : {
1548 0 : if (!_driver_enabled(driver_type)) {
1549 0 : return;
1550 : }
1551 0 : auto *backend = probefn(hal.spi->get_device(name), external, rotation);
1552 :
1553 0 : add_backend(driver_type, backend); // add_backend does nullptr check
1554 : }
1555 :
1556 : // short-lived method which expectes a probe function that doesn't
1557 : // offer the ability to specify an external rotation:
1558 0 : void Compass::probe_spi_dev(DriverType driver_type, probe_spi_dev_noexternal_probefn_t probefn, const char *name, Rotation rotation)
1559 : {
1560 0 : if (!_driver_enabled(driver_type)) {
1561 0 : return;
1562 : }
1563 0 : auto *backend = probefn(hal.spi->get_device(name), rotation);
1564 :
1565 0 : add_backend(driver_type, backend); // add_backend does nullptr check
1566 : }
1567 :
1568 : #if AP_COMPASS_DRONECAN_ENABLED
1569 : /*
1570 : look for DroneCAN compasses
1571 : */
1572 2093 : void Compass::probe_dronecan_compasses(void)
1573 : {
1574 2093 : if (!_driver_enabled(DRIVER_UAVCAN)) {
1575 0 : return;
1576 : }
1577 18837 : for (uint8_t i=0; i<MAX_CONNECTED_MAGS; i++) {
1578 16744 : AP_Compass_Backend* _uavcan_backend = AP_Compass_DroneCAN::probe(i);
1579 16744 : if (_uavcan_backend) {
1580 0 : add_backend(DRIVER_UAVCAN, _uavcan_backend);
1581 : }
1582 : #if COMPASS_MAX_UNREG_DEV > 0
1583 16744 : if (_unreg_compass_count == COMPASS_MAX_UNREG_DEV) {
1584 0 : break;
1585 : }
1586 : #endif
1587 : }
1588 :
1589 : #if COMPASS_MAX_UNREG_DEV > 0
1590 2093 : if (option_set(Option::ALLOW_DRONECAN_AUTO_REPLACEMENT) && !suppress_devid_save) {
1591 : // check if there's any uavcan compass in prio slot that's not found
1592 : // and replace it if there's a replacement compass
1593 0 : for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
1594 0 : if (AP_HAL::Device::devid_get_bus_type(_priority_did_list[i]) != AP_HAL::Device::BUS_TYPE_UAVCAN
1595 0 : || _get_state(i).registered) {
1596 0 : continue;
1597 : }
1598 : // There's a UAVCAN compass missing
1599 : // Let's check if there's a replacement
1600 0 : for (uint8_t j=0; j<COMPASS_MAX_INSTANCES; j++) {
1601 0 : uint32_t detected_devid = AP_Compass_DroneCAN::get_detected_devid(j);
1602 : // Check if this is a potential replacement mag
1603 0 : if (!is_replacement_mag(detected_devid)) {
1604 0 : continue;
1605 : }
1606 : // We have found a replacement mag, let's replace the existing one
1607 : // with this by setting the priority to zero and calling uavcan probe
1608 0 : GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "Mag: Compass #%d with DEVID %lu replaced", uint8_t(i), (unsigned long)_priority_did_list[i]);
1609 0 : _priority_did_stored_list[i].set_and_save(0);
1610 0 : _priority_did_list[i] = 0;
1611 :
1612 0 : AP_Compass_Backend* _uavcan_backend = AP_Compass_DroneCAN::probe(j);
1613 0 : if (_uavcan_backend) {
1614 0 : add_backend(DRIVER_UAVCAN, _uavcan_backend);
1615 : // we also need to remove the id from unreg list
1616 0 : remove_unreg_dev_id(detected_devid);
1617 : } else {
1618 : // the mag has already been allocated,
1619 : // let's begin the replacement
1620 0 : bool found_replacement = false;
1621 0 : for (StateIndex k(0); k<COMPASS_MAX_INSTANCES; k++) {
1622 0 : if ((uint32_t)_state[k].dev_id == detected_devid) {
1623 0 : if (_state[k].priority <= uint8_t(i)) {
1624 : // we are already on higher priority
1625 : // nothing to do
1626 0 : break;
1627 : }
1628 0 : found_replacement = true;
1629 : // reset old priority of replacement mag
1630 0 : _priority_did_stored_list[_state[k].priority].set_and_save(0);
1631 0 : _priority_did_list[_state[k].priority] = 0;
1632 : // update new priority
1633 0 : _state[k].priority = i;
1634 : }
1635 : }
1636 0 : if (!found_replacement) {
1637 0 : continue;
1638 : }
1639 0 : _priority_did_stored_list[i].set_and_save(detected_devid);
1640 0 : _priority_did_list[i] = detected_devid;
1641 : }
1642 : }
1643 : }
1644 : }
1645 : #endif // #if COMPASS_MAX_UNREG_DEV > 0
1646 : }
1647 : #endif // AP_COMPASS_DRONECAN_ENABLED
1648 :
1649 :
1650 : // Check if the devid is a potential replacement compass
1651 : // Following are the checks done to ensure the compass is a replacement
1652 : // * The compass is an UAVCAN compass
1653 : // * The compass wasn't seen before this boot as additional unreg mag
1654 : // * The compass might have been seen before but never setup
1655 0 : bool Compass::is_replacement_mag(uint32_t devid) {
1656 : #if COMPASS_MAX_INSTANCES > 1
1657 : // We only do this for UAVCAN mag
1658 0 : if (devid == 0 || (AP_HAL::Device::devid_get_bus_type(devid) != AP_HAL::Device::BUS_TYPE_UAVCAN)) {
1659 0 : return false;
1660 : }
1661 :
1662 : #if COMPASS_MAX_UNREG_DEV > 0
1663 : // Check that its not an unused additional mag
1664 0 : for (uint8_t i = 0; i<COMPASS_MAX_UNREG_DEV; i++) {
1665 0 : if (_previously_unreg_mag[i] == devid) {
1666 0 : return false;
1667 : }
1668 : }
1669 : #endif
1670 :
1671 : // Check that its not previously setup mag
1672 0 : for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
1673 0 : if ((uint32_t)_state[i].expected_dev_id == devid) {
1674 0 : return false;
1675 : }
1676 : }
1677 : #endif
1678 0 : return true;
1679 : }
1680 :
1681 0 : void Compass::remove_unreg_dev_id(uint32_t devid)
1682 : {
1683 : #if COMPASS_MAX_INSTANCES > 1
1684 : // We only do this for UAVCAN mag
1685 0 : if (devid == 0 || (AP_HAL::Device::devid_get_bus_type(devid) != AP_HAL::Device::BUS_TYPE_UAVCAN)) {
1686 0 : return;
1687 : }
1688 :
1689 : #if COMPASS_MAX_UNREG_DEV > 0
1690 0 : for (uint8_t i = 0; i<COMPASS_MAX_UNREG_DEV; i++) {
1691 0 : if ((uint32_t)extra_dev_id[i] == devid) {
1692 0 : extra_dev_id[i].set(0);
1693 0 : return;
1694 : }
1695 : }
1696 : #endif
1697 : #endif
1698 : }
1699 :
1700 10 : void Compass::_reset_compass_id()
1701 : {
1702 : #if COMPASS_MAX_INSTANCES > 1
1703 : // Check if any of the registered devs are not registered
1704 40 : for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
1705 60 : if (_priority_did_stored_list[i] != _priority_did_list[i] ||
1706 30 : _priority_did_stored_list[i] == 0) {
1707 : //We don't touch priorities that might have been touched by the user
1708 0 : continue;
1709 : }
1710 30 : if (!_get_state(i).registered) {
1711 0 : _priority_did_stored_list[i].set_and_save(0);
1712 0 : GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "Mag: Compass #%d with DEVID %lu removed", uint8_t(i), (unsigned long)_priority_did_list[i]);
1713 : }
1714 : }
1715 :
1716 : // Check if any of the old registered devs are not registered
1717 : // and hence can be removed
1718 40 : for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
1719 30 : if (_state[i].dev_id == 0 && _state[i].expected_dev_id != 0) {
1720 : // also hard reset dev_ids that are not detected
1721 0 : _state[i].dev_id.save();
1722 : }
1723 : }
1724 : #endif
1725 10 : }
1726 :
1727 : // Look for devices beyond initialisation
1728 : void
1729 1508109 : Compass::_detect_runtime(void)
1730 : {
1731 : #if AP_COMPASS_DRONECAN_ENABLED
1732 1508109 : if (!available()) {
1733 0 : return;
1734 : }
1735 :
1736 : //Don't try to add device while armed
1737 1508109 : if (hal.util->get_soft_armed()) {
1738 1006965 : return;
1739 : }
1740 : static uint32_t last_try;
1741 : //Try once every second
1742 501144 : if ((AP_HAL::millis() - last_try) < 1000) {
1743 450931 : return;
1744 : }
1745 50213 : last_try = AP_HAL::millis();
1746 50213 : if (_driver_enabled(DRIVER_UAVCAN)) {
1747 200852 : for (uint8_t i=0; i<COMPASS_MAX_BACKEND; i++) {
1748 150639 : AP_Compass_Backend* _uavcan_backend = AP_Compass_DroneCAN::probe(i);
1749 150639 : if (_uavcan_backend) {
1750 0 : add_backend(DRIVER_UAVCAN, _uavcan_backend);
1751 : }
1752 150639 : RETURN_IF_NO_SPACE;
1753 : }
1754 : }
1755 : #endif // AP_COMPASS_DRONECAN_ENABLED
1756 : }
1757 :
1758 : bool
1759 1510565 : Compass::read(void)
1760 : {
1761 1510565 : if (!available()) {
1762 2456 : return false;
1763 : }
1764 :
1765 : #if HAL_LOGGING_ENABLED
1766 1508109 : const bool old_healthy = healthy();
1767 : #endif
1768 :
1769 : #ifndef HAL_BUILD_AP_PERIPH
1770 1508109 : if (!_initial_location_set) {
1771 112086 : try_set_initial_location();
1772 : }
1773 : #endif
1774 :
1775 1508109 : _detect_runtime();
1776 :
1777 6032436 : for (uint8_t i=0; i< _backend_count; i++) {
1778 : // call read on each of the backend. This call updates field[i]
1779 4524327 : _backends[i]->read();
1780 : }
1781 1508109 : uint32_t time = AP_HAL::millis();
1782 1508109 : bool any_healthy = false;
1783 6032436 : for (StateIndex i(0); i < COMPASS_MAX_INSTANCES; i++) {
1784 4524327 : _state[i].healthy = (time - _state[i].last_update_ms < 500);
1785 4524327 : any_healthy |= _state[i].healthy;
1786 : }
1787 : #if COMPASS_LEARN_ENABLED
1788 1508109 : if (_learn == LearnType::INFLIGHT && !learn_allocated) {
1789 2 : learn_allocated = true;
1790 2 : learn = NEW_NOTHROW CompassLearn(*this);
1791 : }
1792 1508109 : if (_learn == LearnType::INFLIGHT && learn != nullptr) {
1793 493 : learn->update();
1794 : }
1795 : #endif
1796 : #if HAL_LOGGING_ENABLED
1797 1508109 : if (any_healthy && _log_bit != (uint32_t)-1 && AP::logger().should_log(_log_bit)) {
1798 1415805 : Write_Compass();
1799 : }
1800 : #endif
1801 :
1802 : // Set _first_usable parameter
1803 1511305 : for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
1804 1510240 : if (_use_for_yaw[i]) {
1805 1507044 : _first_usable = uint8_t(i);
1806 1507044 : break;
1807 : }
1808 : }
1809 1508109 : const bool new_healthy = healthy();
1810 :
1811 : #if HAL_LOGGING_ENABLED
1812 :
1813 : #define MASK_LOG_ANY 0xFFFF
1814 :
1815 1508109 : if (new_healthy != old_healthy) {
1816 2090 : if (AP::logger().should_log(MASK_LOG_ANY)) {
1817 1677 : const LogErrorCode code = new_healthy ? LogErrorCode::ERROR_RESOLVED : LogErrorCode::UNHEALTHY;
1818 1677 : AP::logger().Write_Error(LogErrorSubsystem::COMPASS, code);
1819 : }
1820 : }
1821 : #endif
1822 :
1823 1508109 : return new_healthy;
1824 : }
1825 :
1826 : uint8_t
1827 0 : Compass::get_healthy_mask() const
1828 : {
1829 0 : uint8_t healthy_mask = 0;
1830 0 : for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
1831 0 : if (healthy(uint8_t(i))) {
1832 0 : healthy_mask |= 1 << uint8_t(i);
1833 : }
1834 : }
1835 0 : return healthy_mask;
1836 : }
1837 :
1838 : void
1839 2388 : Compass::set_offsets(uint8_t i, const Vector3f &offsets)
1840 : {
1841 : // sanity check compass instance provided
1842 2388 : StateIndex id = _get_state_id(Priority(i));
1843 2388 : if (id < COMPASS_MAX_INSTANCES) {
1844 2388 : _state[id].offset.set(offsets);
1845 : }
1846 2388 : }
1847 :
1848 : void
1849 26 : Compass::set_and_save_offsets(uint8_t i, const Vector3f &offsets)
1850 : {
1851 : // sanity check compass instance provided
1852 26 : StateIndex id = _get_state_id(Priority(i));
1853 26 : if (id < COMPASS_MAX_INSTANCES) {
1854 26 : _state[id].offset.set(offsets);
1855 26 : save_offsets(i);
1856 : }
1857 26 : }
1858 :
1859 : #if AP_COMPASS_DIAGONALS_ENABLED
1860 : void
1861 18 : Compass::set_and_save_diagonals(uint8_t i, const Vector3f &diagonals)
1862 : {
1863 : // sanity check compass instance provided
1864 18 : StateIndex id = _get_state_id(Priority(i));
1865 18 : if (id < COMPASS_MAX_INSTANCES) {
1866 18 : _state[id].diagonals.set_and_save(diagonals);
1867 : }
1868 18 : }
1869 :
1870 : void
1871 18 : Compass::set_and_save_offdiagonals(uint8_t i, const Vector3f &offdiagonals)
1872 : {
1873 : // sanity check compass instance provided
1874 18 : StateIndex id = _get_state_id(Priority(i));
1875 18 : if (id < COMPASS_MAX_INSTANCES) {
1876 18 : _state[id].offdiagonals.set_and_save(offdiagonals);
1877 : }
1878 18 : }
1879 : #endif // AP_COMPASS_DIAGONALS_ENABLED
1880 :
1881 : void
1882 9 : Compass::set_and_save_scale_factor(uint8_t i, float scale_factor)
1883 : {
1884 9 : StateIndex id = _get_state_id(Priority(i));
1885 9 : if (i < COMPASS_MAX_INSTANCES) {
1886 9 : _state[id].scale_factor.set_and_save(scale_factor);
1887 : }
1888 9 : }
1889 :
1890 : void
1891 26 : Compass::save_offsets(uint8_t i)
1892 : {
1893 26 : StateIndex id = _get_state_id(Priority(i));
1894 26 : if (id < COMPASS_MAX_INSTANCES) {
1895 26 : _state[id].offset.save(); // save offsets
1896 26 : _state[id].dev_id.set_and_save(_state[id].detected_dev_id);
1897 : }
1898 26 : }
1899 :
1900 : void
1901 0 : Compass::set_and_save_orientation(uint8_t i, Rotation orientation)
1902 : {
1903 0 : StateIndex id = _get_state_id(Priority(i));
1904 0 : if (id < COMPASS_MAX_INSTANCES) {
1905 0 : _state[id].orientation.set_and_save_ifchanged(orientation);
1906 : }
1907 0 : }
1908 :
1909 : void
1910 0 : Compass::save_offsets(void)
1911 : {
1912 0 : for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
1913 0 : save_offsets(uint8_t(i));
1914 : }
1915 0 : }
1916 :
1917 : void
1918 9 : Compass::set_motor_compensation(uint8_t i, const Vector3f &motor_comp_factor)
1919 : {
1920 9 : StateIndex id = _get_state_id(Priority(i));
1921 9 : if (id < COMPASS_MAX_INSTANCES) {
1922 9 : _state[id].motor_compensation.set(motor_comp_factor);
1923 : }
1924 9 : }
1925 :
1926 : void
1927 1 : Compass::save_motor_compensation()
1928 : {
1929 1 : StateIndex id;
1930 1 : _motor_comp_type.save();
1931 4 : for (Priority k(0); k<COMPASS_MAX_INSTANCES; k++) {
1932 3 : id = _get_state_id(k);
1933 3 : if (id<COMPASS_MAX_INSTANCES) {
1934 3 : _state[id].motor_compensation.save();
1935 : }
1936 : }
1937 1 : }
1938 :
1939 112086 : void Compass::try_set_initial_location()
1940 : {
1941 112086 : if (!_auto_declination) {
1942 110986 : return;
1943 : }
1944 112086 : if (!_enabled) {
1945 0 : return;
1946 : }
1947 :
1948 112086 : Location loc;
1949 112086 : if (!AP::ahrs().get_location(loc)) {
1950 110986 : return;
1951 : }
1952 1100 : _initial_location_set = true;
1953 :
1954 : // if automatic declination is configured, then compute
1955 : // the declination based on the initial GPS fix
1956 : // Set the declination based on the lat/lng from GPS
1957 1100 : _declination.set(radians(
1958 : AP_Declination::get_declination(
1959 1100 : (float)loc.lat / 10000000,
1960 1100 : (float)loc.lng / 10000000)));
1961 : }
1962 :
1963 : /// return true if the compass should be used for yaw calculations
1964 : bool
1965 51094897 : Compass::use_for_yaw(void) const
1966 : {
1967 51094897 : return healthy(_first_usable) && use_for_yaw(_first_usable);
1968 : }
1969 :
1970 : /// return true if the specified compass can be used for yaw calculations
1971 : bool
1972 420884989 : Compass::use_for_yaw(uint8_t i) const
1973 : {
1974 420884989 : if (!available()) {
1975 2115 : return false;
1976 : }
1977 : // when we are doing in-flight compass learning the state
1978 : // estimator must not use the compass. The learning code turns off
1979 : // inflight learning when it has converged
1980 420882874 : return _use_for_yaw[Priority(i)] && _learn != LearnType::INFLIGHT;
1981 : }
1982 :
1983 : /*
1984 : return the number of enabled sensors. Used to determine if
1985 : non-compass operation is desired
1986 : */
1987 41156285 : uint8_t Compass::get_num_enabled(void) const
1988 : {
1989 41156285 : if (get_count() == 0) {
1990 10412 : return 0;
1991 : }
1992 41145873 : uint8_t count = 0;
1993 164583492 : for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
1994 123437619 : if (use_for_yaw(i)) {
1995 122774509 : count++;
1996 : }
1997 : }
1998 41145873 : return count;
1999 : }
2000 :
2001 : void
2002 0 : Compass::set_declination(float radians, bool save_to_eeprom)
2003 : {
2004 0 : if (save_to_eeprom) {
2005 0 : _declination.set_and_save(radians);
2006 : } else {
2007 0 : _declination.set(radians);
2008 : }
2009 0 : }
2010 :
2011 : float
2012 42645918 : Compass::get_declination() const
2013 : {
2014 42645918 : return _declination.get();
2015 : }
2016 :
2017 : /*
2018 : calculate a compass heading given the attitude from DCM and the mag vector
2019 : */
2020 : float
2021 2082 : Compass::calculate_heading(const Matrix3f &dcm_matrix, uint8_t i) const
2022 : {
2023 : /*
2024 : This extracts a roll/pitch-only rotation which is then used to rotate the body frame field into earth frame so the heading can be calculated.
2025 : One could do:
2026 : float roll, pitch, yaw;
2027 : dcm_matrix.to_euler(roll, pitch, yaw)
2028 : Matrix3f rp_rot;
2029 : rp_rot.from_euler(roll, pitch, 0)
2030 : Vector3f ef = rp_rot * field
2031 :
2032 : Because only the X and Y components are needed it's more efficient to manually calculate:
2033 :
2034 : rp_rot = [ cos(pitch), sin(roll) * sin(pitch), cos(roll) * sin(pitch)
2035 : 0, cos(roll), -sin(roll)]
2036 :
2037 : If the whole matrix is multiplied by cos(pitch) the required trigonometric values can be extracted directly from the existing dcm matrix.
2038 : This multiplication has no effect on the calculated heading as it changes the length of the North/East vector but not its angle.
2039 :
2040 : rp_rot = [ cos(pitch)^2, sin(roll) * sin(pitch) * cos(pitch), cos(roll) * sin(pitch) * cos(pitch)
2041 : 0, cos(roll) * cos(pitch), -sin(roll) * cos(pitch)]
2042 :
2043 : Preexisting values can be substituted in:
2044 :
2045 : dcm_matrix.c.x = -sin(pitch)
2046 : dcm_matrix.c.y = sin(roll) * cos(pitch)
2047 : dcm_matrix.c.z = cos(roll) * cos(pitch)
2048 :
2049 : rp_rot = [ cos(pitch)^2, dcm_matrix.c.y * -dcm_matrix.c.x, dcm_matrix.c.z * -dcm_matrix.c.x
2050 : 0, dcm_matrix.c.z, -dcm_matrix.c.y]
2051 :
2052 : cos(pitch)^2 is stil needed. This is the same as 1 - sin(pitch)^2.
2053 : sin(pitch) is avalable as dcm_matrix.c.x
2054 : */
2055 :
2056 2082 : const float cos_pitch_sq = 1.0f-(dcm_matrix.c.x*dcm_matrix.c.x);
2057 :
2058 : // Tilt compensated magnetic field Y component:
2059 2082 : const Vector3f &field = get_field(i);
2060 :
2061 2082 : const float headY = field.y * dcm_matrix.c.z - field.z * dcm_matrix.c.y;
2062 :
2063 : // Tilt compensated magnetic field X component:
2064 2082 : const float headX = field.x * cos_pitch_sq - dcm_matrix.c.x * (field.y * dcm_matrix.c.y + field.z * dcm_matrix.c.z);
2065 :
2066 : // magnetic heading
2067 : // 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.
2068 2082 : const float heading = constrain_float(atan2f(-headY,headX), -M_PI, M_PI);
2069 :
2070 : // Declination correction
2071 2082 : return wrap_PI(heading + _declination);
2072 : }
2073 :
2074 : /// Returns True if the compasses have been configured (i.e. offsets saved)
2075 : ///
2076 : /// @returns True if compass has been configured
2077 : ///
2078 399643 : bool Compass::configured(uint8_t i)
2079 : {
2080 : // exit immediately if instance is beyond the number of compasses we have available
2081 399643 : if (i > get_count()) {
2082 0 : return false;
2083 : }
2084 :
2085 : // exit immediately if all offsets are zero
2086 399643 : if (is_zero(get_offsets(i).length())) {
2087 51 : return false;
2088 : }
2089 :
2090 399592 : StateIndex id = _get_state_id(Priority(i));
2091 : // exit immediately if dev_id hasn't been detected
2092 799184 : if (_state[id].detected_dev_id == 0 ||
2093 799184 : id == COMPASS_MAX_INSTANCES) {
2094 0 : return false;
2095 : }
2096 :
2097 : #ifdef HAL_USE_EMPTY_STORAGE
2098 : // the load() call below returns zeroes on empty storage, so the
2099 : // check-stored-value check here will always fail. Since nobody
2100 : // really cares about the empty-storage case, shortcut out here
2101 : // for simplicity.
2102 : return true;
2103 : #endif
2104 :
2105 : // back up cached value of dev_id
2106 399592 : int32_t dev_id_cache_value = _state[id].dev_id;
2107 :
2108 : // load dev_id from eeprom
2109 399592 : _state[id].dev_id.load();
2110 :
2111 : // if dev_id loaded from eeprom is different from detected dev id or dev_id loaded from eeprom is different from cached dev_id, compass is unconfigured
2112 399592 : if (_state[id].dev_id != _state[id].detected_dev_id || _state[id].dev_id != dev_id_cache_value) {
2113 : // restore cached value
2114 62 : _state[id].dev_id.set(dev_id_cache_value);
2115 : // return failure
2116 62 : return false;
2117 : }
2118 :
2119 : // if we got here then it must be configured
2120 399530 : return true;
2121 : }
2122 :
2123 133273 : bool Compass::configured(char *failure_msg, uint8_t failure_msg_len)
2124 : {
2125 : #if COMPASS_MAX_INSTANCES > 1
2126 : // Check if any of the registered devs are not registered
2127 533080 : for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
2128 399811 : if (_priority_did_list[i] != 0 && use_for_yaw(uint8_t(i))) {
2129 399404 : if (!_get_state(i).registered) {
2130 0 : snprintf(failure_msg, failure_msg_len, "Compass %d not found", uint8_t(i + 1));
2131 4 : return false;
2132 : }
2133 399404 : if (_priority_did_list[i] != _priority_did_stored_list[i]) {
2134 4 : snprintf(failure_msg, failure_msg_len, "Compass order change requires reboot");
2135 4 : return false;
2136 : }
2137 : }
2138 : }
2139 : #endif
2140 :
2141 133269 : bool all_configured = true;
2142 532799 : for (uint8_t i=0; i<get_count(); i++) {
2143 399643 : if (configured(i)) {
2144 399530 : continue;
2145 : }
2146 113 : if (!use_for_yaw(i)) {
2147 : // we're not planning on using this anyway so sure,
2148 : // whatever, it's configured....
2149 0 : continue;
2150 : }
2151 113 : all_configured = false;
2152 113 : break;
2153 : }
2154 133269 : if (!all_configured) {
2155 113 : snprintf(failure_msg, failure_msg_len, "Compass not calibrated");
2156 : }
2157 133269 : return all_configured;
2158 : }
2159 :
2160 : /*
2161 : set the type of motor compensation to use
2162 : */
2163 2 : void Compass::motor_compensation_type(const uint8_t comp_type)
2164 : {
2165 2 : if (_motor_comp_type <= AP_COMPASS_MOT_COMP_CURRENT && _motor_comp_type != (int8_t)comp_type) {
2166 1 : _motor_comp_type.set((int8_t)comp_type);
2167 1 : _thr = 0; // set current throttle to zero
2168 4 : for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
2169 3 : set_motor_compensation(i, Vector3f(0,0,0)); // clear out invalid compensation vectors
2170 : }
2171 : }
2172 2 : }
2173 :
2174 41289392 : bool Compass::consistent() const
2175 : {
2176 41289392 : const Vector3f &primary_mag_field { get_field() };
2177 41289392 : const Vector2f &primary_mag_field_xy { primary_mag_field.xy() };
2178 :
2179 162375486 : for (uint8_t i=0; i<get_count(); i++) {
2180 122476511 : if (!use_for_yaw(i)) {
2181 : // configured not-to-be-used
2182 588837 : continue;
2183 : }
2184 :
2185 121887674 : const Vector3f &mag_field = get_field(i);
2186 121887674 : const Vector2f &mag_field_xy = mag_field.xy();
2187 :
2188 121887674 : if (mag_field_xy.is_zero()) {
2189 24713 : return false;
2190 : }
2191 :
2192 : // check for gross misalignment on all axes
2193 121862961 : const float xyz_ang_diff = mag_field.angle(primary_mag_field);
2194 121862961 : if (xyz_ang_diff > AP_COMPASS_MAX_XYZ_ANG_DIFF) {
2195 18031 : return false;
2196 : }
2197 :
2198 : // check for an unacceptable angle difference on the xy plane
2199 121844930 : const float xy_ang_diff = mag_field_xy.angle(primary_mag_field_xy);
2200 121844930 : if (xy_ang_diff > AP_COMPASS_MAX_XY_ANG_DIFF) {
2201 134833 : return false;
2202 : }
2203 :
2204 : // check for an unacceptable length difference on the xy plane
2205 121710097 : const float xy_len_diff = (primary_mag_field_xy-mag_field_xy).length();
2206 121710097 : if (xy_len_diff > AP_COMPASS_MAX_XY_LENGTH_DIFF) {
2207 1212840 : return false;
2208 : }
2209 : }
2210 39898975 : return true;
2211 : }
2212 :
2213 183062113 : bool Compass::healthy(uint8_t i) const
2214 : {
2215 183062113 : return (i < COMPASS_MAX_INSTANCES) ? _get_state(Priority(i)).healthy : false;
2216 : }
2217 :
2218 : /*
2219 : return true if we have a valid scale factor
2220 : */
2221 170088028 : bool Compass::have_scale_factor(uint8_t i) const
2222 : {
2223 170088028 : if (!available()) {
2224 64554 : return false;
2225 : }
2226 170023474 : StateIndex id = _get_state_id(Priority(i));
2227 170023474 : if (id >= COMPASS_MAX_INSTANCES ||
2228 340046948 : _state[id].scale_factor < COMPASS_MIN_SCALE_FACTOR ||
2229 170023474 : _state[id].scale_factor > COMPASS_MAX_SCALE_FACTOR) {
2230 79240 : return false;
2231 : }
2232 169944234 : return true;
2233 : }
2234 :
2235 : #if AP_COMPASS_MSP_ENABLED
2236 0 : void Compass::handle_msp(const MSP::msp_compass_data_message_t &pkt)
2237 : {
2238 0 : if (!_driver_enabled(DRIVER_MSP)) {
2239 0 : return;
2240 : }
2241 0 : if (!init_done) {
2242 0 : if (pkt.instance < 8) {
2243 0 : msp_instance_mask |= 1U<<pkt.instance;
2244 : }
2245 : } else {
2246 0 : for (uint8_t i=0; i<_backend_count; i++) {
2247 0 : _backends[i]->handle_msp(pkt);
2248 : }
2249 : }
2250 : }
2251 : #endif // AP_COMPASS_MSP_ENABLED
2252 :
2253 : #if AP_COMPASS_EXTERNALAHRS_ENABLED
2254 137584 : void Compass::handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt)
2255 : {
2256 137584 : if (!_driver_enabled(DRIVER_EXTERNALAHRS)) {
2257 0 : return;
2258 : }
2259 540859 : for (uint8_t i=0; i<_backend_count; i++) {
2260 403275 : _backends[i]->handle_external(pkt);
2261 : }
2262 : }
2263 : #endif // AP_COMPASS_EXTERNALAHRS_ENABLED
2264 :
2265 : // force save of current calibration as valid
2266 8 : void Compass::force_save_calibration(void)
2267 : {
2268 32 : for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
2269 24 : if (_state[i].dev_id != 0) {
2270 24 : _state[i].dev_id.save();
2271 : }
2272 : }
2273 8 : }
2274 :
2275 : // singleton instance
2276 : Compass *Compass::_singleton;
2277 :
2278 : namespace AP
2279 : {
2280 :
2281 189603256 : Compass &compass()
2282 : {
2283 189603256 : return *Compass::get_singleton();
2284 : }
2285 :
2286 : }
2287 :
2288 : #endif // AP_COMPASS_ENABLED
|
