diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index e48691e6c5..9a35ece6de 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -418,13 +418,25 @@ uint8_t Compass::register_compass(void) return _compass_count++; } -void Compass::_add_backend(AP_Compass_Backend *backend) +bool Compass::_add_backend(AP_Compass_Backend *backend, const char *name, bool external) { - if (!backend) - return; - if (_backend_count == COMPASS_MAX_BACKEND) + if (name != nullptr) { + hal.console->printf("%s: %s compass %sdetected\n", name, + external ? "External" : "Onboard", + backend == nullptr ? "not " : ""); + } + + if (!backend) { + return false; + } + + if (_backend_count == COMPASS_MAX_BACKEND) { AP_HAL::panic("Too many compass backends"); + } + _backends[_backend_count++] = backend; + + return true; } /* @@ -433,118 +445,84 @@ void Compass::_add_backend(AP_Compass_Backend *backend) void Compass::_detect_backends(void) { if (_hil_mode) { - _add_backend(AP_Compass_HIL::detect(*this)); + _add_backend(AP_Compass_HIL::detect(*this), nullptr, false); return; } #if HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL - _add_backend(AP_Compass_HIL::detect(*this)); + _add_backend(AP_Compass_HIL::detect(*this), nullptr, false); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_PX4 || HAL_COMPASS_DEFAULT == HAL_COMPASS_VRBRAIN - _add_backend(AP_Compass_PX4::detect(*this)); + _add_backend(AP_Compass_PX4::detect(*this), nullptr, false); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QURT - _add_backend(AP_Compass_QURT::detect(*this)); + _add_backend(AP_Compass_QURT::detect(*this), nullptr, false); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_RASPILOT - _add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true)); - _add_backend(AP_Compass_LSM303D::probe(*this, hal.spi->get_device("lsm9ds0_am"))); + _add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true), + AP_Compass_HMC5843::name, true); + _add_backend(AP_Compass_LSM303D::probe(*this, hal.spi->get_device("lsm9ds0_am")), + AP_Compass_LSM303D::name, false); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BH - // detect_mpu9250() failed will cause panic if no actual mpu9250 backend, - // in BH, only one compass should be detected - AP_Compass_Backend *backend = AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)); - if (backend) { - _add_backend(backend); - } else { - _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0)); + // In BH, only one compass should be detected + bool ret = _add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)), + AP_Compass_HMC5843::name, false); + if (!ret) { + _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0), + AP_Compass_AK8963::name, false); } #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QFLIGHT _add_backend(AP_Compass_QFLIGHT::detect(*this)); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BBBMINI - AP_Compass_Backend *backend = AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)); - if (backend) { - _add_backend(backend); - hal.console->printf("HMC5843: External compass detected\n"); - } else { - hal.console->printf("HMC5843: External compass not detected\n"); - } - - backend = AP_Compass_AK8963::probe_mpu9250(*this, 0); - if (backend) { - _add_backend(backend); - hal.console->printf("AK8953: Onboard compass detected\n"); - } else { - hal.console->printf("AK8953: Onboard compass not detected\n"); - } - - backend = AP_Compass_AK8963::probe_mpu9250(*this, 1); - if (backend) { - _add_backend(backend); - hal.console->printf("AK8953: External compass detected\n"); - } else { - hal.console->printf("AK8953: External compass not detected\n"); - } + _add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true), + AP_Compass_HMC5843::name, true); + _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0), + AP_Compass_AK8963::name, false); + _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 1), + AP_Compass_AK8963::name, true); #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE - _add_backend(AP_Compass_HMC5843::probe_mpu6000(*this)); + _add_backend(AP_Compass_HMC5843::probe_mpu6000(*this), + AP_Compass_HMC5843::name, false); _add_backend(AP_Compass_HMC5843::probe(*this, - Linux::I2CDeviceManager::from(hal.i2c_mgr)->get_device( - { /* UEFI with lpss set to ACPI */ - "platform/80860F41:05", - /* UEFI with lpss set to PCI */ - "pci0000:00/0000:00:18.6" }, - HAL_COMPASS_HMC5843_I2C_ADDR), - true)); + Linux::I2CDeviceManager::from(hal.i2c_mgr)->get_device( + { /* UEFI with lpss set to ACPI */ + "platform/80860F41:05", + /* UEFI with lpss set to PCI */ + "pci0000:00/0000:00:18.6" }, + HAL_COMPASS_HMC5843_I2C_ADDR), + true), + AP_Compass_HMC5843::name, true); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NAVIO2 - AP_Compass_Backend *backend = AP_Compass_AK8963::probe_mpu9250(*this, 0); - if (backend) { - _add_backend(backend); - hal.console->printf("AK8953: Compass detected\n"); - } else { - hal.console->printf("AK8953: Compass not detected\n"); - } - - backend = AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m")); - if (backend) { - _add_backend(backend); - hal.console->printf("LSM9DS1: Compass detected\n"); - } else { - hal.console->printf("LSM9DS1: Compass not detected\n"); - } - - backend = AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)); - if (backend) { - _add_backend(backend); - hal.console->printf("HMC5843: External compass detected\n"); - } else { - hal.console->printf("HMC5843: External compass not detected\n"); - } + _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0), + AP_Compass_AK8963::name, false); + _add_backend(AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m")), + AP_Compass_LSM9DS1::name, false); + _add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true), + AP_Compass_HMC5843::name, true); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NAVIO - AP_Compass_Backend *backend = AP_Compass_AK8963::probe_mpu9250(*this, 0); - if (backend) { - _add_backend(backend); - hal.console->printf("AK8953: Compass detected\n"); - } else { - hal.console->printf("AK8953: Compass not detected\n"); - } - - backend = AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)); - if (backend) { - _add_backend(backend); - hal.console->printf("HMC5843: External compass detected\n"); - } else { - hal.console->printf("HMC5843: External compass not detected\n"); - } + _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0), + AP_Compass_AK8963::name, false); + _add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true), + AP_Compass_HMC5843::name, true); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250 - _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0)); + _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0), + AP_Compass_AK8963::name, false); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843 - _add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR))); + _add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)), + AP_Compass_HMC5843::name, false); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843_MPU6000 - _add_backend(AP_Compass_HMC5843::probe_mpu6000(*this)); + _add_backend(AP_Compass_HMC5843::probe_mpu6000(*this), + AP_Compass_HMC5843::name, false); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_I2C - _add_backend(AP_Compass_AK8963::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR))); + _add_backend(AP_Compass_AK8963::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)), + AP_Compass_AK8963::name, false); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250_I2C - _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR))); + _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)), + AP_Compass_AK8963::name, false); #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE - _add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR))); - _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0)); - _add_backend(AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m"))); + _add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)), + AP_Compass_HMC5843::name, false); + _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0), + AP_Compass_AK8963::name, false); + _add_backend(AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m")), + AP_Compass_LSM9DS1::name, false); #else #error Unrecognised HAL_COMPASS_TYPE setting #endif diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index e0037c0f33..0d3c0dae4b 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -313,7 +313,7 @@ private: uint8_t register_compass(void); // load backend drivers - void _add_backend(AP_Compass_Backend *backend); + bool _add_backend(AP_Compass_Backend *backend, const char *name, bool external); void _detect_backends(void); //keep track of number of calibration reports sent