CoCalc -- AP_Compass.cpp

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/libraries/AP_Compass/AP_Compass.cpp
⁹³⁶² views
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 <AP_CustomRotations/AP_CustomRotations.h>
16
#include <GCS_MAVLink/GCS.h>
17
#include <AP_AHRS/AP_AHRS.h>
18

19
#include "AP_Compass_config.h"
20

21
#include "AP_Compass_SITL.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_LIS2MDL.h"
34
#include "AP_Compass_AK09916.h"
35
#include "AP_Compass_QMC5883L.h"
36
#if AP_COMPASS_DRONECAN_ENABLED
37
#include "AP_Compass_DroneCAN.h"
38
#endif
39
#include "AP_Compass_QMC5883P.h"
40
#include "AP_Compass_MMC3416.h"
41
#include "AP_Compass_MMC5xx3.h"
42
#include "AP_Compass_MAG3110.h"
43
#include "AP_Compass_RM3100.h"
44
#if AP_COMPASS_MSP_ENABLED
45
#include "AP_Compass_MSP.h"
46
#endif
47
#if AP_COMPASS_EXTERNALAHRS_ENABLED
48
#include "AP_Compass_ExternalAHRS.h"
49
#endif
50
#include "AP_Compass.h"
51
#include "Compass_learn.h"
52
#include <stdio.h>
53

54
extern const AP_HAL::HAL& hal;
55

56
#ifndef AP_COMPASS_ENABLE_DEFAULT
57
 #define AP_COMPASS_ENABLE_DEFAULT 1
58
#endif
59

60
#ifndef COMPASS_LEARN_DEFAULT
61
#define COMPASS_LEARN_DEFAULT Compass::LearnType::NONE
62
#endif
63

64
#ifndef AP_COMPASS_OFFSETS_MAX_DEFAULT
65
#define AP_COMPASS_OFFSETS_MAX_DEFAULT 1800
66
#endif
67

68
#ifndef HAL_COMPASS_FILTER_DEFAULT
69
#define HAL_COMPASS_FILTER_DEFAULT 0 // turned off by default
70
#endif
71

72
#ifndef HAL_COMPASS_AUTO_ROT_DEFAULT
73
#define HAL_COMPASS_AUTO_ROT_DEFAULT 2
74
#endif
75

76
const AP_Param::GroupInfo Compass::var_info[] = {
77
    // index 0 was used for the old orientation matrix
78

79
#ifndef HAL_BUILD_AP_PERIPH
80
    // @Param: OFS_X
81
    // @DisplayName: Compass offsets in milligauss on the X axis
82
    // @Description: Offset to be added to the compass x-axis values to compensate for metal in the frame
83
    // @Range: -400 400
84
    // @Units: mGauss
85
    // @Increment: 1
86
    // @User: Advanced
87
    // @Calibration: 1
88

89
    // @Param: OFS_Y
90
    // @DisplayName: Compass offsets in milligauss on the Y axis
91
    // @Description: Offset to be added to the compass y-axis values to compensate for metal in the frame
92
    // @Range: -400 400
93
    // @Units: mGauss
94
    // @Increment: 1
95
    // @User: Advanced
96
    // @Calibration: 1
97

98
    // @Param: OFS_Z
99
    // @DisplayName: Compass offsets in milligauss on the Z axis
100
    // @Description: Offset to be added to the compass z-axis values to compensate for metal in the frame
101
    // @Range: -400 400
102
    // @Units: mGauss
103
    // @Increment: 1
104
    // @User: Advanced
105
    // @Calibration: 1
106
    AP_GROUPINFO("OFS",    1, Compass, _state._priv_instance[0].offset, 0),
107

108
    // @Param: DEC
109
    // @DisplayName: Compass declination
110
    // @Description: An angle to compensate between the true north and magnetic north
111
    // @Range: -3.142 3.142
112
    // @Units: rad
113
    // @Increment: 0.01
114
    // @User: Standard
115
    AP_GROUPINFO("DEC",    2, Compass, _declination, 0),
116
#endif // HAL_BUILD_AP_PERIPH
117

118
#if COMPASS_LEARN_ENABLED
119
    // @Param: LEARN
120
    // @DisplayName: Learn compass offsets automatically
121
    // @Description: Enable or disable the automatic learning of compass offsets. You can enable learning either using a compass-only method that is suitable only for fixed wing aircraft or using the offsets learnt by the active EKF state estimator. If this option is enabled then the learnt offsets are saved when you disarm the vehicle. If InFlight learning is enabled then the compass with automatically start learning once a flight starts (must be armed). While InFlight learning is running you cannot use position control modes.
122
    // @Values: 0:Disabled,2:EKF-Learning,3:InFlight-Learning
123
    // @User: Advanced
124
    AP_GROUPINFO("LEARN",  3, Compass, _learn, float(COMPASS_LEARN_DEFAULT)),
125
#endif
126

127
#ifndef HAL_BUILD_AP_PERIPH
128
    // @Param: USE
129
    // @DisplayName: Use compass for yaw
130
    // @Description: Enable or disable the use of the compass (instead of the GPS) for determining heading
131
    // @Values: 0:Disabled,1:Enabled
132
    // @User: Advanced
133
    AP_GROUPINFO("USE",    4, Compass, _use_for_yaw._priv_instance[0], 1), // true if used for DCM yaw
134

135
    // @Param: AUTODEC
136
    // @DisplayName: Auto Declination
137
    // @Description: Enable or disable the automatic calculation of the declination based on gps location
138
    // @Values: 0:Disabled,1:Enabled
139
    // @User: Advanced
140
    AP_GROUPINFO("AUTODEC",5, Compass, _auto_declination, 1),
141
#endif
142

143
#if COMPASS_MOT_ENABLED
144
    // @Param: MOTCT
145
    // @DisplayName: Motor interference compensation type
146
    // @Description: Set motor interference compensation type to disabled, throttle or current.  Do not change manually.
147
    // @Values: 0:Disabled,1:Use Throttle,2:Use Current
148
    // @User: Advanced
149
    // @Calibration: 1
150
    AP_GROUPINFO("MOTCT",    6, Compass, _motor_comp_type, AP_COMPASS_MOT_COMP_DISABLED),
151

152
    // @Param: MOT_X
153
    // @DisplayName: Motor interference compensation for body frame X axis
154
    // @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)
155
    // @Range: -1000 1000
156
    // @Units: mGauss/A
157
    // @Increment: 1
158
    // @User: Advanced
159
    // @Calibration: 1
160

161
    // @Param: MOT_Y
162
    // @DisplayName: Motor interference compensation for body frame Y axis
163
    // @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)
164
    // @Range: -1000 1000
165
    // @Units: mGauss/A
166
    // @Increment: 1
167
    // @User: Advanced
168
    // @Calibration: 1
169

170
    // @Param: MOT_Z
171
    // @DisplayName: Motor interference compensation for body frame Z axis
172
    // @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)
173
    // @Range: -1000 1000
174
    // @Units: mGauss/A
175
    // @Increment: 1
176
    // @User: Advanced
177
    // @Calibration: 1
178
    AP_GROUPINFO("MOT",    7, Compass, _state._priv_instance[0].motor_compensation, 0),
179
#endif
180

181
#ifndef HAL_BUILD_AP_PERIPH
182
    // @Param: ORIENT
183
    // @DisplayName: Compass orientation
184
    // @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.
185
    // @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
186
    // @User: Advanced
187
    AP_GROUPINFO("ORIENT", 8, Compass, _state._priv_instance[0].orientation, ROTATION_NONE),
188

189
    // @Param: EXTERNAL
190
    // @DisplayName: Compass is attached via an external cable
191
    // @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.
192
    // @Values: 0:Internal,1:External,2:ForcedExternal
193
    // @User: Advanced
194
    AP_GROUPINFO("EXTERNAL", 9, Compass, _state._priv_instance[0].external, 0),
195
#endif
196

197
#if COMPASS_MAX_INSTANCES > 1
198
    // @Param: OFS2_X
199
    // @DisplayName: Compass2 offsets in milligauss on the X axis
200
    // @Description: Offset to be added to compass2's x-axis values to compensate for metal in the frame
201
    // @Range: -400 400
202
    // @Units: mGauss
203
    // @Increment: 1
204
    // @User: Advanced
205
    // @Calibration: 1
206

207
    // @Param: OFS2_Y
208
    // @DisplayName: Compass2 offsets in milligauss on the Y axis
209
    // @Description: Offset to be added to compass2's y-axis values to compensate for metal in the frame
210
    // @Range: -400 400
211
    // @Units: mGauss
212
    // @Increment: 1
213
    // @User: Advanced
214
    // @Calibration: 1
215

216
    // @Param: OFS2_Z
217
    // @DisplayName: Compass2 offsets in milligauss on the Z axis
218
    // @Description: Offset to be added to compass2's z-axis values to compensate for metal in the frame
219
    // @Range: -400 400
220
    // @Units: mGauss
221
    // @Increment: 1
222
    // @User: Advanced
223
    // @Calibration: 1
224
    AP_GROUPINFO("OFS2",    10, Compass, _state._priv_instance[1].offset, 0),
225

226
    // @Param: MOT2_X
227
    // @DisplayName: Motor interference compensation to compass2 for body frame X axis
228
    // @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)
229
    // @Range: -1000 1000
230
    // @Units: mGauss/A
231
    // @Increment: 1
232
    // @User: Advanced
233
    // @Calibration: 1
234

235
    // @Param: MOT2_Y
236
    // @DisplayName: Motor interference compensation to compass2 for body frame Y axis
237
    // @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)
238
    // @Range: -1000 1000
239
    // @Units: mGauss/A
240
    // @Increment: 1
241
    // @User: Advanced
242
    // @Calibration: 1
243

244
    // @Param: MOT2_Z
245
    // @DisplayName: Motor interference compensation to compass2 for body frame Z axis
246
    // @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)
247
    // @Range: -1000 1000
248
    // @Units: mGauss/A
249
    // @Increment: 1
250
    // @User: Advanced
251
    // @Calibration: 1
252
    AP_GROUPINFO("MOT2",    11, Compass, _state._priv_instance[1].motor_compensation, 0),
253

254
#endif // COMPASS_MAX_INSTANCES
255

256
#if COMPASS_MAX_INSTANCES > 2
257
    // @Param: OFS3_X
258
    // @DisplayName: Compass3 offsets in milligauss on the X axis
259
    // @Description: Offset to be added to compass3's x-axis values to compensate for metal in the frame
260
    // @Range: -400 400
261
    // @Units: mGauss
262
    // @Increment: 1
263
    // @User: Advanced
264
    // @Calibration: 1
265

266
    // @Param: OFS3_Y
267
    // @DisplayName: Compass3 offsets in milligauss on the Y axis
268
    // @Description: Offset to be added to compass3's y-axis values to compensate for metal in the frame
269
    // @Range: -400 400
270
    // @Units: mGauss
271
    // @Increment: 1
272
    // @User: Advanced
273
    // @Calibration: 1
274

275
    // @Param: OFS3_Z
276
    // @DisplayName: Compass3 offsets in milligauss on the Z axis
277
    // @Description: Offset to be added to compass3's z-axis values to compensate for metal in the frame
278
    // @Range: -400 400
279
    // @Units: mGauss
280
    // @Increment: 1
281
    // @User: Advanced
282
    // @Calibration: 1
283
    AP_GROUPINFO("OFS3",    13, Compass, _state._priv_instance[2].offset, 0),
284

285
    // @Param: MOT3_X
286
    // @DisplayName: Motor interference compensation to compass3 for body frame X axis
287
    // @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)
288
    // @Range: -1000 1000
289
    // @Units: mGauss/A
290
    // @Increment: 1
291
    // @User: Advanced
292
    // @Calibration: 1
293

294
    // @Param: MOT3_Y
295
    // @DisplayName: Motor interference compensation to compass3 for body frame Y axis
296
    // @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)
297
    // @Range: -1000 1000
298
    // @Units: mGauss/A
299
    // @Increment: 1
300
    // @User: Advanced
301
    // @Calibration: 1
302

303
    // @Param: MOT3_Z
304
    // @DisplayName: Motor interference compensation to compass3 for body frame Z axis
305
    // @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)
306
    // @Range: -1000 1000
307
    // @Units: mGauss/A
308
    // @Increment: 1
309
    // @User: Advanced
310
    // @Calibration: 1
311
    AP_GROUPINFO("MOT3",    14, Compass, _state._priv_instance[2].motor_compensation, 0),
312
#endif // COMPASS_MAX_INSTANCES
313

314
    // @Param: DEV_ID
315
    // @DisplayName: Compass device id
316
    // @Description: Compass device id.  Automatically detected, do not set manually
317
    // @ReadOnly: True
318
    // @User: Advanced
319
    AP_GROUPINFO("DEV_ID",  15, Compass, _state._priv_instance[0].dev_id, 0),
320

321
#if COMPASS_MAX_INSTANCES > 1
322
    // @Param: DEV_ID2
323
    // @DisplayName: Compass2 device id
324
    // @Description: Second compass's device id.  Automatically detected, do not set manually
325
    // @ReadOnly: True
326
    // @User: Advanced
327
    AP_GROUPINFO("DEV_ID2", 16, Compass, _state._priv_instance[1].dev_id, 0),
328
#endif // COMPASS_MAX_INSTANCES
329

330
#if COMPASS_MAX_INSTANCES > 2
331
    // @Param: DEV_ID3
332
    // @DisplayName: Compass3 device id
333
    // @Description: Third compass's device id.  Automatically detected, do not set manually
334
    // @ReadOnly: True
335
    // @User: Advanced
336
    AP_GROUPINFO("DEV_ID3", 17, Compass, _state._priv_instance[2].dev_id, 0),
337
#endif // COMPASS_MAX_INSTANCES
338

339
#if COMPASS_MAX_INSTANCES > 1
340
    // @Param: USE2
341
    // @DisplayName: Compass2 used for yaw
342
    // @Description: Enable or disable the secondary compass for determining heading.
343
    // @Values: 0:Disabled,1:Enabled
344
    // @User: Advanced
345
    AP_GROUPINFO("USE2",    18, Compass, _use_for_yaw._priv_instance[1], 1),
346

347
    // @Param: ORIENT2
348
    // @DisplayName: Compass2 orientation
349
    // @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.
350
    // @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
351
    // @User: Advanced
352
    AP_GROUPINFO("ORIENT2", 19, Compass, _state._priv_instance[1].orientation, ROTATION_NONE),
353

354
    // @Param: EXTERN2
355
    // @DisplayName: Compass2 is attached via an external cable
356
    // @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.
357
    // @Values: 0:Internal,1:External,2:ForcedExternal
358
    // @User: Advanced
359
    AP_GROUPINFO("EXTERN2",20, Compass, _state._priv_instance[1].external, 0),
360
#endif // COMPASS_MAX_INSTANCES
361

362
#if COMPASS_MAX_INSTANCES > 2
363
    // @Param: USE3
364
    // @DisplayName: Compass3 used for yaw
365
    // @Description: Enable or disable the tertiary compass for determining heading.
366
    // @Values: 0:Disabled,1:Enabled
367
    // @User: Advanced
368
    AP_GROUPINFO("USE3",    21, Compass, _use_for_yaw._priv_instance[2], 1),
369

370
    // @Param: ORIENT3
371
    // @DisplayName: Compass3 orientation
372
    // @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.
373
    // @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
374
    // @User: Advanced
375
    AP_GROUPINFO("ORIENT3", 22, Compass, _state._priv_instance[2].orientation, ROTATION_NONE),
376

377
    // @Param: EXTERN3
378
    // @DisplayName: Compass3 is attached via an external cable
379
    // @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.
380
    // @Values: 0:Internal,1:External,2:ForcedExternal
381
    // @User: Advanced
382
    AP_GROUPINFO("EXTERN3",23, Compass, _state._priv_instance[2].external, 0),
383
#endif // COMPASS_MAX_INSTANCES
384

385
#if AP_COMPASS_DIAGONALS_ENABLED
386
    // @Param: DIA_X
387
    // @DisplayName: Compass soft-iron diagonal X component
388
    // @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]]
389
    // @User: Advanced
390
    // @Calibration: 1
391

392
    // @Param: DIA_Y
393
    // @DisplayName: Compass soft-iron diagonal Y component
394
    // @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]]
395
    // @User: Advanced
396
    // @Calibration: 1
397

398
    // @Param: DIA_Z
399
    // @DisplayName: Compass soft-iron diagonal Z component
400
    // @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]]
401
    // @User: Advanced
402
    // @Calibration: 1
403
    AP_GROUPINFO("DIA",    24, Compass, _state._priv_instance[0].diagonals, 1.0),
404

405
    // @Param: ODI_X
406
    // @DisplayName: Compass soft-iron off-diagonal X component
407
    // @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]]
408
    // @User: Advanced
409
    // @Calibration: 1
410

411
    // @Param: ODI_Y
412
    // @DisplayName: Compass soft-iron off-diagonal Y component
413
    // @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]]
414
    // @User: Advanced
415
    // @Calibration: 1
416

417
    // @Param: ODI_Z
418
    // @DisplayName: Compass soft-iron off-diagonal Z component
419
    // @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]]
420
    // @User: Advanced
421
    // @Calibration: 1
422
    AP_GROUPINFO("ODI",    25, Compass, _state._priv_instance[0].offdiagonals, 0),
423

424
#if COMPASS_MAX_INSTANCES > 1
425
    // @Param: DIA2_X
426
    // @DisplayName: Compass2 soft-iron diagonal X component
427
    // @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]]
428
    // @User: Advanced
429
    // @Calibration: 1
430

431
    // @Param: DIA2_Y
432
    // @DisplayName: Compass2 soft-iron diagonal Y component
433
    // @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]]
434
    // @User: Advanced
435
    // @Calibration: 1
436

437
    // @Param: DIA2_Z
438
    // @DisplayName: Compass2 soft-iron diagonal Z component
439
    // @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]]
440
    // @User: Advanced
441
    // @Calibration: 1
442
    AP_GROUPINFO("DIA2",    26, Compass, _state._priv_instance[1].diagonals, 1.0),
443

444
    // @Param: ODI2_X
445
    // @DisplayName: Compass2 soft-iron off-diagonal X component
446
    // @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]]
447
    // @User: Advanced
448
    // @Calibration: 1
449

450
    // @Param: ODI2_Y
451
    // @DisplayName: Compass2 soft-iron off-diagonal Y component
452
    // @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]]
453
    // @User: Advanced
454
    // @Calibration: 1
455

456
    // @Param: ODI2_Z
457
    // @DisplayName: Compass2 soft-iron off-diagonal Z component
458
    // @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]]
459
    // @User: Advanced
460
    // @Calibration: 1
461
    AP_GROUPINFO("ODI2",    27, Compass, _state._priv_instance[1].offdiagonals, 0),
462
#endif // COMPASS_MAX_INSTANCES
463

464
#if COMPASS_MAX_INSTANCES > 2
465
    // @Param: DIA3_X
466
    // @DisplayName: Compass3 soft-iron diagonal X component
467
    // @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]]
468
    // @User: Advanced
469
    // @Calibration: 1
470

471
    // @Param: DIA3_Y
472
    // @DisplayName: Compass3 soft-iron diagonal Y component
473
    // @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]]
474
    // @User: Advanced
475
    // @Calibration: 1
476

477
    // @Param: DIA3_Z
478
    // @DisplayName: Compass3 soft-iron diagonal Z component
479
    // @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]]
480
    // @User: Advanced
481
    // @Calibration: 1
482
    AP_GROUPINFO("DIA3",    28, Compass, _state._priv_instance[2].diagonals, 1.0),
483

484
    // @Param: ODI3_X
485
    // @DisplayName: Compass3 soft-iron off-diagonal X component
486
    // @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]]
487
    // @User: Advanced
488
    // @Calibration: 1
489

490
    // @Param: ODI3_Y
491
    // @DisplayName: Compass3 soft-iron off-diagonal Y component
492
    // @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]]
493
    // @User: Advanced
494
    // @Calibration: 1
495

496
    // @Param: ODI3_Z
497
    // @DisplayName: Compass3 soft-iron off-diagonal Z component
498
    // @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]]
499
    // @User: Advanced
500
    // @Calibration: 1
501
    AP_GROUPINFO("ODI3",    29, Compass, _state._priv_instance[2].offdiagonals, 0),
502
#endif // COMPASS_MAX_INSTANCES
503
#endif // AP_COMPASS_DIAGONALS_ENABLED
504

505
#if COMPASS_CAL_ENABLED
506
    // @Param: CAL_FIT
507
    // @DisplayName: Compass calibration fitness
508
    // @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.
509
    // @Range: 4 32
510
    // @Values: 4:Very Strict,8:Strict,16:Default,32:Relaxed
511
    // @Increment: 0.1
512
    // @User: Advanced
513
    AP_GROUPINFO("CAL_FIT", 30, Compass, _calibration_threshold, AP_COMPASS_CALIBRATION_FITNESS_DEFAULT),
514
#endif
515

516
#ifndef HAL_BUILD_AP_PERIPH
517
    // @Param: OFFS_MAX
518
    // @DisplayName: Compass maximum offset
519
    // @Description: This sets the maximum allowed compass offset in calibration and arming checks
520
    // @Range: 500 3000
521
    // @Increment: 1
522
    // @User: Advanced
523
    AP_GROUPINFO("OFFS_MAX", 31, Compass, _offset_max, AP_COMPASS_OFFSETS_MAX_DEFAULT),
524
#endif
525

526
#if COMPASS_MOT_ENABLED
527
    // @Group: PMOT
528
    // @Path: Compass_PerMotor.cpp
529
    AP_SUBGROUPINFO(_per_motor, "PMOT", 32, Compass, Compass_PerMotor),
530
#endif
531

532
    // @Param: DISBLMSK
533
    // @DisplayName: Compass disable driver type mask
534
    // @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
535
    // @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,23:LIS2MDL
536
    // @User: Advanced
537
    AP_GROUPINFO("DISBLMSK", 33, Compass, _driver_type_mask, 0),
538

539
    // @Param: FLTR_RNG
540
    // @DisplayName: Range in which sample is accepted
541
    // @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.
542
    // @Units: %
543
    // @Range: 0 100
544
    // @Increment: 1
545
    AP_GROUPINFO("FLTR_RNG", 34, Compass, _filter_range, HAL_COMPASS_FILTER_DEFAULT),
546

547
#if COMPASS_CAL_ENABLED
548
    // @Param: AUTO_ROT
549
    // @DisplayName: Automatically check orientation
550
    // @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.
551
    // @Values: 0:Disabled,1:CheckOnly,2:CheckAndFix,3:use same tolerance to auto rotate 45 deg rotations
552
    AP_GROUPINFO("AUTO_ROT", 35, Compass, _rotate_auto, HAL_COMPASS_AUTO_ROT_DEFAULT),
553
#endif
554

555
#if COMPASS_MAX_INSTANCES > 1
556
    // @Param: PRIO1_ID
557
    // @DisplayName: Compass device id with 1st order priority
558
    // @Description: Compass device id with 1st order priority, set automatically if 0. Reboot required after change.
559
    // @RebootRequired: True
560
    // @User: Advanced
561
    AP_GROUPINFO("PRIO1_ID",  36, Compass, _priority_did_stored_list._priv_instance[0], 0),
562

563
    // @Param: PRIO2_ID
564
    // @DisplayName: Compass device id with 2nd order priority
565
    // @Description: Compass device id with 2nd order priority, set automatically if 0. Reboot required after change.
566
    // @RebootRequired: True
567
    // @User: Advanced
568
    AP_GROUPINFO("PRIO2_ID", 37, Compass, _priority_did_stored_list._priv_instance[1], 0),
569
#endif // COMPASS_MAX_INSTANCES
570

571
#if COMPASS_MAX_INSTANCES > 2
572
    // @Param: PRIO3_ID
573
    // @DisplayName: Compass device id with 3rd order priority
574
    // @Description: Compass device id with 3rd order priority, set automatically if 0. Reboot required after change.
575
    // @RebootRequired: True
576
    // @User: Advanced
577
    AP_GROUPINFO("PRIO3_ID", 38, Compass, _priority_did_stored_list._priv_instance[2], 0),
578
#endif // COMPASS_MAX_INSTANCES
579

580
    // @Param: ENABLE
581
    // @DisplayName: Enable Compass
582
    // @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.
583
    // @User: Standard
584
    // @RebootRequired: True
585
    // @Values: 0:Disabled,1:Enabled
586
    AP_GROUPINFO("ENABLE", 39, Compass, _enabled, AP_COMPASS_ENABLE_DEFAULT),
587

588
#ifndef HAL_BUILD_AP_PERIPH
589
    // @Param: SCALE
590
    // @DisplayName: Compass1 scale factor
591
    // @Description: Scaling factor for first compass to compensate for sensor scaling errors. If this is 0 then no scaling is done
592
    // @User: Standard
593
    // @Range: 0 1.3
594
    AP_GROUPINFO("SCALE", 40, Compass, _state._priv_instance[0].scale_factor, 0),
595

596
#if COMPASS_MAX_INSTANCES > 1
597
    // @Param: SCALE2
598
    // @DisplayName: Compass2 scale factor
599
    // @Description: Scaling factor for 2nd compass to compensate for sensor scaling errors. If this is 0 then no scaling is done
600
    // @User: Standard
601
    // @Range: 0 1.3
602
    AP_GROUPINFO("SCALE2", 41, Compass, _state._priv_instance[1].scale_factor, 0),
603
#endif
604

605
#if COMPASS_MAX_INSTANCES > 2
606
    // @Param: SCALE3
607
    // @DisplayName: Compass3 scale factor
608
    // @Description: Scaling factor for 3rd compass to compensate for sensor scaling errors. If this is 0 then no scaling is done
609
    // @User: Standard
610
    // @Range: 0 1.3
611
    AP_GROUPINFO("SCALE3", 42, Compass, _state._priv_instance[2].scale_factor, 0),
612
#endif
613
#endif // HAL_BUILD_AP_PERIPH
614

615
#ifndef HAL_BUILD_AP_PERIPH
616
    // @Param: OPTIONS
617
    // @DisplayName: Compass options
618
    // @Description: This sets options to change the behaviour of the compass
619
    // @Bitmask: 0:CalRequireGPS
620
    // @Bitmask: 1: Allow missing DroneCAN compasses to be automaticaly replaced (calibration still required)
621
    // @User: Advanced
622
    AP_GROUPINFO("OPTIONS", 43, Compass, _options, 0),
623
#endif
624

625
#if COMPASS_MAX_UNREG_DEV > 0
626
    // @Param: DEV_ID4
627
    // @DisplayName: Compass4 device id
628
    // @Description: Extra 4th compass's device id.  Automatically detected, do not set manually
629
    // @ReadOnly: True
630
    // @User: Advanced
631
    AP_GROUPINFO("DEV_ID4", 44, Compass, extra_dev_id[0], 0),
632
#endif // COMPASS_MAX_UNREG_DEV
633

634
#if COMPASS_MAX_UNREG_DEV > 1
635
    // @Param: DEV_ID5
636
    // @DisplayName: Compass5 device id
637
    // @Description: Extra 5th compass's device id.  Automatically detected, do not set manually
638
    // @ReadOnly: True
639
    // @User: Advanced
640
    AP_GROUPINFO("DEV_ID5", 45, Compass, extra_dev_id[1], 0),
641
#endif // COMPASS_MAX_UNREG_DEV
642

643
#if COMPASS_MAX_UNREG_DEV > 2
644
    // @Param: DEV_ID6
645
    // @DisplayName: Compass6 device id
646
    // @Description: Extra 6th compass's device id.  Automatically detected, do not set manually
647
    // @ReadOnly: True
648
    // @User: Advanced
649
    AP_GROUPINFO("DEV_ID6", 46, Compass, extra_dev_id[2], 0),
650
#endif // COMPASS_MAX_UNREG_DEV
651

652
#if COMPASS_MAX_UNREG_DEV > 3
653
    // @Param: DEV_ID7
654
    // @DisplayName: Compass7 device id
655
    // @Description: Extra 7th compass's device id.  Automatically detected, do not set manually
656
    // @ReadOnly: True
657
    // @User: Advanced
658
    AP_GROUPINFO("DEV_ID7", 47, Compass, extra_dev_id[3], 0),
659
#endif // COMPASS_MAX_UNREG_DEV
660

661
#if COMPASS_MAX_UNREG_DEV > 4
662
    // @Param: DEV_ID8
663
    // @DisplayName: Compass8 device id
664
    // @Description: Extra 8th compass's device id.  Automatically detected, do not set manually
665
    // @ReadOnly: True
666
    // @User: Advanced
667
    AP_GROUPINFO("DEV_ID8", 48, Compass, extra_dev_id[4], 0),
668
#endif // COMPASS_MAX_UNREG_DEV
669

670
    // @Param: CUS_ROLL
671
    // @DisplayName: Custom orientation roll offset
672
    // @Description: Compass mounting position roll offset. Positive values = roll right, negative values = roll left. This parameter is only used when COMPASS_ORIENT/2/3 is set to CUSTOM.
673
    // @Range: -180 180
674
    // @Units: deg
675
    // @Increment: 1
676
    // @RebootRequired: True
677
    // @User: Advanced
678

679
    // index 49
680

681
    // @Param: CUS_PIT
682
    // @DisplayName: Custom orientation pitch offset
683
    // @Description: Compass mounting position pitch offset. Positive values = pitch up, negative values = pitch down. This parameter is only used when COMPASS_ORIENT/2/3 is set to CUSTOM.
684
    // @Range: -180 180
685
    // @Units: deg
686
    // @Increment: 1
687
    // @RebootRequired: True
688
    // @User: Advanced
689

690
    // index 50
691

692
    // @Param: CUS_YAW
693
    // @DisplayName: Custom orientation yaw offset
694
    // @Description: Compass mounting position yaw offset. Positive values = yaw right, negative values = yaw left. This parameter is only used when COMPASS_ORIENT/2/3 is set to CUSTOM.
695
    // @Range: -180 180
696
    // @Units: deg
697
    // @Increment: 1
698
    // @RebootRequired: True
699
    // @User: Advanced
700

701
    // index 51
702

703
    AP_GROUPEND
704
};
705

706
// Default constructor.
707
// Note that the Vector/Matrix constructors already implicitly zero
708
// their values.
709
//
710
Compass::Compass(void)
711
{
712
    if (_singleton != nullptr) {
713
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
714
        AP_HAL::panic("Compass must be singleton");
715
#endif
716
        return;
717
    }
718
    _singleton = this;
719
    AP_Param::setup_object_defaults(this, var_info);
720
}
721

722
// Default init method
723
//
724
void Compass::init()
725
{
726
    if (!_enabled) {
727
        return;
728
    }
729

730
    /*
731
      on init() if any devid is set then we set suppress_devid_save to
732
      false. This is used to determine if we save device ids during
733
      the init process.
734
     */
735
    suppress_devid_save = true;
736
    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
737
        if (_state._priv_instance[i].dev_id != 0) {
738
            suppress_devid_save = false;
739
            break;
740
        }
741
#if COMPASS_MAX_INSTANCES > 1
742
        if (_priority_did_stored_list._priv_instance[i] != 0) {
743
            suppress_devid_save = false;
744
            break;
745
        }
746
#endif
747
    }
748

749
    // convert to new custom rotation method
750
    // PARAMETER_CONVERSION - Added: Nov-2021
751
#if AP_CUSTOMROTATIONS_ENABLED
752
    for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
753
        if (_state[i].orientation != ROTATION_CUSTOM_OLD) {
754
            continue;
755
        }
756
        _state[i].orientation.set_and_save(ROTATION_CUSTOM_2);
757
        AP_Param::ConversionInfo info;
758
        if (AP_Param::find_top_level_key_by_pointer(this, info.old_key)) {
759
            info.type = AP_PARAM_FLOAT;
760
            float rpy[3] = {};
761
            AP_Float rpy_param;
762
            for (info.old_group_element=49; info.old_group_element<=51; info.old_group_element++) {
763
                if (AP_Param::find_old_parameter(&info, &rpy_param)) {
764
                    rpy[info.old_group_element-49] = rpy_param.get();
765
                }
766
            }
767
            AP::custom_rotations().convert(ROTATION_CUSTOM_2, rpy[0], rpy[1], rpy[2]);
768
        }
769
        break;
770
    }
771
#endif  // AP_CUSTOMROTATIONS_ENABLED
772

773
#if COMPASS_MAX_INSTANCES > 1
774
    // Look if there was a primary compass setup in previous version
775
    // if so and the primary compass is not set in current setup
776
    // make the devid as primary.
777
    if (_priority_did_stored_list[Priority(0)] == 0) {
778
        uint16_t k_param_compass;
779
        if (AP_Param::find_top_level_key_by_pointer(this, k_param_compass)) {
780
            const AP_Param::ConversionInfo primary_compass_old_param = {k_param_compass, 12, AP_PARAM_INT8, ""};
781
            AP_Int8 value;
782
            value.set(0);
783
            bool primary_param_exists = AP_Param::find_old_parameter(&primary_compass_old_param, &value);
784
            int8_t oldvalue = value.get();
785
            if ((oldvalue!=0) && (oldvalue<COMPASS_MAX_INSTANCES) && primary_param_exists) {
786
                _priority_did_stored_list[Priority(0)].set_and_save_ifchanged(_state[StateIndex(oldvalue)].dev_id);
787
            }
788
        }
789
    }
790

791
    // Load priority list from storage, the changes to priority list
792
    // by user only take effect post reboot, after this
793
    if (!suppress_devid_save) {
794
        for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
795
            if (_priority_did_stored_list[i] != 0) {
796
                _priority_did_list[i] = _priority_did_stored_list[i];
797
            } else {
798
                // Maintain a list without gaps and duplicates
799
                for (Priority j(i+1); j<COMPASS_MAX_INSTANCES; j++) {
800
                    int32_t temp;
801
                    if (_priority_did_stored_list[j] == _priority_did_stored_list[i]) {
802
                        _priority_did_stored_list[j].set_and_save_ifchanged(0);
803
                    }
804
                    if (_priority_did_stored_list[j] == 0) {
805
                        continue;
806
                    }
807
                    temp = _priority_did_stored_list[j];
808
                    _priority_did_stored_list[j].set_and_save_ifchanged(0);
809
                    _priority_did_list[i] = temp;
810
                    _priority_did_stored_list[i].set_and_save_ifchanged(temp);
811
                    break;
812
                }
813
            }
814
        }
815
    }
816
#endif // COMPASS_MAX_INSTANCES
817

818
    // cache expected dev ids for use during runtime detection
819
    for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
820
        _state[i].expected_dev_id = _state[i].dev_id;
821
    }
822

823
#if COMPASS_MAX_UNREG_DEV
824
    // set the dev_id to 0 for undetected compasses. extra_dev_id is just an
825
    // interface for users to see unreg compasses, we actually never store it
826
    // in storage.
827
    for (uint8_t i=_unreg_compass_count; i<COMPASS_MAX_UNREG_DEV; i++) {
828
        // cache the extra devices detected in last boot
829
        // for detecting replacement mag
830
        _previously_unreg_mag[i] = extra_dev_id[i];
831
        extra_dev_id[i].set(0);
832
    }
833
#endif
834

835
#if COMPASS_MAX_INSTANCES > 1
836
    // This method calls set_and_save_ifchanged on parameters
837
    // which are set() but not saved() during normal runtime,
838
    // do not move this call without ensuring that is not happening
839
    // read comments under set_and_save_ifchanged for details
840
    if (!suppress_devid_save) {
841
        _reorder_compass_params();
842
    }
843
#endif
844

845
    if (_compass_count == 0) {
846
        // detect available backends. Only called once
847
        _detect_backends();
848
    }
849

850
    if (_compass_count != 0) {
851
        // get initial health status
852
        hal.scheduler->delay(100);
853
        read();
854
    }
855
    // set the dev_id to 0 for undetected compasses, to make it easier
856
    // for users to see how many compasses are detected. We don't do a
857
    // set_and_save() as the user may have temporarily removed the
858
    // compass, and we don't want to force a re-cal if they plug it
859
    // back in again
860
    for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
861
        if (!_state[i].registered) {
862
            _state[i].dev_id.set(0);
863
        }
864
    }
865

866
#ifndef HAL_BUILD_AP_PERIPH
867
    // updating the AHRS orientation updates our own orientation:
868
    AP::ahrs().update_orientation();
869
#endif
870

871
    init_done = true;
872
    suppress_devid_save = false;
873
}
874

875
#if COMPASS_MAX_INSTANCES > 1 || COMPASS_MAX_UNREG_DEV
876
// Update Priority List for Mags, by default, we just
877
// load them as they come up the first time
878
Compass::Priority Compass::_update_priority_list(int32_t dev_id)
879
{
880
    // Check if already in priority list
881
    for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
882
        if (_priority_did_list[i] == dev_id) {
883
            if (i >= _compass_count) {
884
                _compass_count = uint8_t(i)+1;
885
            }
886
            return i;
887
        }
888
    }
889

890
    // We are not in priority list, let's add at first empty
891
    for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
892
        if (_priority_did_stored_list[i] == 0) {
893
            if (suppress_devid_save) {
894
                _priority_did_stored_list[i].set(dev_id);
895
            } else {
896
                _priority_did_stored_list[i].set_and_save(dev_id);
897
            }
898
            _priority_did_list[i] = dev_id;
899
            if (i >= _compass_count) {
900
                _compass_count = uint8_t(i)+1;
901
            }
902
            return i;
903
        }
904
    }
905
    return Priority(COMPASS_MAX_INSTANCES);
906
}
907
#endif
908

909

910
#if COMPASS_MAX_INSTANCES > 1
911
// This method reorganises devid list to match
912
// priority list, only call before detection at boot
913
void Compass::_reorder_compass_params()
914
{
915
    mag_state swap_state;
916
    StateIndex curr_state_id;
917
    for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
918
        if (_priority_did_list[i] == 0) {
919
            continue;
920
        }
921
        curr_state_id = COMPASS_MAX_INSTANCES;
922
        for (StateIndex j(0); j<COMPASS_MAX_INSTANCES; j++) {
923
            if (_priority_did_list[i] == _state[j].dev_id) {
924
                curr_state_id = j;
925
                break;
926
            }
927
        }
928
        if (curr_state_id != COMPASS_MAX_INSTANCES && uint8_t(curr_state_id) != uint8_t(i)) {
929
            //let's swap
930
            swap_state.copy_from(_state[curr_state_id]);
931
            _state[curr_state_id].copy_from(_state[StateIndex(uint8_t(i))]);
932
            _state[StateIndex(uint8_t(i))].copy_from(swap_state);
933
        }
934
    }
935
}
936
#endif
937

938
void Compass::mag_state::copy_from(const Compass::mag_state& state)
939
{
940
    external.set_and_save_ifchanged(state.external);
941
    orientation.set_and_save_ifchanged(state.orientation);
942
    offset.set_and_save_ifchanged(state.offset);
943
#if AP_COMPASS_DIAGONALS_ENABLED
944
    diagonals.set_and_save_ifchanged(state.diagonals);
945
    offdiagonals.set_and_save_ifchanged(state.offdiagonals);
946
#endif
947
    scale_factor.set_and_save_ifchanged(state.scale_factor);
948
    dev_id.set_and_save_ifchanged(state.dev_id);
949
    motor_compensation.set_and_save_ifchanged(state.motor_compensation);
950
    expected_dev_id = state.expected_dev_id;
951
    detected_dev_id = state.detected_dev_id;
952
}
953
//  Register a new compass instance
954
//
955
bool Compass::register_compass(int32_t dev_id, uint8_t& instance)
956
{
957

958
#if COMPASS_MAX_INSTANCES == 1 && !COMPASS_MAX_UNREG_DEV
959
    // simple single compass setup for AP_Periph
960
    Priority priority(0);
961
    StateIndex i(0);
962
    if (_state[i].registered) {
963
        return false;
964
    }
965
    _state[i].registered = true;
966
    _state[i].priority = priority;
967
    instance = uint8_t(i);
968
    _compass_count = 1;
969
    return true;
970
#else
971
    Priority priority;
972
    // Check if we already have this dev_id registered
973
    for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
974
        priority = _update_priority_list(dev_id);
975
        if (_state[i].expected_dev_id == dev_id && priority < COMPASS_MAX_INSTANCES) {
976
            _state[i].registered = true;
977
            _state[i].priority = priority;
978
            instance = uint8_t(i);
979
            return true;
980
        }
981
    }
982

983
    // This is an unregistered compass, check if any free slot is available
984
    for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
985
        priority = _update_priority_list(dev_id);
986
        if (_state[i].dev_id == 0 && priority < COMPASS_MAX_INSTANCES) {
987
            _state[i].registered = true;
988
            _state[i].priority = priority;
989
            instance = uint8_t(i);
990
            return true;
991
        }
992
    }
993

994
    // This might be a replacement compass module, find any unregistered compass
995
    // instance and replace that
996
    priority = _update_priority_list(dev_id);
997
    // try to match priority and state index if possible, this ensure that compass order
998
    // to state order while detection is preserved, this ensures that if compasses in priority
999
    // list show up out of order during detection, it does not replace the state.
1000
    StateIndex priority_index = StateIndex(uint8_t(priority));
1001
    if (!_state[priority_index].registered && priority < COMPASS_MAX_INSTANCES) {
1002
        _state[priority_index].registered = true;
1003
        _state[priority_index].priority = priority;
1004
        instance = uint8_t(priority_index);
1005
        return true;
1006
    }
1007
    for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
1008
        priority = _update_priority_list(dev_id);
1009
        if (!_state[i].registered && priority < COMPASS_MAX_INSTANCES) {
1010
            _state[i].registered = true;
1011
            _state[i].priority = priority;
1012
            instance = uint8_t(i);
1013
            return true;
1014
        }
1015
    }
1016
#endif
1017

1018
#if COMPASS_MAX_UNREG_DEV
1019
    // Set extra dev id
1020
    if (_unreg_compass_count >= COMPASS_MAX_UNREG_DEV) {
1021
        AP_HAL::panic("Too many compass instances");
1022
    }
1023

1024
    for (uint8_t i=0; i<COMPASS_MAX_UNREG_DEV; i++) {
1025
        if (extra_dev_id[i] == dev_id) {
1026
            if (i >= _unreg_compass_count) {
1027
                _unreg_compass_count = i+1;
1028
            }
1029
            instance = i+COMPASS_MAX_INSTANCES;
1030
            return false;
1031
        } else if (extra_dev_id[i] == 0) {
1032
            extra_dev_id[_unreg_compass_count++].set(dev_id);
1033
            instance = i+COMPASS_MAX_INSTANCES;
1034
            return false;
1035
        }
1036
    }
1037
#else
1038
    AP_HAL::panic("Too many compass instances");
1039
#endif
1040

1041
    return false;
1042
}
1043

1044
Compass::StateIndex Compass::_get_state_id(Compass::Priority priority) const
1045
{
1046
#if COMPASS_MAX_INSTANCES > 1
1047
    if (_priority_did_list[priority] == 0) {
1048
        return StateIndex(COMPASS_MAX_INSTANCES);
1049
    }
1050
    for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
1051
        if (_priority_did_list[priority] == _state[i].detected_dev_id) {
1052
            return i;
1053
        }
1054
    }
1055
    return StateIndex(COMPASS_MAX_INSTANCES);
1056
#else
1057
    return StateIndex(0);
1058
#endif
1059
}
1060

1061
/*
1062
  return true if a driver type is enabled
1063
 */
1064
bool Compass::_driver_enabled(enum DriverType driver_type)
1065
{
1066
    uint32_t mask = (1U<<uint8_t(driver_type));
1067
    return (mask & uint32_t(_driver_type_mask.get())) == 0;
1068
}
1069

1070
/*
1071
  wrapper around hal.i2c_mgr->get_device() that prevents duplicate devices being opened
1072
 */
1073
bool Compass::_i2c_sensor_is_registered(uint8_t bus, uint8_t address) const
1074
{
1075
    for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
1076
        if (!_state[i].registered) {
1077
            continue;
1078
        }
1079
        if (AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_I2C, bus, address, 0) ==
1080
            AP_HAL::Device::change_bus_id(uint32_t(_state[i].dev_id.get()), 0)) {
1081
            // we are already using this device
1082
            return true;
1083
        }
1084
    }
1085
    return false;
1086
}
1087

1088
#if COMPASS_MAX_UNREG_DEV > 0
1089
#define RETURN_IF_NO_SPACE  if (_unreg_compass_count == COMPASS_MAX_UNREG_DEV) return
1090
#else
1091
#define RETURN_IF_NO_SPACE
1092
#endif
1093

1094
/*
1095
  macro to add a backend with check for too many backends or compass
1096
  instances. We don't try to start more than the maximum allowed
1097
 */
1098
void Compass::add_backend(Compass::DriverType driver_type, AP_Compass_Backend *backend)
1099
{
1100
    if (!_driver_enabled(driver_type)) {
1101
        return;
1102
    }
1103

1104
    if (!backend) {
1105
        return;
1106
    }
1107

1108
    if (_backend_count == COMPASS_MAX_BACKEND) {
1109
        return;
1110
    }
1111

1112
    _backends[_backend_count++] = backend;
1113
}
1114

1115
#define GET_I2C_DEVICE(bus, address) _i2c_sensor_is_registered(bus, address)?nullptr:hal.i2c_mgr->get_device(bus, address)
1116

1117
/*
1118
  look for compasses on external i2c buses
1119
 */
1120
void Compass::_probe_external_i2c_compasses(void)
1121
{
1122
#if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1123
    bool all_external = (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2);
1124
    (void)all_external;  // in case all backends using this are compiled out
1125
#endif  // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1126

1127
#if AP_COMPASS_HMC5843_ENABLED
1128
    // external i2c bus
1129
    FOREACH_I2C_EXTERNAL(i) {
1130
        probe_i2c_dev(DRIVER_HMC5843, AP_Compass_HMC5843::probe, i, HAL_COMPASS_HMC5843_I2C_ADDR, true, ROTATION_ROLL_180);
1131
        RETURN_IF_NO_SPACE;
1132
    }
1133

1134
#if AP_COMPASS_HMC5843_INTERNAL_BUS_PROBING_ENABLED
1135
    if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_AEROFC) {
1136
        // internal i2c bus
1137
        FOREACH_I2C_INTERNAL(i) {
1138
            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);
1139
            RETURN_IF_NO_SPACE;
1140
        }
1141
    }
1142
#endif  // AP_COMPASS_KMC5843_INTERNAL_BUS_PROBING_ENABLED
1143
#endif  // AP_COMPASS_HMC5843_ENABLED
1144

1145
#if AP_COMPASS_QMC5883L_ENABLED
1146
    //external i2c bus
1147
    FOREACH_I2C_EXTERNAL(i) {
1148
        probe_i2c_dev(DRIVER_QMC5883L, AP_Compass_QMC5883L::probe, i, HAL_COMPASS_QMC5883L_I2C_ADDR, true, HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL);
1149
        RETURN_IF_NO_SPACE;
1150
    }
1151

1152
    // internal i2c bus
1153
#if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1154
    if (all_external) {
1155
        // only probe QMC5883L on internal if we are treating internals as externals
1156
        FOREACH_I2C_INTERNAL(i) {
1157
            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);
1158
            RETURN_IF_NO_SPACE;
1159
        }
1160
    }
1161
#endif  // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1162
#endif  // AP_COMPASS_QMC5883L_ENABLED
1163

1164
#if AP_COMPASS_QMC5883P_ENABLED
1165
    //external i2c bus
1166
    FOREACH_I2C_EXTERNAL(i) {
1167
        probe_i2c_dev(DRIVER_QMC5883P, AP_Compass_QMC5883P::probe, i, HAL_COMPASS_QMC5883P_I2C_ADDR, true, HAL_COMPASS_QMC5883P_ORIENTATION_EXTERNAL);
1168
        RETURN_IF_NO_SPACE;
1169
    }
1170

1171
    // internal i2c bus
1172
#if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1173
    if (all_external) {
1174
        // only probe QMC5883P on internal if we are treating internals as externals
1175
        FOREACH_I2C_INTERNAL(i) {
1176
            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);
1177
            RETURN_IF_NO_SPACE;
1178
        }
1179
    }
1180
#endif  // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1181
#endif  // AP_COMPASS_QMC5883P_ENABLED
1182

1183
#if AP_COMPASS_IIS2MDC_ENABLED
1184
    //external i2c bus
1185
    FOREACH_I2C_EXTERNAL(i) {
1186
        probe_i2c_dev(DRIVER_IIS2MDC, AP_Compass_IIS2MDC::probe, i, HAL_COMPASS_IIS2MDC_I2C_ADDR, true, HAL_COMPASS_IIS2MDC_ORIENTATION_EXTERNAL);
1187
        RETURN_IF_NO_SPACE;
1188
    }
1189

1190
    // internal i2c bus
1191
#if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1192
    if (all_external) {
1193
        // only probe IIS2MDC on internal if we are treating internals as externals
1194
        FOREACH_I2C_INTERNAL(i) {
1195
            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);
1196
            RETURN_IF_NO_SPACE;
1197
        }
1198
    }
1199
#endif  // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1200
#endif  // AP_COMPASS_QMC5883P_ENABLED
1201

1202
    // AK09916 on ICM20948
1203
#if AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED
1204
    FOREACH_I2C_EXTERNAL(i) {
1205
        probe_ak09916_via_icm20948(i, HAL_COMPASS_AK09916_I2C_ADDR, HAL_COMPASS_ICM20948_I2C_ADDR, true, ROTATION_PITCH_180_YAW_90);
1206
        RETURN_IF_NO_SPACE;
1207
        probe_ak09916_via_icm20948(i, HAL_COMPASS_AK09916_I2C_ADDR, HAL_COMPASS_ICM20948_I2C_ADDR2, true, ROTATION_PITCH_180_YAW_90);
1208
        RETURN_IF_NO_SPACE;
1209
    }
1210

1211
#if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1212
    FOREACH_I2C_INTERNAL(i) {
1213
        probe_ak09916_via_icm20948(i, HAL_COMPASS_AK09916_I2C_ADDR, HAL_COMPASS_ICM20948_I2C_ADDR, all_external, ROTATION_PITCH_180_YAW_90);
1214
        RETURN_IF_NO_SPACE;
1215
        probe_ak09916_via_icm20948(i, HAL_COMPASS_AK09916_I2C_ADDR, HAL_COMPASS_ICM20948_I2C_ADDR2, all_external, ROTATION_PITCH_180_YAW_90);
1216
        RETURN_IF_NO_SPACE;
1217
    }
1218
#endif  // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1219
#endif  // AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED
1220

1221
#if AP_COMPASS_LIS3MDL_ENABLED
1222
    // lis3mdl on bus 0 with default address
1223
#if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1224
    FOREACH_I2C_INTERNAL(i) {
1225
        probe_i2c_dev(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe, i, HAL_COMPASS_LIS3MDL_I2C_ADDR, all_external, all_external?ROTATION_YAW_90:ROTATION_NONE);
1226
        RETURN_IF_NO_SPACE;
1227
    }
1228

1229
    // lis3mdl on bus 0 with alternate address
1230
    FOREACH_I2C_INTERNAL(i) {
1231
        probe_i2c_dev(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe, i, HAL_COMPASS_LIS3MDL_I2C_ADDR2, all_external, all_external?ROTATION_YAW_90:ROTATION_NONE);
1232
        RETURN_IF_NO_SPACE;
1233
    }
1234
#endif
1235
    // external lis3mdl on bus 1 with default address
1236
    FOREACH_I2C_EXTERNAL(i) {
1237
        probe_i2c_dev(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe, i, HAL_COMPASS_LIS3MDL_I2C_ADDR, true, ROTATION_YAW_90);
1238
        RETURN_IF_NO_SPACE;
1239
    }
1240

1241
    // external lis3mdl on bus 1 with alternate address
1242
    FOREACH_I2C_EXTERNAL(i) {
1243
        probe_i2c_dev(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe, i, HAL_COMPASS_LIS3MDL_I2C_ADDR2, true, ROTATION_YAW_90);
1244
        RETURN_IF_NO_SPACE;
1245
    }
1246
#endif  // AP_COMPASS_LIS3MDL_ENABLED
1247

1248
#if AP_COMPASS_LIS2MDL_ENABLED
1249
    // external lis2mdl
1250
#if AP_COMPASS_LIS2MDL_EXTERNAL_BUS_PROBING_ENABLED
1251
    FOREACH_I2C_EXTERNAL(i) {
1252
        probe_i2c_dev(DRIVER_LIS2MDL, AP_Compass_LIS2MDL::probe, i, HAL_COMPASS_LIS2MDL_I2C_ADDR, true, ROTATION_NONE);
1253
        RETURN_IF_NO_SPACE;
1254
    }
1255
#endif
1256
    // internal lis2mdl
1257
#if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1258
    FOREACH_I2C_INTERNAL(i) {
1259
        probe_i2c_dev(DRIVER_LIS2MDL, AP_Compass_LIS2MDL::probe, i, HAL_COMPASS_LIS2MDL_I2C_ADDR, all_external, ROTATION_NONE);
1260
        RETURN_IF_NO_SPACE;
1261
    }
1262
#endif
1263
#endif  // AP_COMPASS_LIS2MDL_ENABLED
1264

1265
#if AP_COMPASS_AK09916_ENABLED
1266
    // AK09916. This can be found twice, due to the ICM20948 i2c bus pass-thru, so we need to be careful to avoid that
1267
    FOREACH_I2C_EXTERNAL(i) {
1268
        probe_i2c_dev(DRIVER_AK09916, AP_Compass_AK09916::probe, i, HAL_COMPASS_AK09916_I2C_ADDR, true, ROTATION_YAW_270);
1269
        RETURN_IF_NO_SPACE;
1270
    }
1271
#if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1272
    FOREACH_I2C_INTERNAL(i) {
1273
        probe_i2c_dev(DRIVER_AK09916, AP_Compass_AK09916::probe, i, HAL_COMPASS_AK09916_I2C_ADDR, all_external, all_external?ROTATION_YAW_270:ROTATION_NONE);
1274
        RETURN_IF_NO_SPACE;
1275
    }
1276
#endif  // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1277
#endif  // AP_COMPASS_AK09916_ENABLED
1278

1279
#if AP_COMPASS_IST8310_EXTERNAL_BUS_PROBING_ENABLED || AP_COMPASS_IST8310_INTERNAL_BUS_PROBING_ENABLED
1280
    // IST8310 on external and internal bus
1281
    if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_FMUV6) {
1282
        enum Rotation default_rotation = AP_COMPASS_IST8310_DEFAULT_ROTATION;
1283

1284
        if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_AEROFC) {
1285
            default_rotation = ROTATION_PITCH_180_YAW_90;
1286
        }
1287
        // probe all 4 possible addresses
1288
        const uint8_t ist8310_addr[] = { 0x0C, 0x0D, 0x0E, 0x0F };
1289

1290
        for (uint8_t a=0; a<ARRAY_SIZE(ist8310_addr); a++) {
1291
#if AP_COMPASS_IST8310_EXTERNAL_BUS_PROBING_ENABLED
1292
            FOREACH_I2C_EXTERNAL(i) {
1293
                probe_i2c_dev(DRIVER_IST8310, AP_Compass_IST8310::probe, i, ist8310_addr[a], true, default_rotation);
1294
                RETURN_IF_NO_SPACE;
1295
            }
1296
#endif  // AP_COMPASS_IST8310_EXTERNAL_BUS_PROBING_ENABLED
1297
#if AP_COMPASS_IST8310_INTERNAL_BUS_PROBING_ENABLED
1298
            FOREACH_I2C_INTERNAL(i) {
1299
                probe_i2c_dev(DRIVER_IST8310, AP_Compass_IST8310::probe, i, ist8310_addr[a], all_external, default_rotation);
1300
                RETURN_IF_NO_SPACE;
1301
            }
1302
#endif  // AP_COMPASS_IST8310_INTERNAL_BUS_PROBING_ENABLED
1303
        }
1304
    }
1305
#endif  // AP_COMPASS_IST8310_EXTERNAL_BUS_PROBING_ENABLED || AP_COMPASS_IST8310_INTERNAL_BUS_PROBING_ENABLED
1306

1307
#if AP_COMPASS_IST8308_ENABLED
1308
    // external i2c bus
1309
    FOREACH_I2C_EXTERNAL(i) {
1310
        probe_i2c_dev(DRIVER_IST8308, AP_Compass_IST8308::probe, i, HAL_COMPASS_IST8308_I2C_ADDR, true, ROTATION_NONE);
1311
        RETURN_IF_NO_SPACE;
1312
    }
1313
#if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1314
    FOREACH_I2C_INTERNAL(i) {
1315
        probe_i2c_dev(DRIVER_IST8308, AP_Compass_IST8308::probe, i, HAL_COMPASS_IST8308_I2C_ADDR, all_external, ROTATION_NONE);
1316
        RETURN_IF_NO_SPACE;
1317
    }
1318
#endif  // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1319
#endif  // AP_COMPASS_IST8308_ENABLED
1320

1321
#if AP_COMPASS_MMC3416_ENABLED
1322
    // external i2c bus
1323
    FOREACH_I2C_EXTERNAL(i) {
1324
        probe_i2c_dev(DRIVER_MMC3416, AP_Compass_MMC3416::probe, i, HAL_COMPASS_MMC3416_I2C_ADDR, true, ROTATION_NONE);
1325
        RETURN_IF_NO_SPACE;
1326
    }
1327
#if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1328
    FOREACH_I2C_INTERNAL(i) {
1329
        probe_i2c_dev(DRIVER_MMC3416, AP_Compass_MMC3416::probe, i, HAL_COMPASS_MMC3416_I2C_ADDR, all_external, ROTATION_NONE);
1330
        RETURN_IF_NO_SPACE;
1331
    }
1332
#endif  // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1333
#endif  // AP_COMPASS_MMC3416_ENABLED
1334

1335
#if AP_COMPASS_MMC5XX3_ENABLED
1336
    // external i2c bus
1337
    FOREACH_I2C_EXTERNAL(i) {
1338
        probe_i2c_dev(DRIVER_MMC5XX3, AP_Compass_MMC5XX3::probe, i, HAL_COMPASS_MMC5xx3_I2C_ADDR, true, ROTATION_NONE);
1339
        RETURN_IF_NO_SPACE;
1340
    }
1341
#endif  // AP_COMPASS_MMC5XX3_ENABLED (MMC5983MA)
1342

1343
#if AP_COMPASS_RM3100_ENABLED
1344
#ifdef HAL_COMPASS_RM3100_I2C_ADDR
1345
    const uint8_t rm3100_addresses[] = { HAL_COMPASS_RM3100_I2C_ADDR };
1346
#else
1347
    // RM3100 can be on 4 different addresses
1348
    const uint8_t rm3100_addresses[] = { HAL_COMPASS_RM3100_I2C_ADDR1,
1349
                                         HAL_COMPASS_RM3100_I2C_ADDR2,
1350
                                         HAL_COMPASS_RM3100_I2C_ADDR3,
1351
                                         HAL_COMPASS_RM3100_I2C_ADDR4 };
1352
#endif
1353
    // external i2c bus
1354
    FOREACH_I2C_EXTERNAL(i) {
1355
        for (uint8_t j=0; j<ARRAY_SIZE(rm3100_addresses); j++) {
1356
            probe_i2c_dev(DRIVER_RM3100, AP_Compass_RM3100::probe, i, rm3100_addresses[j], true, ROTATION_NONE);
1357
            RETURN_IF_NO_SPACE;
1358
        }
1359
    }
1360

1361
#if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1362
    FOREACH_I2C_INTERNAL(i) {
1363
        for (uint8_t j=0; j<ARRAY_SIZE(rm3100_addresses); j++) {
1364
            probe_i2c_dev(DRIVER_RM3100, AP_Compass_RM3100::probe, i, rm3100_addresses[j], all_external, ROTATION_NONE);
1365
            RETURN_IF_NO_SPACE;
1366
        }
1367
    }
1368
#endif  // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1369
#endif  // AP_COMPASS_RM3100_ENABLED
1370

1371
#if AP_COMPASS_BMM150_ENABLED
1372
    // BMM150 on I2C
1373
    FOREACH_I2C_EXTERNAL(i) {
1374
        for (uint8_t addr=BMM150_I2C_ADDR_MIN; addr <= BMM150_I2C_ADDR_MAX; addr++) {
1375
            probe_i2c_dev(DRIVER_BMM150, AP_Compass_BMM150::probe, i, addr, true, ROTATION_NONE);
1376
            RETURN_IF_NO_SPACE;
1377
        }
1378
    }
1379
#endif  // AP_COMPASS_BMM150_ENABLED
1380

1381
#if AP_COMPASS_BMM350_ENABLED
1382
    // BMM350 on I2C
1383
    FOREACH_I2C_EXTERNAL(i) {
1384
        for (uint8_t addr=BMM350_I2C_ADDR_MIN; addr <= BMM350_I2C_ADDR_MAX; addr++) {
1385
            probe_i2c_dev(DRIVER_BMM350, AP_Compass_BMM350::probe, i, addr, true, ROTATION_NONE);
1386
            RETURN_IF_NO_SPACE;
1387
        }
1388
    }
1389
#if AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1390
    FOREACH_I2C_INTERNAL(i) {
1391
        for (uint8_t addr=BMM350_I2C_ADDR_MIN; addr <= BMM350_I2C_ADDR_MAX; addr++) {
1392
            probe_i2c_dev(DRIVER_BMM350, AP_Compass_BMM350::probe, i, addr, all_external, ROTATION_NONE);
1393
            RETURN_IF_NO_SPACE;
1394
        }
1395
    }
1396
#endif  // AP_COMPASS_INTERNAL_BUS_PROBING_ENABLED
1397
#endif // AP_COMPASS_BMM350_ENABLED
1398
}
1399

1400
/*
1401
  detect available backends for this board
1402
 */
1403
void Compass::_detect_backends(void)
1404
{
1405
#if AP_COMPASS_EXTERNALAHRS_ENABLED
1406
    const int8_t serial_port = AP::externalAHRS().get_port(AP_ExternalAHRS::AvailableSensor::COMPASS);
1407
    if (serial_port >= 0) {
1408
        add_backend(DRIVER_EXTERNALAHRS, AP_Compass_ExternalAHRS::probe(serial_port));
1409
        RETURN_IF_NO_SPACE;
1410
    }
1411
#endif
1412
    
1413
#if AP_FEATURE_BOARD_DETECT
1414
    if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2) {
1415
        // default to disabling LIS3MDL on pixhawk2 due to hardware issue
1416
#if AP_COMPASS_LIS3MDL_ENABLED
1417
    _driver_type_mask.set_default(1U<<DRIVER_LIS3MDL);
1418
#endif
1419
    }
1420
#endif
1421

1422
#if AP_COMPASS_SITL_ENABLED && !AP_TEST_DRONECAN_DRIVERS
1423
    // create several SITL compass backends:
1424
    for (uint8_t i=0; i<MAX_CONNECTED_MAGS; i++) {
1425
        add_backend(DRIVER_SITL, NEW_NOTHROW AP_Compass_SITL(i));
1426
        RETURN_IF_NO_SPACE;
1427
    }
1428
#endif
1429

1430
#if AP_COMPASS_DRONECAN_ENABLED
1431
    // probe DroneCAN before I2C and SPI so that DroneCAN compasses
1432
    // default to first in the list for a new board
1433
    probe_dronecan_compasses();
1434
    RETURN_IF_NO_SPACE;
1435
#endif
1436

1437
#if AP_COMPASS_PROBING_ENABLED
1438
    // allow boards to ask for external probing of all i2c compass types in hwdef.dat
1439
    _probe_external_i2c_compasses();
1440
    RETURN_IF_NO_SPACE;
1441
#endif  // AP_COMPASS_PROBING_ENABLED
1442

1443
#if AP_COMPASS_MSP_ENABLED
1444
    for (uint8_t i=0; i<8; i++) {
1445
        if (msp_instance_mask & (1U<<i)) {
1446
            auto *backend = AP_Compass_MSP::probe(i);
1447
            if (backend == nullptr) {
1448
                continue;
1449
            }
1450
            add_backend(DRIVER_MSP, backend);
1451
            RETURN_IF_NO_SPACE;
1452
        }
1453
    }
1454
#endif
1455

1456
    // finally look for i2c and spi compasses not found yet
1457
    RETURN_IF_NO_SPACE;
1458
    probe_i2c_spi_compasses();
1459

1460
    if (_backend_count == 0 ||
1461
        _compass_count == 0) {
1462
        DEV_PRINTF("No Compass backends available\n");
1463
    }
1464
}
1465

1466
/*
1467
  probe i2c and SPI compasses
1468
 */
1469
void Compass::probe_i2c_spi_compasses(void)
1470
{
1471
#if defined(HAL_MAG_PROBE_LIST)
1472
    // driver probes defined by COMPASS lines in hwdef.dat
1473
    HAL_MAG_PROBE_LIST;
1474
#elif AP_FEATURE_BOARD_DETECT
1475
    switch (AP_BoardConfig::get_board_type()) {
1476
    case AP_BoardConfig::PX4_BOARD_PX4V1:
1477
    case AP_BoardConfig::PX4_BOARD_PIXHAWK:
1478
    case AP_BoardConfig::PX4_BOARD_PHMINI:
1479
    case AP_BoardConfig::PX4_BOARD_AUAV21:
1480
    case AP_BoardConfig::PX4_BOARD_PH2SLIM:
1481
    case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
1482
    case AP_BoardConfig::PX4_BOARD_FMUV6:
1483
    case AP_BoardConfig::PX4_BOARD_AEROFC:
1484
        _probe_external_i2c_compasses();
1485
        RETURN_IF_NO_SPACE;
1486
        break;
1487

1488
    default:
1489
        break;
1490
    }
1491
    switch (AP_BoardConfig::get_board_type()) {
1492
    case AP_BoardConfig::PX4_BOARD_PIXHAWK:
1493
#if AP_COMPASS_HMC5843_ENABLED
1494
        probe_spi_dev(DRIVER_HMC5843, AP_Compass_HMC5843::probe, HAL_COMPASS_HMC5843_NAME, false, ROTATION_PITCH_180);
1495
        RETURN_IF_NO_SPACE;
1496
#endif
1497
#if AP_COMPASS_LSM303D_ENABLED
1498
        probe_spi_dev(DRIVER_LSM303D, AP_Compass_LSM303D::probe, HAL_INS_LSM9DS0_A_NAME, ROTATION_NONE);
1499
        RETURN_IF_NO_SPACE;
1500
#endif
1501
        break;
1502

1503
    case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
1504
#if AP_COMPASS_LSM303D_ENABLED
1505
        probe_spi_dev(DRIVER_LSM303D, AP_Compass_LSM303D::probe, HAL_INS_LSM9DS0_EXT_A_NAME, ROTATION_YAW_270);
1506
        RETURN_IF_NO_SPACE;
1507
#endif
1508
#if AP_COMPASS_AK8963_ENABLED
1509
        // we run the AK8963 only on the 2nd MPU9250, which leaves the
1510
        // first MPU9250 to run without disturbance at high rate
1511
        probe_ak8963_via_mpu9250(1, ROTATION_YAW_270);
1512
        RETURN_IF_NO_SPACE;
1513
#endif
1514
#if AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED
1515
        probe_ak09916_via_icm20948(0, ROTATION_ROLL_180_YAW_90);
1516
        RETURN_IF_NO_SPACE;
1517
#endif
1518
        break;
1519

1520
    case AP_BoardConfig::PX4_BOARD_FMUV6:
1521
#if AP_COMPASS_IST8310_ENABLED
1522
        FOREACH_I2C_EXTERNAL(i) {
1523
            probe_i2c_dev(DRIVER_IST8310, AP_Compass_IST8310::probe, i, HAL_COMPASS_IST8310_I2C_ADDR, true, ROTATION_ROLL_180_YAW_90);
1524
            RETURN_IF_NO_SPACE;
1525
        }
1526
        FOREACH_I2C_INTERNAL(i) {
1527
            probe_i2c_dev(DRIVER_IST8310, AP_Compass_IST8310::probe, i, HAL_COMPASS_IST8310_I2C_ADDR, false, ROTATION_ROLL_180_YAW_90);
1528
            RETURN_IF_NO_SPACE;
1529
        }
1530
#endif  // AP_COMPASS_IST8310_ENABLED
1531
        break;
1532

1533
    case AP_BoardConfig::PX4_BOARD_PHMINI:
1534
#if AP_COMPASS_AK8963_ENABLED
1535
        probe_ak8963_via_mpu9250(0, ROTATION_ROLL_180);
1536
        RETURN_IF_NO_SPACE;
1537
#endif
1538
        break;
1539

1540
    case AP_BoardConfig::PX4_BOARD_AUAV21:
1541
#if AP_COMPASS_AK8963_ENABLED
1542
        probe_ak8963_via_mpu9250(0, ROTATION_ROLL_180_YAW_90);
1543
        RETURN_IF_NO_SPACE;
1544
#endif
1545
        break;
1546

1547
    case AP_BoardConfig::PX4_BOARD_PH2SLIM:
1548
#if AP_COMPASS_AK8963_ENABLED
1549
        probe_ak8963_via_mpu9250(0, ROTATION_YAW_270);
1550
        RETURN_IF_NO_SPACE;
1551
#endif
1552
        break;
1553

1554
    default:
1555
        break;
1556
    }
1557
#endif
1558
}
1559

1560
#if AP_COMPASS_AK8963_ENABLED
1561
void Compass::probe_ak8963_via_mpu9250(uint8_t imu_instance, Rotation rotation)
1562
{
1563
    if (!_driver_enabled(DRIVER_AK8963)) {
1564
        return;
1565
    }
1566
    auto *backend = AP_Compass_AK8963::probe_mpu9250(imu_instance, rotation);
1567
    add_backend(DRIVER_AK8963, backend);  // add_backend does nullptr check
1568
}
1569
#endif  // AP_COMPASS_AK8963_ENABLED
1570

1571
#if AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED
1572
void Compass::probe_ak09916_via_icm20948(uint8_t i2c_bus, uint8_t ak09916_addr, uint8_t icm20948_addr, bool external, Rotation rotation)
1573
{
1574
    if (!_driver_enabled(DRIVER_ICM20948)) {
1575
        return;
1576
    }
1577
    auto *backend = AP_Compass_AK09916::probe_ICM20948(
1578
        GET_I2C_DEVICE(i2c_bus, ak09916_addr),
1579
        GET_I2C_DEVICE(i2c_bus, icm20948_addr),
1580
        external,
1581
        rotation
1582
        );
1583

1584
    add_backend(DRIVER_ICM20948, backend);  // add_backend does nullptr check
1585
}
1586

1587
void Compass::probe_ak09916_via_icm20948(uint8_t ins_instance, Rotation rotation)
1588
{
1589
    if (!_driver_enabled(DRIVER_AK09916)) {
1590
        return;
1591
    }
1592

1593
    auto *backend = AP_Compass_AK09916::probe_ICM20948(ins_instance, rotation);
1594

1595
    add_backend(DRIVER_AK09916, backend);
1596
}
1597

1598
#endif  // #if AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED
1599

1600
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)
1601
{
1602
    if (!_driver_enabled(driver_type)) {
1603
        return;
1604
    }
1605
    auto *backend = probefn(
1606
        GET_I2C_DEVICE(i2c_bus, i2c_addr),
1607
        external,
1608
        rotation
1609
        );
1610

1611
    add_backend(driver_type, backend);  // add_backend does nullptr check
1612
}
1613

1614
void Compass::probe_spi_dev(DriverType driver_type, probe_spi_dev_probefn_t probefn, const char *name, bool external, Rotation rotation)
1615
{
1616
    if (!_driver_enabled(driver_type)) {
1617
        return;
1618
    }
1619
    auto *backend = probefn(hal.spi->get_device(name), external, rotation);
1620

1621
    add_backend(driver_type, backend);  // add_backend does nullptr check
1622
}
1623

1624
// short-lived method which expectes a probe function that doesn't
1625
// offer the ability to specify an external rotation:
1626
void Compass::probe_spi_dev(DriverType driver_type, probe_spi_dev_noexternal_probefn_t probefn, const char *name, Rotation rotation)
1627
{
1628
    if (!_driver_enabled(driver_type)) {
1629
        return;
1630
    }
1631
    auto *backend = probefn(hal.spi->get_device(name), rotation);
1632

1633
    add_backend(driver_type, backend);  // add_backend does nullptr check
1634
}
1635

1636
#if AP_COMPASS_DRONECAN_ENABLED
1637
/*
1638
  look for DroneCAN compasses
1639
 */
1640
void Compass::probe_dronecan_compasses(void)
1641
{
1642
    if (!_driver_enabled(DRIVER_UAVCAN)) {
1643
        return;
1644
    }
1645
    for (uint8_t i=0; i<MAX_CONNECTED_MAGS; i++) {
1646
        AP_Compass_Backend* _uavcan_backend = AP_Compass_DroneCAN::probe(i);
1647
        if (_uavcan_backend) {
1648
            add_backend(DRIVER_UAVCAN, _uavcan_backend);
1649
        }
1650
#if COMPASS_MAX_UNREG_DEV > 0
1651
        if (_unreg_compass_count == COMPASS_MAX_UNREG_DEV)  {
1652
            break;
1653
        }
1654
#endif
1655
    }
1656

1657
#if COMPASS_MAX_UNREG_DEV > 0
1658
    if (option_set(Option::ALLOW_DRONECAN_AUTO_REPLACEMENT) && !suppress_devid_save) {
1659
        // check if there's any uavcan compass in prio slot that's not found
1660
        // and replace it if there's a replacement compass
1661
        for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
1662
            if (AP_HAL::Device::devid_get_bus_type(_priority_did_list[i]) != AP_HAL::Device::BUS_TYPE_UAVCAN
1663
                || _get_state(i).registered) {
1664
                continue;
1665
            }
1666
            // There's a UAVCAN compass missing
1667
            // Let's check if there's a replacement
1668
            for (uint8_t j=0; j<COMPASS_MAX_INSTANCES; j++) {
1669
                uint32_t detected_devid = AP_Compass_DroneCAN::get_detected_devid(j);
1670
                // Check if this is a potential replacement mag
1671
                if (!is_replacement_mag(detected_devid)) {
1672
                    continue;
1673
                }
1674
                // We have found a replacement mag, let's replace the existing one
1675
                // with this by setting the priority to zero and calling uavcan probe
1676
                GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "Mag: Compass #%d with DEVID %lu replaced", uint8_t(i), (unsigned long)_priority_did_list[i]);
1677
                _priority_did_stored_list[i].set_and_save(0);
1678
                _priority_did_list[i] = 0;
1679

1680
                AP_Compass_Backend* _uavcan_backend = AP_Compass_DroneCAN::probe(j);
1681
                if (_uavcan_backend) {
1682
                    add_backend(DRIVER_UAVCAN, _uavcan_backend);
1683
                    // we also need to remove the id from unreg list
1684
                    remove_unreg_dev_id(detected_devid);
1685
                } else {
1686
                    // the mag has already been allocated,
1687
                    // let's begin the replacement
1688
                    bool found_replacement = false;
1689
                    for (StateIndex k(0); k<COMPASS_MAX_INSTANCES; k++) {
1690
                        if ((uint32_t)_state[k].dev_id == detected_devid) {
1691
                            if (_state[k].priority <= uint8_t(i)) {
1692
                                // we are already on higher priority
1693
                                // nothing to do
1694
                                break;
1695
                            }
1696
                            found_replacement = true;
1697
                            // reset old priority of replacement mag
1698
                            _priority_did_stored_list[_state[k].priority].set_and_save(0);
1699
                            _priority_did_list[_state[k].priority] = 0;
1700
                            // update new priority
1701
                            _state[k].priority = i;
1702
                        }
1703
                    }
1704
                    if (!found_replacement) {
1705
                        continue;
1706
                    }
1707
                    _priority_did_stored_list[i].set_and_save(detected_devid);
1708
                    _priority_did_list[i] = detected_devid;
1709
                }
1710
            }
1711
        }
1712
    }
1713
#endif  // #if COMPASS_MAX_UNREG_DEV > 0
1714
}
1715
#endif  // AP_COMPASS_DRONECAN_ENABLED
1716

1717

1718
// Check if the devid is a potential replacement compass
1719
// Following are the checks done to ensure the compass is a replacement
1720
// * The compass is an UAVCAN compass
1721
// * The compass wasn't seen before this boot as additional unreg mag
1722
// * The compass might have been seen before but never setup
1723
bool Compass::is_replacement_mag(uint32_t devid) {
1724
#if COMPASS_MAX_INSTANCES > 1
1725
    // We only do this for UAVCAN mag
1726
    if (devid == 0 || (AP_HAL::Device::devid_get_bus_type(devid) != AP_HAL::Device::BUS_TYPE_UAVCAN)) {
1727
        return false;
1728
    }
1729

1730
#if COMPASS_MAX_UNREG_DEV > 0
1731
    // Check that its not an unused additional mag
1732
    for (uint8_t i = 0; i<COMPASS_MAX_UNREG_DEV; i++) {
1733
        if (_previously_unreg_mag[i] == devid) {
1734
            return false;
1735
        }
1736
    }
1737
#endif
1738

1739
    // Check that its not previously setup mag
1740
    for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
1741
        if ((uint32_t)_state[i].expected_dev_id == devid) {
1742
            return false;
1743
        }
1744
    }
1745
#endif
1746
    return true;
1747
}
1748

1749
void Compass::remove_unreg_dev_id(uint32_t devid)
1750
{
1751
#if COMPASS_MAX_INSTANCES > 1
1752
    // We only do this for UAVCAN mag
1753
    if (devid == 0 || (AP_HAL::Device::devid_get_bus_type(devid) != AP_HAL::Device::BUS_TYPE_UAVCAN)) {
1754
        return;
1755
    }
1756

1757
#if COMPASS_MAX_UNREG_DEV > 0
1758
    for (uint8_t i = 0; i<COMPASS_MAX_UNREG_DEV; i++) {
1759
        if ((uint32_t)extra_dev_id[i] == devid) {
1760
            extra_dev_id[i].set(0);
1761
            return;
1762
        }
1763
    }
1764
#endif
1765
#endif
1766
}
1767

1768
void Compass::_reset_compass_id()
1769
{
1770
#if COMPASS_MAX_INSTANCES > 1
1771
    // Check if any of the registered devs are not registered
1772
    for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
1773
        if (_priority_did_stored_list[i] != _priority_did_list[i] ||
1774
            _priority_did_stored_list[i] == 0) {
1775
            //We don't touch priorities that might have been touched by the user
1776
            continue;
1777
        }
1778
        if (!_get_state(i).registered) {
1779
            _priority_did_stored_list[i].set_and_save(0);
1780
            GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "Mag: Compass #%d with DEVID %lu removed", uint8_t(i), (unsigned long)_priority_did_list[i]);
1781
        }
1782
    }
1783

1784
    // Check if any of the old registered devs are not registered
1785
    // and hence can be removed
1786
    for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
1787
        if (_state[i].dev_id == 0 && _state[i].expected_dev_id != 0) {
1788
            // also hard reset dev_ids that are not detected
1789
            _state[i].dev_id.save();
1790
        }
1791
    }
1792
#endif
1793
}
1794

1795
// Look for devices beyond initialisation
1796
void
1797
Compass::_detect_runtime(void)
1798
{
1799
#if AP_COMPASS_DRONECAN_ENABLED
1800
    if (!available()) {
1801
        return;
1802
    }
1803

1804
    //Don't try to add device while armed
1805
    if (hal.util->get_soft_armed()) {
1806
        return;
1807
    }
1808
    static uint32_t last_try;
1809
    //Try once every second
1810
    if ((AP_HAL::millis() - last_try) < 1000) {
1811
        return;
1812
    }
1813
    last_try = AP_HAL::millis();
1814
    if (_driver_enabled(DRIVER_UAVCAN)) {
1815
        for (uint8_t i=0; i<COMPASS_MAX_BACKEND; i++) {
1816
            AP_Compass_Backend* _uavcan_backend = AP_Compass_DroneCAN::probe(i);
1817
            if (_uavcan_backend) {
1818
                add_backend(DRIVER_UAVCAN, _uavcan_backend);
1819
            }
1820
            RETURN_IF_NO_SPACE;
1821
        }
1822
    }
1823
#endif  // AP_COMPASS_DRONECAN_ENABLED
1824
}
1825

1826
bool
1827
Compass::read(void)
1828
{
1829
    if (!available()) {
1830
        return false;
1831
    }
1832

1833
#if HAL_LOGGING_ENABLED
1834
    const bool old_healthy = healthy();
1835
#endif
1836

1837
#ifndef HAL_BUILD_AP_PERIPH
1838
    if (!_initial_location_set) {
1839
        try_set_initial_location();
1840
    }
1841
#endif
1842

1843
    _detect_runtime();
1844

1845
    for (uint8_t i=0; i< _backend_count; i++) {
1846
        // call read on each of the backend. This call updates field[i]
1847
        _backends[i]->read();
1848
    }
1849
    uint32_t time = AP_HAL::millis();
1850
    bool any_healthy = false;
1851
    for (StateIndex i(0); i < COMPASS_MAX_INSTANCES; i++) {
1852
        _state[i].healthy = (time - _state[i].last_update_ms < 500);
1853
        any_healthy |= _state[i].healthy;
1854
    }
1855
#if COMPASS_LEARN_ENABLED
1856
    if (_learn == LearnType::INFLIGHT && !learn_allocated) {
1857
        learn_allocated = true;
1858
        learn = NEW_NOTHROW CompassLearn(*this);
1859
    }
1860
    if (_learn == LearnType::INFLIGHT && learn != nullptr) {
1861
        learn->update();
1862
    }
1863
#endif
1864
#if HAL_LOGGING_ENABLED
1865
    if (any_healthy && _log_bit != (uint32_t)-1 && AP::logger().should_log(_log_bit)) {
1866
        AP::logger().Write_Compass();
1867
    }
1868
#endif
1869

1870
    // Set _first_usable parameter
1871
    for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
1872
        if (_use_for_yaw[i]) {
1873
            _first_usable = uint8_t(i);
1874
            break;
1875
        }
1876
    }
1877
    const bool new_healthy = healthy();
1878

1879
#if HAL_LOGGING_ENABLED
1880

1881
    #define MASK_LOG_ANY                    0xFFFF
1882

1883
    if (new_healthy != old_healthy) {
1884
        if (AP::logger().should_log(MASK_LOG_ANY)) {
1885
            const LogErrorCode code = new_healthy ? LogErrorCode::ERROR_RESOLVED : LogErrorCode::UNHEALTHY;
1886
            AP::logger().Write_Error(LogErrorSubsystem::COMPASS, code);
1887
        }
1888
    }
1889
#endif
1890

1891
    return new_healthy;
1892
}
1893

1894
uint8_t
1895
Compass::get_healthy_mask() const
1896
{
1897
    uint8_t healthy_mask = 0;
1898
    for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
1899
        if (healthy(uint8_t(i))) {
1900
            healthy_mask |= 1 << uint8_t(i);
1901
        }
1902
    }
1903
    return healthy_mask;
1904
}
1905

1906
void
1907
Compass::set_offsets(uint8_t i, const Vector3f &offsets)
1908
{
1909
    // sanity check compass instance provided
1910
    StateIndex id = _get_state_id(Priority(i));
1911
    if (id < COMPASS_MAX_INSTANCES) {
1912
        _state[id].offset.set(offsets);
1913
    }
1914
}
1915

1916
void
1917
Compass::set_and_save_offsets(uint8_t i, const Vector3f &offsets)
1918
{
1919
    // sanity check compass instance provided
1920
    StateIndex id = _get_state_id(Priority(i));
1921
    if (id < COMPASS_MAX_INSTANCES) {
1922
        _state[id].offset.set(offsets);
1923
        save_offsets(i);
1924
    }
1925
}
1926

1927
#if AP_COMPASS_DIAGONALS_ENABLED
1928
void
1929
Compass::set_and_save_diagonals(uint8_t i, const Vector3f &diagonals)
1930
{
1931
    // sanity check compass instance provided
1932
    StateIndex id = _get_state_id(Priority(i));
1933
    if (id < COMPASS_MAX_INSTANCES) {
1934
        _state[id].diagonals.set_and_save(diagonals);
1935
    }
1936
}
1937

1938
void
1939
Compass::set_and_save_offdiagonals(uint8_t i, const Vector3f &offdiagonals)
1940
{
1941
    // sanity check compass instance provided
1942
    StateIndex id = _get_state_id(Priority(i));
1943
    if (id < COMPASS_MAX_INSTANCES) {
1944
        _state[id].offdiagonals.set_and_save(offdiagonals);
1945
    }
1946
}
1947
#endif // AP_COMPASS_DIAGONALS_ENABLED
1948

1949
void
1950
Compass::set_and_save_scale_factor(uint8_t i, float scale_factor)
1951
{
1952
    StateIndex id = _get_state_id(Priority(i));
1953
    if (i < COMPASS_MAX_INSTANCES) {
1954
        _state[id].scale_factor.set_and_save(scale_factor);
1955
    }
1956
}
1957

1958
void
1959
Compass::save_offsets(uint8_t i)
1960
{
1961
    StateIndex id = _get_state_id(Priority(i));
1962
    if (id < COMPASS_MAX_INSTANCES) {
1963
        _state[id].offset.save();  // save offsets
1964
        _state[id].dev_id.set_and_save(_state[id].detected_dev_id);
1965
    }
1966
}
1967

1968
void
1969
Compass::set_and_save_orientation(uint8_t i, Rotation orientation)
1970
{
1971
    StateIndex id = _get_state_id(Priority(i));
1972
    if (id < COMPASS_MAX_INSTANCES) {
1973
        _state[id].orientation.set_and_save_ifchanged(orientation);
1974
    }
1975
}
1976

1977
void
1978
Compass::save_offsets(void)
1979
{
1980
    for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
1981
        save_offsets(uint8_t(i));
1982
    }
1983
}
1984

1985
void
1986
Compass::set_motor_compensation(uint8_t i, const Vector3f &motor_comp_factor)
1987
{
1988
    StateIndex id = _get_state_id(Priority(i));
1989
    if (id < COMPASS_MAX_INSTANCES) {
1990
        _state[id].motor_compensation.set(motor_comp_factor);
1991
    }
1992
}
1993

1994
void
1995
Compass::save_motor_compensation()
1996
{
1997
    StateIndex id;
1998
    _motor_comp_type.save();
1999
    for (Priority k(0); k<COMPASS_MAX_INSTANCES; k++) {
2000
        id = _get_state_id(k);
2001
        if (id<COMPASS_MAX_INSTANCES) {
2002
            _state[id].motor_compensation.save();
2003
        }
2004
    }
2005
}
2006

2007
void Compass::try_set_initial_location()
2008
{
2009
    if (!_auto_declination) {
2010
        return;
2011
    }
2012
    if (!_enabled) {
2013
        return;
2014
    }
2015

2016
    Location loc;
2017
    if (!AP::ahrs().get_location(loc)) {
2018
        return;
2019
    }
2020
    _initial_location_set = true;
2021

2022
    // if automatic declination is configured, then compute
2023
    // the declination based on the initial GPS fix
2024
    // Set the declination based on the lat/lng from GPS
2025
    _declination.set(radians(
2026
                         AP_Declination::get_declination(
2027
                             (float)loc.lat / 10000000,
2028
                             (float)loc.lng / 10000000)));
2029
}
2030

2031
/// return true if the compass should be used for yaw calculations
2032
bool
2033
Compass::use_for_yaw(void) const
2034
{
2035
    return healthy(_first_usable) && use_for_yaw(_first_usable);
2036
}
2037

2038
/// return true if the specified compass can be used for yaw calculations
2039
bool
2040
Compass::use_for_yaw(uint8_t i) const
2041
{
2042
    if (!available()) {
2043
        return false;
2044
    }
2045
    // when we are doing in-flight compass learning the state
2046
    // estimator must not use the compass. The learning code turns off
2047
    // inflight learning when it has converged
2048
    return _use_for_yaw[Priority(i)] && _learn != LearnType::INFLIGHT;
2049
}
2050

2051
/*
2052
  return the number of enabled sensors. Used to determine if
2053
  non-compass operation is desired
2054
 */
2055
uint8_t Compass::get_num_enabled(void) const
2056
{
2057
    if (get_count() == 0) {
2058
        return 0;
2059
    }
2060
    uint8_t count = 0;
2061
    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
2062
        if (use_for_yaw(i)) {
2063
            count++;
2064
        }
2065
    }
2066
    return count;
2067
}
2068

2069
void
2070
Compass::set_declination(float radians, bool save_to_eeprom)
2071
{
2072
    if (save_to_eeprom) {
2073
        _declination.set_and_save(radians);
2074
    } else {
2075
        _declination.set(radians);
2076
    }
2077
}
2078

2079
float
2080
Compass::get_declination() const
2081
{
2082
    return _declination.get();
2083
}
2084

2085
/*
2086
  calculate a compass heading given the attitude from DCM and the mag vector
2087
 */
2088
float
2089
Compass::calculate_heading(const Matrix3f &dcm_matrix, uint8_t i) const
2090
{
2091
/*
2092
    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.
2093
    One could do:
2094
        float roll, pitch, yaw;
2095
        dcm_matrix.to_euler(roll, pitch, yaw)
2096
        Matrix3f rp_rot;
2097
        rp_rot.from_euler(roll, pitch, 0)
2098
        Vector3f ef = rp_rot * field
2099

2100
    Because only the X and Y components are needed it's more efficient to manually calculate:
2101

2102
        rp_rot = [ cos(pitch), sin(roll) * sin(pitch),  cos(roll) * sin(pitch)
2103
                            0,              cos(roll),              -sin(roll)]
2104

2105
    If the whole matrix is multiplied by cos(pitch) the required trigonometric values can be extracted directly from the existing dcm matrix.
2106
    This multiplication has no effect on the calculated heading as it changes the length of the North/East vector but not its angle.
2107

2108
        rp_rot = [ cos(pitch)^2, sin(roll) * sin(pitch) * cos(pitch),  cos(roll) * sin(pitch) * cos(pitch)
2109
                              0,              cos(roll) * cos(pitch),              -sin(roll) * cos(pitch)]
2110

2111
    Preexisting values can be substituted in:
2112

2113
        dcm_matrix.c.x = -sin(pitch)
2114
        dcm_matrix.c.y =  sin(roll) * cos(pitch)
2115
        dcm_matrix.c.z =  cos(roll) * cos(pitch)
2116

2117
        rp_rot = [ cos(pitch)^2, dcm_matrix.c.y * -dcm_matrix.c.x,  dcm_matrix.c.z * -dcm_matrix.c.x
2118
                              0,                   dcm_matrix.c.z,                   -dcm_matrix.c.y]
2119

2120
    cos(pitch)^2 is stil needed. This is the same as 1 - sin(pitch)^2.
2121
    sin(pitch) is avalable as dcm_matrix.c.x
2122
*/
2123

2124
    const float cos_pitch_sq = 1.0f-(dcm_matrix.c.x*dcm_matrix.c.x);
2125

2126
    // Tilt compensated magnetic field Y component:
2127
    const Vector3f &field = get_field(i);
2128

2129
    const float headY = field.y * dcm_matrix.c.z - field.z * dcm_matrix.c.y;
2130

2131
    // Tilt compensated magnetic field X component:
2132
    const float headX = field.x * cos_pitch_sq - dcm_matrix.c.x * (field.y * dcm_matrix.c.y + field.z * dcm_matrix.c.z);
2133

2134
    // magnetic heading
2135
    // 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.
2136
    const float heading = constrain_float(atan2f(-headY,headX), -M_PI, M_PI);
2137

2138
    // Declination correction
2139
    return wrap_PI(heading + _declination);
2140
}
2141

2142
/// Returns True if the compasses have been configured (i.e. offsets saved)
2143
///
2144
/// @returns                    True if compass has been configured
2145
///
2146
bool Compass::configured(uint8_t i)
2147
{
2148
    // exit immediately if instance is beyond the number of compasses we have available
2149
    if (i > get_count()) {
2150
        return false;
2151
    }
2152

2153
    // exit immediately if all offsets are zero
2154
    if (is_zero(get_offsets(i).length())) {
2155
        return false;
2156
    }
2157

2158
    StateIndex id = _get_state_id(Priority(i));
2159
    // exit immediately if dev_id hasn't been detected
2160
    if (_state[id].detected_dev_id == 0 || 
2161
        id == COMPASS_MAX_INSTANCES) {
2162
        return false;
2163
    }
2164

2165
#ifdef HAL_USE_EMPTY_STORAGE
2166
    // the load() call below returns zeroes on empty storage, so the
2167
    // check-stored-value check here will always fail.  Since nobody
2168
    // really cares about the empty-storage case, shortcut out here
2169
    // for simplicity.
2170
    return true;
2171
#endif
2172

2173
    // back up cached value of dev_id
2174
    int32_t dev_id_cache_value = _state[id].dev_id;
2175

2176
    // load dev_id from eeprom
2177
    _state[id].dev_id.load();
2178

2179
    // 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
2180
    if (_state[id].dev_id != _state[id].detected_dev_id || _state[id].dev_id != dev_id_cache_value) {
2181
        // restore cached value
2182
        _state[id].dev_id.set(dev_id_cache_value);
2183
        // return failure
2184
        return false;
2185
    }
2186

2187
    // if we got here then it must be configured
2188
    return true;
2189
}
2190

2191
bool Compass::configured(char *failure_msg, uint8_t failure_msg_len)
2192
{
2193
#if COMPASS_MAX_INSTANCES > 1
2194
    // Check if any of the registered devs are not registered
2195
    for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
2196
        if (_priority_did_list[i] != 0 && use_for_yaw(uint8_t(i))) {
2197
            if (!_get_state(i).registered) {
2198
                snprintf(failure_msg, failure_msg_len, "Compass %d not found", uint8_t(i + 1));
2199
                return false;
2200
            }
2201
            if (_priority_did_list[i] != _priority_did_stored_list[i]) {
2202
                snprintf(failure_msg, failure_msg_len, "Compass order change requires reboot");
2203
                return false;
2204
            }
2205
        }
2206
    }
2207
#endif
2208

2209
    bool all_configured = true;
2210
    for (uint8_t i=0; i<get_count(); i++) {
2211
        if (configured(i)) {
2212
            continue;
2213
        }
2214
        if (!use_for_yaw(i)) {
2215
            // we're not planning on using this anyway so sure,
2216
            // whatever, it's configured....
2217
            continue;
2218
        }
2219
        all_configured = false;
2220
        break;
2221
    }
2222
    if (!all_configured) {
2223
        snprintf(failure_msg, failure_msg_len, "Compass not calibrated");
2224
    }
2225
    return all_configured;
2226
}
2227

2228
/*
2229
  set the type of motor compensation to use
2230
 */
2231
void Compass::motor_compensation_type(const uint8_t comp_type)
2232
{
2233
    if (_motor_comp_type <= AP_COMPASS_MOT_COMP_CURRENT && _motor_comp_type != (int8_t)comp_type) {
2234
        _motor_comp_type.set((int8_t)comp_type);
2235
        _thr = 0; // set current  throttle to zero
2236
        for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
2237
            set_motor_compensation(i, Vector3f(0,0,0)); // clear out invalid compensation vectors
2238
        }
2239
    }
2240
}
2241

2242
bool Compass::consistent() const
2243
{
2244
    const Vector3f &primary_mag_field { get_field() };
2245
    const Vector2f &primary_mag_field_xy { primary_mag_field.xy() };
2246

2247
    for (uint8_t i=0; i<get_count(); i++) {
2248
        if (!use_for_yaw(i)) {
2249
            // configured not-to-be-used
2250
            continue;
2251
        }
2252

2253
        const Vector3f &mag_field = get_field(i);
2254
        const Vector2f &mag_field_xy = mag_field.xy();
2255

2256
        if (mag_field_xy.is_zero()) {
2257
            return false;
2258
        }
2259

2260
        // check for gross misalignment on all axes
2261
        const float xyz_ang_diff  = mag_field.angle(primary_mag_field);
2262
        if (xyz_ang_diff > AP_COMPASS_MAX_XYZ_ANG_DIFF) {
2263
            return false;
2264
        }
2265

2266
        // check for an unacceptable angle difference on the xy plane
2267
        const float xy_ang_diff  = mag_field_xy.angle(primary_mag_field_xy);
2268
        if (xy_ang_diff > AP_COMPASS_MAX_XY_ANG_DIFF) {
2269
            return false;
2270
        }
2271

2272
        // check for an unacceptable length difference on the xy plane
2273
        const float xy_len_diff = (primary_mag_field_xy-mag_field_xy).length();
2274
        if (xy_len_diff > AP_COMPASS_MAX_XY_LENGTH_DIFF) {
2275
            return false;
2276
        }
2277
    }
2278
    return true;
2279
}
2280

2281
bool Compass::healthy(uint8_t i) const
2282
{
2283
    return (i < COMPASS_MAX_INSTANCES) ? _get_state(Priority(i)).healthy : false;
2284
}
2285

2286
/*
2287
  return true if we have a valid scale factor
2288
 */
2289
bool Compass::have_scale_factor(uint8_t i) const
2290
{
2291
    if (!available()) {
2292
        return false;
2293
    }
2294
    StateIndex id = _get_state_id(Priority(i));
2295
    if (id >= COMPASS_MAX_INSTANCES ||
2296
        _state[id].scale_factor < COMPASS_MIN_SCALE_FACTOR ||
2297
        _state[id].scale_factor > COMPASS_MAX_SCALE_FACTOR) {
2298
        return false;
2299
    }
2300
    return true;
2301
}
2302

2303
#if AP_COMPASS_MSP_ENABLED
2304
void Compass::handle_msp(const MSP::msp_compass_data_message_t &pkt)
2305
{
2306
    if (!_driver_enabled(DRIVER_MSP)) {
2307
        return;
2308
    }
2309
    if (!init_done) {
2310
        if (pkt.instance < 8) {
2311
            msp_instance_mask |= 1U<<pkt.instance;
2312
        }
2313
    } else {
2314
        for (uint8_t i=0; i<_backend_count; i++) {
2315
            _backends[i]->handle_msp(pkt);
2316
        }
2317
    }
2318
}
2319
#endif // AP_COMPASS_MSP_ENABLED
2320

2321
#if AP_COMPASS_EXTERNALAHRS_ENABLED
2322
void Compass::handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt)
2323
{
2324
    if (!_driver_enabled(DRIVER_EXTERNALAHRS)) {
2325
        return;
2326
    }
2327
    for (uint8_t i=0; i<_backend_count; i++) {
2328
        _backends[i]->handle_external(pkt);
2329
    }
2330
}
2331
#endif // AP_COMPASS_EXTERNALAHRS_ENABLED
2332

2333
// force save of current calibration as valid
2334
void Compass::force_save_calibration(void)
2335
{
2336
    for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) {
2337
        if (_state[i].dev_id != 0) {
2338
            _state[i].dev_id.save();
2339
        }
2340
    }
2341
}
2342

2343
// singleton instance
2344
Compass *Compass::_singleton;
2345

2346
namespace AP
2347
{
2348

2349
Compass &compass()
2350
{
2351
    return *Compass::get_singleton();
2352
}
2353

2354
}
2355

2356
#endif  // AP_COMPASS_ENABLED
2357

2358
Product

Resources

Company
