|
|
@ -1014,6 +1014,8 @@ bool Compass::_have_i2c_driver(uint8_t bus, uint8_t address) const |
|
|
|
void Compass::_probe_external_i2c_compasses(void) |
|
|
|
void Compass::_probe_external_i2c_compasses(void) |
|
|
|
{ |
|
|
|
{ |
|
|
|
bool all_external = (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2); |
|
|
|
bool all_external = (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if !defined(HAL_DISABLE_I2C_MAGS_BY_DEFAULT) || defined(HAL_USE_I2C_MAG_HMC5843) |
|
|
|
// external i2c bus
|
|
|
|
// external i2c bus
|
|
|
|
FOREACH_I2C_EXTERNAL(i) { |
|
|
|
FOREACH_I2C_EXTERNAL(i) { |
|
|
|
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(i, HAL_COMPASS_HMC5843_I2C_ADDR), |
|
|
|
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(i, HAL_COMPASS_HMC5843_I2C_ADDR), |
|
|
@ -1028,7 +1030,9 @@ void Compass::_probe_external_i2c_compasses(void) |
|
|
|
all_external, all_external?ROTATION_ROLL_180:ROTATION_YAW_270)); |
|
|
|
all_external, all_external?ROTATION_ROLL_180:ROTATION_YAW_270)); |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if !defined(HAL_DISABLE_I2C_MAGS_BY_DEFAULT) || defined(HAL_USE_I2C_MAG_QMC5883L) |
|
|
|
//external i2c bus
|
|
|
|
//external i2c bus
|
|
|
|
FOREACH_I2C_EXTERNAL(i) { |
|
|
|
FOREACH_I2C_EXTERNAL(i) { |
|
|
|
ADD_BACKEND(DRIVER_QMC5883L, AP_Compass_QMC5883L::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883L_I2C_ADDR), |
|
|
|
ADD_BACKEND(DRIVER_QMC5883L, AP_Compass_QMC5883L::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883L_I2C_ADDR), |
|
|
@ -1044,6 +1048,7 @@ void Compass::_probe_external_i2c_compasses(void) |
|
|
|
all_external?HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL:HAL_COMPASS_QMC5883L_ORIENTATION_INTERNAL)); |
|
|
|
all_external?HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL:HAL_COMPASS_QMC5883L_ORIENTATION_INTERNAL)); |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
|
|
#ifndef HAL_BUILD_AP_PERIPH |
|
|
|
#ifndef HAL_BUILD_AP_PERIPH |
|
|
|
// AK09916 on ICM20948
|
|
|
|
// AK09916 on ICM20948
|
|
|
@ -1060,6 +1065,7 @@ void Compass::_probe_external_i2c_compasses(void) |
|
|
|
} |
|
|
|
} |
|
|
|
#endif // HAL_BUILD_AP_PERIPH
|
|
|
|
#endif // HAL_BUILD_AP_PERIPH
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if !defined(HAL_DISABLE_I2C_MAGS_BY_DEFAULT) || defined(HAL_USE_I2C_MAG_LIS3MDL) |
|
|
|
// lis3mdl on bus 0 with default address
|
|
|
|
// lis3mdl on bus 0 with default address
|
|
|
|
FOREACH_I2C_INTERNAL(i) { |
|
|
|
FOREACH_I2C_INTERNAL(i) { |
|
|
|
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR), |
|
|
|
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR), |
|
|
@ -1083,7 +1089,9 @@ void Compass::_probe_external_i2c_compasses(void) |
|
|
|
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR2), |
|
|
|
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR2), |
|
|
|
true, ROTATION_YAW_90)); |
|
|
|
true, ROTATION_YAW_90)); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if !defined(HAL_DISABLE_I2C_MAGS_BY_DEFAULT) || defined(HAL_USE_I2C_MAG_AK09916) |
|
|
|
// AK09916. This can be found twice, due to the ICM20948 i2c bus pass-thru, so we need to be careful to avoid that
|
|
|
|
// AK09916. This can be found twice, due to the ICM20948 i2c bus pass-thru, so we need to be careful to avoid that
|
|
|
|
FOREACH_I2C_EXTERNAL(i) { |
|
|
|
FOREACH_I2C_EXTERNAL(i) { |
|
|
|
ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR), |
|
|
|
ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR), |
|
|
@ -1093,7 +1101,9 @@ void Compass::_probe_external_i2c_compasses(void) |
|
|
|
ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR), |
|
|
|
ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR), |
|
|
|
all_external, all_external?ROTATION_YAW_270:ROTATION_NONE)); |
|
|
|
all_external, all_external?ROTATION_YAW_270:ROTATION_NONE)); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if !defined(HAL_DISABLE_I2C_MAGS_BY_DEFAULT) || defined(HAL_USE_I2C_MAG_IST8310) |
|
|
|
// IST8310 on external and internal bus
|
|
|
|
// IST8310 on external and internal bus
|
|
|
|
if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_FMUV5 && |
|
|
|
if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_FMUV5 && |
|
|
|
AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_FMUV6) { |
|
|
|
AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_FMUV6) { |
|
|
@ -1118,13 +1128,17 @@ void Compass::_probe_external_i2c_compasses(void) |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if !defined(HAL_DISABLE_I2C_MAGS_BY_DEFAULT) || defined(HAL_USE_I2C_MAG_IST8308) |
|
|
|
// external i2c bus
|
|
|
|
// external i2c bus
|
|
|
|
FOREACH_I2C_EXTERNAL(i) { |
|
|
|
FOREACH_I2C_EXTERNAL(i) { |
|
|
|
ADD_BACKEND(DRIVER_IST8308, AP_Compass_IST8308::probe(GET_I2C_DEVICE(i, HAL_COMPASS_IST8308_I2C_ADDR), |
|
|
|
ADD_BACKEND(DRIVER_IST8308, AP_Compass_IST8308::probe(GET_I2C_DEVICE(i, HAL_COMPASS_IST8308_I2C_ADDR), |
|
|
|
true, ROTATION_NONE)); |
|
|
|
true, ROTATION_NONE)); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if !defined(HAL_DISABLE_I2C_MAGS_BY_DEFAULT) || defined(HAL_USE_I2C_MAG_RM3100) |
|
|
|
#ifdef HAL_COMPASS_RM3100_I2C_ADDR |
|
|
|
#ifdef HAL_COMPASS_RM3100_I2C_ADDR |
|
|
|
const uint8_t rm3100_addresses[] = { HAL_COMPASS_RM3100_I2C_ADDR }; |
|
|
|
const uint8_t rm3100_addresses[] = { HAL_COMPASS_RM3100_I2C_ADDR }; |
|
|
|
#else |
|
|
|
#else |
|
|
@ -1140,11 +1154,13 @@ void Compass::_probe_external_i2c_compasses(void) |
|
|
|
ADD_BACKEND(DRIVER_RM3100, AP_Compass_RM3100::probe(GET_I2C_DEVICE(i, rm3100_addresses[j]), true, ROTATION_NONE)); |
|
|
|
ADD_BACKEND(DRIVER_RM3100, AP_Compass_RM3100::probe(GET_I2C_DEVICE(i, rm3100_addresses[j]), true, ROTATION_NONE)); |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
FOREACH_I2C_INTERNAL(i) { |
|
|
|
FOREACH_I2C_INTERNAL(i) { |
|
|
|
for (uint8_t j=0; j<ARRAY_SIZE(rm3100_addresses); j++) { |
|
|
|
for (uint8_t j=0; j<ARRAY_SIZE(rm3100_addresses); j++) { |
|
|
|
ADD_BACKEND(DRIVER_RM3100, AP_Compass_RM3100::probe(GET_I2C_DEVICE(i, rm3100_addresses[j]), all_external, ROTATION_NONE)); |
|
|
|
ADD_BACKEND(DRIVER_RM3100, AP_Compass_RM3100::probe(GET_I2C_DEVICE(i, rm3100_addresses[j]), all_external, ROTATION_NONE)); |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
#endif |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
/*
|
|
|
|