Line data Source code
1 : #pragma once
2 :
3 : #include "AP_Compass_config.h"
4 :
5 : #include <inttypes.h>
6 :
7 : #include <AP_Common/AP_Common.h>
8 : #include <AP_Declination/AP_Declination.h>
9 : #include <AP_HAL/AP_HAL.h>
10 : #include <AP_Math/AP_Math.h>
11 : #include <AP_Param/AP_Param.h>
12 : #include <GCS_MAVLink/GCS_MAVLink.h>
13 : #include <AP_MSP/msp.h>
14 : #include <AP_ExternalAHRS/AP_ExternalAHRS.h>
15 :
16 : #include "AP_Compass_Backend.h"
17 : #include "Compass_PerMotor.h"
18 : #include <AP_Common/TSIndex.h>
19 :
20 : // motor compensation types (for use with motor_comp_enabled)
21 : #define AP_COMPASS_MOT_COMP_DISABLED 0x00
22 : #define AP_COMPASS_MOT_COMP_THROTTLE 0x01
23 : #define AP_COMPASS_MOT_COMP_CURRENT 0x02
24 : #define AP_COMPASS_MOT_COMP_PER_MOTOR 0x03
25 :
26 : #ifndef MAG_BOARD_ORIENTATION
27 : #define MAG_BOARD_ORIENTATION ROTATION_NONE
28 : #endif
29 :
30 : #ifndef COMPASS_MOT_ENABLED
31 : #define COMPASS_MOT_ENABLED 1
32 : #endif
33 :
34 : // define default compass calibration fitness and consistency checks
35 : #define AP_COMPASS_CALIBRATION_FITNESS_DEFAULT 16.0f
36 : #define AP_COMPASS_MAX_XYZ_ANG_DIFF radians(90.0f)
37 : #define AP_COMPASS_MAX_XY_ANG_DIFF radians(60.0f)
38 : #define AP_COMPASS_MAX_XY_LENGTH_DIFF 200.0f
39 :
40 : /**
41 : maximum number of compass instances available on this platform. If more
42 : than 1 then redundant sensors may be available
43 : */
44 : #ifndef HAL_BUILD_AP_PERIPH
45 : #ifndef HAL_COMPASS_MAX_SENSORS
46 : #define HAL_COMPASS_MAX_SENSORS 3
47 : #endif
48 : #if HAL_COMPASS_MAX_SENSORS > 1
49 : #define COMPASS_MAX_UNREG_DEV 5
50 : #else
51 : #define COMPASS_MAX_UNREG_DEV 0
52 : #endif
53 : #else
54 : #ifndef HAL_COMPASS_MAX_SENSORS
55 : #define HAL_COMPASS_MAX_SENSORS 1
56 : #endif
57 : #define COMPASS_MAX_UNREG_DEV 0
58 : #endif
59 :
60 : #define COMPASS_MAX_INSTANCES HAL_COMPASS_MAX_SENSORS
61 : #define COMPASS_MAX_BACKEND HAL_COMPASS_MAX_SENSORS
62 :
63 : #define MAX_CONNECTED_MAGS (COMPASS_MAX_UNREG_DEV+COMPASS_MAX_INSTANCES)
64 :
65 : #include "CompassCalibrator.h"
66 :
67 : class CompassLearn;
68 :
69 : class Compass
70 : {
71 : friend class AP_Compass_Backend;
72 : friend class AP_Compass_DroneCAN;
73 : public:
74 : Compass();
75 :
76 : /* Do not allow copies */
77 : CLASS_NO_COPY(Compass);
78 :
79 : // get singleton instance
80 189603252 : static Compass *get_singleton() {
81 189603252 : return _singleton;
82 : }
83 :
84 : friend class CompassLearn;
85 :
86 : /// Initialize the compass device.
87 : ///
88 : /// @returns True if the compass was initialized OK, false if it was not
89 : /// found or is not functioning.
90 : ///
91 : __INITFUNC__ void init();
92 :
93 : /// Read the compass and update the mag_ variables.
94 : ///
95 : bool read();
96 :
97 : // available returns true if the compass is both enabled and has
98 : // been initialised
99 638204228 : bool available() const { return _enabled && init_done; }
100 :
101 : /// Calculate the tilt-compensated heading_ variables.
102 : ///
103 : /// @param dcm_matrix The current orientation rotation matrix
104 : ///
105 : /// @returns heading in radians
106 : ///
107 2082 : float calculate_heading(const Matrix3f &dcm_matrix) const {
108 2082 : return calculate_heading(dcm_matrix, _first_usable);
109 : }
110 : float calculate_heading(const Matrix3f &dcm_matrix, uint8_t i) const;
111 :
112 : /// Sets offset x/y/z values.
113 : ///
114 : /// @param i compass instance
115 : /// @param offsets Offsets to the raw mag_ values in milligauss.
116 : ///
117 : void set_offsets(uint8_t i, const Vector3f &offsets);
118 :
119 : /// Sets and saves the compass offset x/y/z values.
120 : ///
121 : /// @param i compass instance
122 : /// @param offsets Offsets to the raw mag_ values in milligauss.
123 : ///
124 : void set_and_save_offsets(uint8_t i, const Vector3f &offsets);
125 : #if AP_COMPASS_DIAGONALS_ENABLED
126 : void set_and_save_diagonals(uint8_t i, const Vector3f &diagonals);
127 : void set_and_save_offdiagonals(uint8_t i, const Vector3f &diagonals);
128 : #endif
129 : void set_and_save_scale_factor(uint8_t i, float scale_factor);
130 : void set_and_save_orientation(uint8_t i, Rotation orientation);
131 :
132 : /// Saves the current offset x/y/z values for one or all compasses
133 : ///
134 : /// @param i compass instance
135 : ///
136 : void save_offsets(uint8_t i);
137 : void save_offsets(void);
138 :
139 : // return the number of compass instances
140 254660622 : uint8_t get_count(void) const { return _compass_count; }
141 :
142 : // return the number of enabled sensors
143 : uint8_t get_num_enabled(void) const;
144 :
145 : /// Return the current field as a Vector3f in milligauss
146 295898359 : const Vector3f &get_field(uint8_t i) const { return _get_state(Priority(i)).field; }
147 42966058 : const Vector3f &get_field(void) const { return get_field(_first_usable); }
148 :
149 : /// Return true if we have set a scale factor for a compass
150 : bool have_scale_factor(uint8_t i) const;
151 :
152 : #if COMPASS_MOT_ENABLED
153 : // per-motor calibration access
154 0 : void per_motor_calibration_start(void) {
155 0 : _per_motor.calibration_start();
156 0 : }
157 0 : void per_motor_calibration_update(void) {
158 0 : _per_motor.calibration_update();
159 0 : }
160 0 : void per_motor_calibration_end(void) {
161 0 : _per_motor.calibration_end();
162 0 : }
163 : #endif
164 :
165 : #if COMPASS_CAL_ENABLED
166 : // compass calibrator interface
167 : void cal_update();
168 :
169 : // start_calibration_all will only return false if there are no
170 : // compasses to calibrate.
171 : bool start_calibration_all(bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot = false);
172 :
173 : void cancel_calibration_all();
174 :
175 139081 : bool compass_cal_requires_reboot() const { return _cal_requires_reboot; }
176 : bool is_calibrating() const;
177 :
178 : #if HAL_MAVLINK_BINDINGS_ENABLED
179 : /*
180 : handle an incoming MAG_CAL command
181 : */
182 : MAV_RESULT handle_mag_cal_command(const mavlink_command_int_t &packet);
183 :
184 : bool send_mag_cal_progress(const class GCS_MAVLINK& link);
185 : bool send_mag_cal_report(const class GCS_MAVLINK& link);
186 : #endif // HAL_MAVLINK_BINDINGS_ENABLED
187 : #endif // COMPASS_CAL_ENABLED
188 :
189 : // indicate which bit in LOG_BITMASK indicates we should log compass readings
190 2094 : void set_log_bit(uint32_t log_bit) { _log_bit = log_bit; }
191 :
192 : void Write_Compass(void);
193 : void Write_Compass_instance(uint64_t time_us, uint8_t mag_instance);
194 :
195 : // check if the compasses are pointing in the same direction
196 : bool consistent() const;
197 :
198 : /// Return the health of a compass
199 : bool healthy(uint8_t i) const;
200 4282113 : bool healthy(void) const { return healthy(_first_usable); }
201 : uint8_t get_healthy_mask() const;
202 :
203 : /// Returns the current offset values
204 : ///
205 : /// @returns The current compass offsets in milligauss.
206 : ///
207 128224121 : const Vector3f &get_offsets(uint8_t i) const { return _get_state(Priority(i)).offset; }
208 133156 : const Vector3f &get_offsets(void) const { return get_offsets(_first_usable); }
209 :
210 : #if AP_COMPASS_DIAGONALS_ENABLED
211 9 : const Vector3f &get_diagonals(uint8_t i) const { return _get_state(Priority(i)).diagonals; }
212 : const Vector3f &get_diagonals(void) const { return get_diagonals(_first_usable); }
213 :
214 9 : const Vector3f &get_offdiagonals(uint8_t i) const { return _get_state(Priority(i)).offdiagonals; }
215 : const Vector3f &get_offdiagonals(void) const { return get_offdiagonals(_first_usable); }
216 : #endif // AP_COMPASS_DIAGONALS_ENABLED
217 :
218 : // learn offsets accessor
219 41199290 : bool learn_offsets_enabled() const { return _learn == LearnType::INFLIGHT; }
220 :
221 : #if AP_COMPASS_LEARN_COPY_FROM_EKF_ENABLED
222 : // if COMPASS_LEARN==COPY_FROM_EKF, save the offsets the EKF has
223 : // learned into the COMPASS_OFS parameters. Called from
224 : // AP_Arming::disarm(); note that this *must* happen before the
225 : // vehicle calls hal.util->set_soft_armed(false).
226 : void save_ekf_learned_offsets();
227 : #endif // AP_COMPASS_LEARN_COPY_FROM_EKF_ENABLED
228 :
229 : /// return true if the compass should be used for yaw calculations
230 : bool use_for_yaw(uint8_t i) const;
231 : bool use_for_yaw(void) const;
232 :
233 : /// Sets the local magnetic field declination.
234 : ///
235 : /// @param radians Local field declination.
236 : /// @param save_to_eeprom true to save to eeprom (false saves only to memory)
237 : ///
238 : void set_declination(float radians, bool save_to_eeprom = true);
239 : float get_declination() const;
240 :
241 41156285 : bool auto_declination_enabled() const { return _auto_declination != 0; }
242 :
243 : // set overall board orientation
244 54325 : void set_board_orientation(enum Rotation orientation) {
245 54325 : _board_orientation = orientation;
246 54325 : }
247 :
248 : // get overall board orientation
249 46666723 : enum Rotation get_board_orientation(void) const {
250 46666723 : return _board_orientation;
251 : }
252 :
253 : #if AP_COMPASS_LEARN_COPY_FROM_EKF_ENABLED
254 : // true if the field published for this instance reaches us in the
255 : // body frame - that is, AP_Compass_Backend::rotate_field() applies
256 : // no rotation to it. An offset computed in the body frame is only
257 : // meaningful for such an instance; see AP_AHRS_SIM::get_mag_offsets().
258 : bool instance_is_unrotated(uint8_t i) const;
259 :
260 : // device id of the compass at this priority index. Used to find
261 : // the simulated sensor a priority index corresponds to; see
262 : // AP_AHRS_SIM::get_mag_offsets(). Zero if there is none.
263 : uint32_t get_dev_id(uint8_t i) const;
264 : #endif
265 :
266 : /// Set the motor compensation type
267 : ///
268 : /// @param comp_type 0 = disabled, 1 = enabled use throttle, 2 = enabled use current
269 : ///
270 : void motor_compensation_type(const uint8_t comp_type);
271 :
272 : /// get the motor compensation value.
273 : uint8_t get_motor_compensation_type() const {
274 : return _motor_comp_type;
275 : }
276 :
277 : /// Set the motor compensation factor x/y/z values.
278 : ///
279 : /// @param i instance of compass
280 : /// @param offsets Offsets multiplied by the throttle value and added to the raw mag_ values.
281 : ///
282 : void set_motor_compensation(uint8_t i, const Vector3f &motor_comp_factor);
283 :
284 : /// get motor compensation factors as a vector
285 : const Vector3f& get_motor_compensation(uint8_t i) const { return _get_state(Priority(i)).motor_compensation; }
286 : const Vector3f& get_motor_compensation(void) const { return get_motor_compensation(_first_usable); }
287 :
288 : /// Saves the current motor compensation x/y/z values.
289 : ///
290 : /// This should be invoked periodically to save the offset values calculated by the motor compensation auto learning
291 : ///
292 : void save_motor_compensation();
293 :
294 : /// Returns the current motor compensation offset values
295 : ///
296 : /// @returns The current compass offsets in milligauss.
297 : ///
298 4247415 : const Vector3f &get_motor_offsets(uint8_t i) const { return _get_state(Priority(i)).motor_offset; }
299 : const Vector3f &get_motor_offsets(void) const { return get_motor_offsets(_first_usable); }
300 :
301 : /// Set the throttle as a percentage from 0.0 to 1.0
302 : /// @param thr_pct throttle expressed as a percentage from 0 to 1.0
303 814014 : void set_throttle(float thr_pct) {
304 814014 : if (_motor_comp_type == AP_COMPASS_MOT_COMP_THROTTLE) {
305 0 : _thr = thr_pct;
306 : }
307 814014 : }
308 :
309 : #if COMPASS_MOT_ENABLED
310 : /// Set the battery voltage for per-motor compensation
311 759512 : void set_voltage(float voltage) {
312 759512 : _per_motor.set_voltage(voltage);
313 759512 : }
314 : #endif
315 :
316 : /// Returns True if the compasses have been configured (i.e. offsets saved)
317 : ///
318 : /// @returns True if compass has been configured
319 : ///
320 : bool configured(uint8_t i);
321 : bool configured(char *failure_msg, uint8_t failure_msg_len);
322 :
323 : // return last update time in microseconds
324 44009756 : uint32_t last_update_usec(void) const { return last_update_usec(_first_usable); }
325 171694790 : uint32_t last_update_usec(uint8_t i) const { return _get_state(Priority(i)).last_update_usec; }
326 :
327 : uint32_t last_update_ms(void) const { return last_update_ms(_first_usable); }
328 : uint32_t last_update_ms(uint8_t i) const { return _get_state(Priority(i)).last_update_ms; }
329 :
330 : static const struct AP_Param::GroupInfo var_info[];
331 :
332 : enum class LearnType {
333 : NONE = 0,
334 : // INTERNAL = 1,
335 : COPY_FROM_EKF = 2,
336 : INFLIGHT = 3,
337 : };
338 :
339 : // return the chosen learning type
340 1596 : LearnType get_learn_type(void) const {
341 1596 : const LearnType learn_type = (LearnType)_learn.get();
342 : #if !AP_COMPASS_LEARN_COPY_FROM_EKF_ENABLED
343 : if (learn_type == LearnType::COPY_FROM_EKF) {
344 : // there is no code compiled in to save the EKF's offsets,
345 : // so report the feature as off rather than letting callers
346 : // believe a learning type is selected which cannot run
347 : return LearnType::NONE;
348 : }
349 : #endif
350 1596 : return learn_type;
351 : }
352 :
353 : // set the learning type
354 1 : void set_learn_type(LearnType type, bool save) {
355 1 : if (save) {
356 1 : _learn.set_and_save(type);
357 : } else {
358 0 : _learn.set(type);
359 : }
360 1 : }
361 :
362 : // return maximum allowed compass offsets
363 133213 : uint16_t get_offsets_max(void) const {
364 133213 : return (uint16_t)_offset_max.get();
365 : }
366 :
367 46650409 : uint8_t get_filter_range() const { return uint8_t(_filter_range.get()); }
368 :
369 : #if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED
370 : /*
371 : fast compass calibration given vehicle position and yaw
372 : */
373 : bool mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
374 : float lat_deg, float lon_deg,
375 : bool force_use=false);
376 : #endif
377 :
378 : #if AP_COMPASS_MSP_ENABLED
379 : void handle_msp(const MSP::msp_compass_data_message_t &pkt);
380 : #endif
381 :
382 : #if AP_COMPASS_EXTERNALAHRS_ENABLED
383 : void handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt);
384 : #endif
385 :
386 : // force save of current calibration as valid
387 : void force_save_calibration(void);
388 :
389 : // get the first compass marked for use by COMPASSx_USE
390 41156288 : uint8_t get_first_usable(void) const { return _first_usable; }
391 :
392 : private:
393 : static Compass *_singleton;
394 :
395 : // Use Priority and StateIndex typesafe index types
396 : // to distinguish between two different type of indexing
397 : // We use StateIndex for access by Backend
398 : // and Priority for access by Frontend
399 : DECLARE_TYPESAFE_INDEX(Priority, uint8_t);
400 : DECLARE_TYPESAFE_INDEX(StateIndex, uint8_t);
401 :
402 : /// Register a new compas driver, allocating an instance number
403 : ///
404 : /// @param dev_id Dev ID of compass to register against
405 : ///
406 : /// @return instance number saved against the dev id or first available empty instance number
407 : bool register_compass(int32_t dev_id, uint8_t& instance) WARN_IF_UNUSED;
408 :
409 : // load backend drivers
410 : __INITFUNC__ void _probe_external_i2c_compasses(void);
411 : __INITFUNC__ void _detect_backends(void);
412 : __INITFUNC__ void probe_i2c_spi_compasses(void);
413 : #if AP_COMPASS_DRONECAN_ENABLED
414 : void probe_dronecan_compasses(void);
415 : #endif
416 :
417 : #if COMPASS_CAL_ENABLED
418 : // compass cal
419 : void _update_calibration_trampoline();
420 : bool _accept_calibration(uint8_t i);
421 : bool _accept_calibration_mask(uint8_t mask);
422 : void _cancel_calibration(uint8_t i);
423 : void _cancel_calibration_mask(uint8_t mask);
424 : uint8_t _get_cal_mask();
425 : bool _start_calibration(uint8_t i, bool retry=false, float delay_sec=0.0f);
426 : bool _start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot=false);
427 : #if HAL_MAVLINK_BINDINGS_ENABLED
428 : Priority next_cal_progress_idx[MAVLINK_COMM_NUM_BUFFERS];
429 : Priority next_cal_report_idx[MAVLINK_COMM_NUM_BUFFERS];
430 : #endif
431 : #endif // COMPASS_CAL_ENABLED
432 :
433 : // see if we already have probed a i2c sensor by bus number and address
434 : bool _i2c_sensor_is_registered(uint8_t bus_num, uint8_t address) const;
435 :
436 : #if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED
437 : /*
438 : get mag field with the effects of offsets, diagonals and
439 : off-diagonals removed
440 : */
441 : bool get_uncorrected_field(uint8_t instance, Vector3f &field) const;
442 : #endif
443 :
444 : #if COMPASS_CAL_ENABLED
445 : //keep track of which calibrators have been saved
446 : RestrictIDTypeArray<bool, COMPASS_MAX_INSTANCES, Priority> _cal_saved;
447 : bool _cal_autosave;
448 :
449 : //autoreboot after compass calibration
450 : bool _compass_cal_autoreboot;
451 : bool _cal_requires_reboot;
452 : bool _cal_has_run;
453 : #endif // COMPASS_CAL_ENABLED
454 :
455 : // enum of drivers for COMPASS_DISBLMSK
456 : enum DriverType {
457 : #if AP_COMPASS_HMC5843_ENABLED
458 : DRIVER_HMC5843 =0,
459 : #endif
460 : #if AP_COMPASS_LSM303D_ENABLED
461 : DRIVER_LSM303D =1,
462 : #endif
463 : #if AP_COMPASS_AK8963_ENABLED
464 : DRIVER_AK8963 =2,
465 : #endif
466 : #if AP_COMPASS_BMM150_ENABLED
467 : DRIVER_BMM150 =3,
468 : #endif
469 : #if AP_COMPASS_LSM9DS1_ENABLED
470 : DRIVER_LSM9DS1 =4,
471 : #endif
472 : #if AP_COMPASS_LIS3MDL_ENABLED
473 : DRIVER_LIS3MDL =5,
474 : #endif
475 : #if AP_COMPASS_AK09916_ENABLED
476 : DRIVER_AK09916 =6,
477 : #endif
478 : #if AP_COMPASS_IST8310_ENABLED
479 : DRIVER_IST8310 =7,
480 : #endif
481 : #if AP_COMPASS_ICM20948_ENABLED
482 : DRIVER_ICM20948 =8,
483 : #endif
484 : #if AP_COMPASS_MMC3416_ENABLED
485 : DRIVER_MMC3416 =9,
486 : #endif
487 : #if AP_COMPASS_DRONECAN_ENABLED
488 : DRIVER_UAVCAN =11,
489 : #endif
490 : #if AP_COMPASS_QMC5883L_ENABLED
491 : DRIVER_QMC5883L =12,
492 : #endif
493 : #if AP_COMPASS_SITL_ENABLED
494 : DRIVER_SITL =13,
495 : #endif
496 : #if AP_COMPASS_MAG3110_ENABLED
497 : DRIVER_MAG3110 =14,
498 : #endif
499 : #if AP_COMPASS_IST8308_ENABLED
500 : DRIVER_IST8308 =15,
501 : #endif
502 : #if AP_COMPASS_RM3100_ENABLED
503 : DRIVER_RM3100 =16,
504 : #endif
505 : #if AP_COMPASS_MSP_ENABLED
506 : DRIVER_MSP =17,
507 : #endif
508 : #if AP_COMPASS_EXTERNALAHRS_ENABLED
509 : DRIVER_EXTERNALAHRS =18,
510 : #endif
511 : #if AP_COMPASS_MMC5XX3_ENABLED
512 : DRIVER_MMC5XX3 =19,
513 : #endif
514 : #if AP_COMPASS_QMC5883P_ENABLED
515 : DRIVER_QMC5883P =20,
516 : #endif
517 : #if AP_COMPASS_BMM350_ENABLED
518 : DRIVER_BMM350 =21,
519 : #endif
520 : #if AP_COMPASS_IIS2MDC_ENABLED
521 : DRIVER_IIS2MDC =22,
522 : #endif
523 : // DRIVER_LIS2MDL =23, // DO NOT re-use this ID; same sensor as IIS2MDC
524 : #if AP_COMPASS_AF9838_ENABLED
525 : DRIVER_AF9838 =24,
526 : #endif
527 : };
528 :
529 : bool _driver_enabled(enum DriverType driver_type);
530 : void add_backend(DriverType driver_type, AP_Compass_Backend *backend);
531 :
532 : // helper probe functions
533 : void probe_ak09916_via_icm20948(uint8_t i2c_bus, uint8_t ak09916_addr, uint8_t icm20948_addr, bool external, Rotation rotation);
534 : void probe_ak09916_via_icm20948(uint8_t ins_instance, Rotation rotation);
535 : void probe_ak8963_via_mpu9250(uint8_t imu_instance, Rotation rotation);
536 :
537 : using probe_i2c_dev_probefn_t = AP_Compass_Backend* (*)(AP_HAL::OwnPtr<AP_HAL::Device>, bool, Rotation);
538 : void probe_i2c_dev(DriverType driver_type, probe_i2c_dev_probefn_t probefn, uint8_t i2c_bus, uint8_t i2c_addr, bool external, Rotation rotation);
539 :
540 : using probe_spi_dev_probefn_t = AP_Compass_Backend* (*)(AP_HAL::OwnPtr<AP_HAL::Device>, bool, Rotation);
541 : void probe_spi_dev(DriverType driver_type, probe_spi_dev_probefn_t probefn, const char *name, bool external, Rotation rotation);
542 : // short-lived method which expects a probe function that doesn't
543 : // offer the ability to specify an external rotation
544 : using probe_spi_dev_noexternal_probefn_t = AP_Compass_Backend* (*)(AP_HAL::OwnPtr<AP_HAL::Device>, Rotation);
545 : void probe_spi_dev(DriverType driver_type, probe_spi_dev_noexternal_probefn_t probefn, const char *name, Rotation rotation);
546 :
547 : // backend objects
548 : AP_Compass_Backend *_backends[COMPASS_MAX_BACKEND];
549 : uint8_t _backend_count;
550 :
551 : // whether to enable the compass drivers at all
552 : AP_Int8 _enabled;
553 :
554 : // number of registered compasses.
555 : uint8_t _compass_count;
556 :
557 : // number of unregistered compasses.
558 : uint8_t _unreg_compass_count;
559 :
560 : // settable parameters
561 : AP_Enum<LearnType> _learn;
562 :
563 : // board orientation from AHRS
564 : enum Rotation _board_orientation = ROTATION_NONE;
565 :
566 : // declination in radians
567 : AP_Float _declination;
568 :
569 : // enable automatic declination code
570 : AP_Int8 _auto_declination;
571 :
572 : // stores which bit is used to indicate we should log compass readings
573 : uint32_t _log_bit = -1;
574 :
575 : // motor compensation type
576 : // 0 = disabled, 1 = enabled for throttle, 2 = enabled for current
577 : AP_Int8 _motor_comp_type;
578 :
579 : // automatic compass orientation on calibration
580 : AP_Int8 _rotate_auto;
581 :
582 : // throttle expressed as a percentage from 0 ~ 1.0, used for motor compensation
583 : float _thr;
584 :
585 : struct mag_state {
586 : AP_Int8 external;
587 : bool healthy;
588 : bool registered;
589 : Compass::Priority priority;
590 : AP_Int8 orientation;
591 : AP_Vector3f offset;
592 : #if AP_COMPASS_DIAGONALS_ENABLED
593 : AP_Vector3f diagonals;
594 : AP_Vector3f offdiagonals;
595 : #endif
596 : AP_Float scale_factor;
597 :
598 : // device id detected at init.
599 : // saved to eeprom when offsets are saved allowing ram &
600 : // eeprom values to be compared as consistency check
601 : AP_Int32 dev_id;
602 : // Initialised when compass is detected
603 : int32_t detected_dev_id;
604 : // Initialised at boot from saved devid
605 : int32_t expected_dev_id;
606 :
607 : // factors multiplied by throttle and added to compass outputs
608 : AP_Vector3f motor_compensation;
609 :
610 : // latest compensation added to compass
611 : Vector3f motor_offset;
612 :
613 : // corrected magnetic field strength
614 : Vector3f field;
615 :
616 : // when we last got data
617 : uint32_t last_update_ms;
618 : uint32_t last_update_usec;
619 :
620 : // board specific orientation
621 : enum Rotation rotation;
622 :
623 : // accumulated samples, protected by _sem, used by AP_Compass_Backend
624 : Vector3f accum;
625 : uint32_t accum_count;
626 : // We only copy persistent params
627 : void copy_from(const mag_state& state);
628 : };
629 :
630 : //Create an Array of mag_state to be accessible by StateIndex only
631 : RestrictIDTypeArray<mag_state, COMPASS_MAX_INSTANCES+1, StateIndex> _state;
632 :
633 : //Convert Priority to StateIndex
634 : StateIndex _get_state_id(Priority priority) const;
635 : //Get State Struct by Priority
636 783526280 : const struct mag_state& _get_state(Priority priority) const { return _state[_get_state_id(priority)]; }
637 : //Convert StateIndex to Priority
638 46650409 : Priority _get_priority(StateIndex state_id) { return _state[state_id].priority; }
639 : //Method to detect compass beyond initialisation stage
640 : void _detect_runtime(void);
641 : // This method reorganises devid list to match
642 : // priority list, only call before detection at boot
643 : #if COMPASS_MAX_INSTANCES > 1
644 : void _reorder_compass_params();
645 : #endif
646 : // Update Priority List for Mags, by default, we just
647 : // load them as they come up the first time
648 : Priority _update_priority_list(int32_t dev_id);
649 :
650 : // method to check if the mag with the devid
651 : // is a replacement mag
652 : bool is_replacement_mag(uint32_t dev_id);
653 :
654 : //remove the devid from unreg compass list
655 : void remove_unreg_dev_id(uint32_t devid);
656 :
657 : void _reset_compass_id();
658 : //Create Arrays to be accessible by Priority only
659 : RestrictIDTypeArray<AP_Int8, COMPASS_MAX_INSTANCES, Priority> _use_for_yaw;
660 : #if COMPASS_MAX_INSTANCES > 1
661 : RestrictIDTypeArray<AP_Int32, COMPASS_MAX_INSTANCES, Priority> _priority_did_stored_list;
662 : RestrictIDTypeArray<int32_t, COMPASS_MAX_INSTANCES, Priority> _priority_did_list;
663 : #endif
664 :
665 : AP_Int16 _offset_max;
666 :
667 : // bitmask of options
668 : enum class Option : uint16_t {
669 : CAL_REQUIRE_GPS = (1U<<0),
670 : ALLOW_DRONECAN_AUTO_REPLACEMENT = (1U<<1),
671 : };
672 2150 : bool option_set(Option opt) const { return (_options.get() & uint16_t(opt)) != 0; }
673 : AP_Int16 _options;
674 :
675 : #if COMPASS_CAL_ENABLED
676 : RestrictIDTypeArray<CompassCalibrator*, COMPASS_MAX_INSTANCES, Priority> _calibrator;
677 : #endif
678 :
679 : #if COMPASS_MOT_ENABLED
680 : // per-motor compass compensation
681 : Compass_PerMotor _per_motor{*this};
682 : #endif
683 :
684 : AP_Float _calibration_threshold;
685 :
686 : // mask of driver types to not load. Bit positions match DEVTYPE_ in backend
687 : AP_Int32 _driver_type_mask;
688 :
689 : #if COMPASS_MAX_UNREG_DEV
690 : // Put extra dev ids detected
691 : AP_Int32 extra_dev_id[COMPASS_MAX_UNREG_DEV];
692 : uint32_t _previously_unreg_mag[COMPASS_MAX_UNREG_DEV];
693 : #endif
694 :
695 : AP_Int8 _filter_range;
696 :
697 : CompassLearn *learn;
698 : bool learn_allocated;
699 :
700 : /// Sets the initial location used to get declination
701 : ///
702 : /// @param latitude GPS Latitude.
703 : /// @param longitude GPS Longitude.
704 : ///
705 : void try_set_initial_location();
706 : bool _initial_location_set;
707 :
708 : bool _cal_thread_started;
709 :
710 : #if AP_COMPASS_MSP_ENABLED
711 : uint8_t msp_instance_mask;
712 : #endif
713 : bool init_done;
714 :
715 : bool suppress_devid_save;
716 :
717 : uint8_t _first_usable; // first compass usable based on COMPASSx_USE param
718 : };
719 :
720 : namespace AP {
721 : Compass &compass();
722 : };
|
