diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index ba1ca2a602..68bcb448f0 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -1321,6 +1321,23 @@ void Compass::_detect_backends(void) } #endif + probe_i2c_spi_compasses(); + +#if AP_COMPASS_DRONECAN_ENABLED + probe_dronecan_compasses(); +#endif + + if (_backend_count == 0 || + _compass_count == 0) { + DEV_PRINTF("No Compass backends available\n"); + } +} + +/* + probe i2c and SPI compasses + */ +void Compass::probe_i2c_spi_compasses(void) +{ #if defined(HAL_MAG_PROBE_LIST) // driver probes defined by COMPASS lines in hwdef.dat HAL_MAG_PROBE_LIST; @@ -1464,87 +1481,89 @@ void Compass::_detect_backends(void) break; } #endif - +} #if AP_COMPASS_DRONECAN_ENABLED - if (_driver_enabled(DRIVER_UAVCAN)) { - for (uint8_t i=0; i 0 - if (_unreg_compass_count == COMPASS_MAX_UNREG_DEV) { - break; - } -#endif +/* + look for DroneCAN compasses + */ +void Compass::probe_dronecan_compasses(void) +{ + if (!_driver_enabled(DRIVER_UAVCAN)) { + return; + } + for (uint8_t i=0; i 0 + if (_unreg_compass_count == COMPASS_MAX_UNREG_DEV) { + break; + } +#endif + } #if COMPASS_MAX_UNREG_DEV > 0 - if (option_set(Option::ALLOW_DRONECAN_AUTO_REPLACEMENT) && !suppress_devid_save) { - // check if there's any uavcan compass in prio slot that's not found - // and replace it if there's a replacement compass - for (Priority i(0); i 0 } +#endif // #if COMPASS_MAX_UNREG_DEV > 0 +} #endif // AP_COMPASS_DRONECAN_ENABLED - if (_backend_count == 0 || - _compass_count == 0) { - DEV_PRINTF("No Compass backends available\n"); - } -} // Check if the devid is a potential replacement compass // Following are the checks done to ensure the compass is a replacement diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index f6dbe68e2c..b798f31661 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -384,6 +384,10 @@ private: bool _add_backend(AP_Compass_Backend *backend); void _probe_external_i2c_compasses(void); void _detect_backends(void); + void probe_i2c_spi_compasses(void); +#if AP_COMPASS_DRONECAN_ENABLED + void probe_dronecan_compasses(void); +#endif // compass cal void _update_calibration_trampoline();