Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: added COMPASS_AUTO_ROT support 

this is set to "check but don't fix" by default. This matches the
behaviour for plane3.9.0
This commit is contained in:
Andrew Tridgell 2018-08-01 19:10:32 +10:00 committed by Randy Mackay
parent a403254c81
commit b959e353e2
12 changed files with 542 additions and 181 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

345
libraries/AP_Compass/AP_Compass.cpp
View File

@ -41,6 +41,10 @@ extern AP_HAL::HAL& hal;
#define HAL_COMPASS_FILTER_DEFAULT 0 // turned off by default
#endif
#ifndef HAL_COMPASS_AUTO_ROT_DEFAULT
#define HAL_COMPASS_AUTO_ROT_DEFAULT 1
#endif
const AP_Param::GroupInfo Compass::var_info[] = {
// index 0 was used for the old orientation matrix
@ -444,6 +448,12 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Increment: 1
AP_GROUPINFO("FLTR_RNG", 34, Compass, _filter_range, HAL_COMPASS_FILTER_DEFAULT),
// @Param: AUTO_ROT
// @DisplayName: Automatically check orientation
// @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.
// @Values: 0:Disabled,1:CheckOnly,2:CheckAndFix
AP_GROUPINFO("AUTO_ROT", 35, Compass, _rotate_auto, HAL_COMPASS_AUTO_ROT_DEFAULT),
AP_GROUPEND
};
@ -523,19 +533,144 @@ bool Compass::_driver_enabled(enum DriverType driver_type)
}
/*
see if we already have probed a driver by bus type
wrapper around hal.i2c_mgr->get_device() that prevents duplicate devices being opened
*/
bool Compass::_have_driver(AP_HAL::Device::BusType bus_type, uint8_t bus_num, uint8_t address, uint8_t devtype) const
bool Compass::_have_i2c_driver(uint8_t bus, uint8_t address) const
{
uint32_t id = AP_HAL::Device::make_bus_id(bus_type, bus_num, address, devtype);
for (uint8_t i=0; i<_compass_count; i++) {
if (id == uint32_t(_state[i].dev_id.get())) {
if (AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_I2C, bus, address, 0) ==
AP_HAL::Device::change_bus_id(uint32_t(_state[i].dev_id.get()), 0)) {
// we are already using this device
return true;
}
}
return false;
}
/*
macro to add a backend with check for too many backends or compass
instances. We don't try to start more than the maximum allowed
*/
#define ADD_BACKEND(driver_type, backend, name, external) \
do { if (_driver_enabled(driver_type)) { _add_backend(backend, name, external); } \
if (_backend_count == COMPASS_MAX_BACKEND || \
_compass_count == COMPASS_MAX_INSTANCES) { \
return; \
} \
} while (0)
#define GET_I2C_DEVICE(bus, address) _have_i2c_driver(bus, address)?nullptr:hal.i2c_mgr->get_device(bus, address)
/*
look for compasses on external i2c buses
*/
void Compass::_probe_external_i2c_compasses(void)
{
bool both_i2c_external = (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2);
// external i2c bus
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(i, HAL_COMPASS_HMC5843_I2C_ADDR),
true, ROTATION_ROLL_180),
AP_Compass_HMC5843::name, true);
}
if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_MINDPXV2) {
// internal i2c bus
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(i, HAL_COMPASS_HMC5843_I2C_ADDR),
both_i2c_external, both_i2c_external?ROTATION_ROLL_180:ROTATION_YAW_270),
AP_Compass_HMC5843::name, both_i2c_external);
}
}
//external i2c bus
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_QMC5883, AP_Compass_QMC5883L::probe(*this, GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883L_I2C_ADDR),
true, HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL),
AP_Compass_QMC5883L::name, true);
}
//internal i2c bus
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_QMC5883, AP_Compass_QMC5883L::probe(*this, GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883L_I2C_ADDR),
both_i2c_external,
both_i2c_external?HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL:HAL_COMPASS_QMC5883L_ORIENTATION_INTERNAL),
AP_Compass_QMC5883L::name,both_i2c_external);
}
#if !HAL_MINIMIZE_FEATURES
// AK09916 on ICM20948
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(*this,
GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR),
GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR),
true, ROTATION_PITCH_180_YAW_90),
AP_Compass_AK09916::name, true);
}
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(*this,
GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR),
GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR),
both_i2c_external, ROTATION_PITCH_180_YAW_90),
AP_Compass_AK09916::name, true);
}
// lis3mdl on bus 0 with default address
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR),
both_i2c_external, both_i2c_external?ROTATION_YAW_90:ROTATION_NONE),
AP_Compass_LIS3MDL::name, both_i2c_external);
}
// lis3mdl on bus 0 with alternate address
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR2),
both_i2c_external, both_i2c_external?ROTATION_YAW_90:ROTATION_NONE),
AP_Compass_LIS3MDL::name, both_i2c_external);
}
// external lis3mdl on bus 1 with default address
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR),
true, ROTATION_YAW_90),
AP_Compass_LIS3MDL::name, true);
}
// external lis3mdl on bus 1 with alternate address
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR2),
true, ROTATION_YAW_90),
AP_Compass_LIS3MDL::name, true);
}
// 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) {
ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(*this, GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR),
true, ROTATION_YAW_270),
AP_Compass_AK09916::name, true);
}
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(*this, GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR),
both_i2c_external, both_i2c_external?ROTATION_YAW_270:ROTATION_NONE),
AP_Compass_AK09916::name, both_i2c_external);
}
// IST8310 on external and internal bus
if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_FMUV5) {
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, GET_I2C_DEVICE(i, HAL_COMPASS_IST8310_I2C_ADDR),
true, ROTATION_PITCH_180), AP_Compass_IST8310::name, true);
}
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, GET_I2C_DEVICE(i, HAL_COMPASS_IST8310_I2C_ADDR),
both_i2c_external, ROTATION_PITCH_180), AP_Compass_IST8310::name, both_i2c_external);
}
}
#endif // HAL_MINIMIZE_FEATURES
}
/*
detect available backends for this board
*/
@ -553,23 +688,20 @@ void Compass::_detect_backends(void)
}
#endif
/*
macro to add a backend with check for too many backends or compass
instances. We don't try to start more than the maximum allowed
*/
#define ADD_BACKEND(driver_type, backend, name, external) \
do { if (_driver_enabled(driver_type)) { _add_backend(backend, name, external); } \
if (_backend_count == COMPASS_MAX_BACKEND || \
_compass_count == COMPASS_MAX_INSTANCES) { \
return; \
} \
} while (0)
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
ADD_BACKEND(DRIVER_SITL, new AP_Compass_SITL(*this), nullptr, false);
return;
#endif
#ifdef HAL_PROBE_EXTERNAL_I2C_COMPASSES
// allow boards to ask for external probing of all i2c compass types in hwdef.dat
_probe_external_i2c_compasses();
if (_backend_count == COMPASS_MAX_BACKEND ||
_compass_count == COMPASS_MAX_INSTANCES) {
return;
}
#endif
#if HAL_COMPASS_DEFAULT == HAL_COMPASS_HIL
ADD_BACKEND(DRIVER_SITL, AP_Compass_HIL::detect(*this), nullptr, false);
#elif AP_FEATURE_BOARD_DETECT
@ -583,122 +715,35 @@ void Compass::_detect_backends(void)
case AP_BoardConfig::PX4_BOARD_PIXRACER:
case AP_BoardConfig::PX4_BOARD_MINDPXV2:
case AP_BoardConfig::PX4_BOARD_FMUV5:
case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO:{
bool both_i2c_external = (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2);
// external i2c bus
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(i, HAL_COMPASS_HMC5843_I2C_ADDR),
true, ROTATION_ROLL_180),
AP_Compass_HMC5843::name, true);
}
if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_MINDPXV2) {
// internal i2c bus
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_HMC5843_I2C_ADDR),
both_i2c_external, both_i2c_external?ROTATION_ROLL_180:ROTATION_YAW_270),
AP_Compass_HMC5843::name, both_i2c_external);
}
//external i2c bus
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_QMC5883, AP_Compass_QMC5883L::probe(*this, hal.i2c_mgr->get_device(i, HAL_COMPASS_QMC5883L_I2C_ADDR),
true,ROTATION_ROLL_180),
AP_Compass_QMC5883L::name, true);
}
//internal i2c bus
ADD_BACKEND(DRIVER_QMC5883, AP_Compass_QMC5883L::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_QMC5883L_I2C_ADDR),
both_i2c_external, both_i2c_external?ROTATION_ROLL_180:ROTATION_ROLL_180_YAW_270),
AP_Compass_QMC5883L::name,both_i2c_external);
#if !HAL_MINIMIZE_FEATURES
// AK09916 on ICM20948
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(*this,
hal.i2c_mgr->get_device(i, HAL_COMPASS_AK09916_I2C_ADDR),
hal.i2c_mgr->get_device(i, HAL_COMPASS_ICM20948_I2C_ADDR),
true, ROTATION_PITCH_180_YAW_90),
AP_Compass_AK09916::name, true);
}
ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(*this,
hal.i2c_mgr->get_device(0, HAL_COMPASS_AK09916_I2C_ADDR),
hal.i2c_mgr->get_device(0, HAL_COMPASS_ICM20948_I2C_ADDR),
both_i2c_external, ROTATION_PITCH_180_YAW_90),
AP_Compass_AK09916::name, true);
// lis3mdl on bus 0 with default address
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_LIS3MDL_I2C_ADDR),
both_i2c_external, both_i2c_external?ROTATION_YAW_90:ROTATION_NONE),
AP_Compass_LIS3MDL::name, both_i2c_external);
// lis3mdl on bus 0 with alternate address
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_LIS3MDL_I2C_ADDR2),
both_i2c_external, both_i2c_external?ROTATION_YAW_90:ROTATION_NONE),
AP_Compass_LIS3MDL::name, both_i2c_external);
// external lis3mdl on bus 1 with default address
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, hal.i2c_mgr->get_device(i, HAL_COMPASS_LIS3MDL_I2C_ADDR),
true, ROTATION_YAW_90),
AP_Compass_LIS3MDL::name, true);
}
// external lis3mdl on bus 1 with alternate address
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, hal.i2c_mgr->get_device(i, HAL_COMPASS_LIS3MDL_I2C_ADDR2),
true, ROTATION_YAW_90),
AP_Compass_LIS3MDL::name, true);
}
// 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) {
if (!_have_driver(AP_HAL::Device::BUS_TYPE_I2C, i, HAL_COMPASS_AK09916_I2C_ADDR, AP_Compass_Backend::DEVTYPE_ICM20948)) {
ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(*this, hal.i2c_mgr->get_device(i, HAL_COMPASS_AK09916_I2C_ADDR),
true, ROTATION_YAW_270),
AP_Compass_AK09916::name, true);
}
}
if (!_have_driver(AP_HAL::Device::BUS_TYPE_I2C, 0, HAL_COMPASS_AK09916_I2C_ADDR, AP_Compass_Backend::DEVTYPE_ICM20948)) {
ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_AK09916_I2C_ADDR),
both_i2c_external, both_i2c_external?ROTATION_YAW_270:ROTATION_NONE),
AP_Compass_AK09916::name, both_i2c_external);
}
// IST8310 on external and internal bus
if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_FMUV5) {
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(i, HAL_COMPASS_IST8310_I2C_ADDR),
true, ROTATION_PITCH_180), AP_Compass_IST8310::name, true);
}
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_IST8310_I2C_ADDR),
both_i2c_external, ROTATION_PITCH_180), AP_Compass_IST8310::name, both_i2c_external);
}
#endif // HAL_MINIMIZE_FEATURES
case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO:
_probe_external_i2c_compasses();
if (_backend_count == COMPASS_MAX_BACKEND ||
_compass_count == COMPASS_MAX_INSTANCES) {
return;
}
break;
case AP_BoardConfig::PX4_BOARD_PCNC1:
ADD_BACKEND(DRIVER_BMM150,
AP_Compass_BMM150::probe(*this, hal.i2c_mgr->get_device(0, 0x10)),
AP_Compass_BMM150::probe(*this, GET_I2C_DEVICE(0, 0x10)),
AP_Compass_BMM150::name, true);
break;
case AP_BoardConfig::PX4_BOARD_AEROFC:
#ifdef HAL_COMPASS_IST8310_I2C_BUS
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_IST8310_I2C_BUS, HAL_COMPASS_IST8310_I2C_ADDR),
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, GET_I2C_DEVICE(HAL_COMPASS_IST8310_I2C_BUS, HAL_COMPASS_IST8310_I2C_ADDR),
true, ROTATION_PITCH_180_YAW_90), AP_Compass_IST8310::name, true);
#endif
break;
case AP_BoardConfig::VRX_BOARD_BRAIN54: {
// external i2c bus
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_HMC5843_I2C_ADDR),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(1, HAL_COMPASS_HMC5843_I2C_ADDR),
true, ROTATION_ROLL_180),
AP_Compass_HMC5843::name, true);
}
// internal i2c bus
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_HMC5843_I2C_ADDR),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(0, HAL_COMPASS_HMC5843_I2C_ADDR),
false, ROTATION_YAW_270),
AP_Compass_HMC5843::name, false);
break;
@ -710,7 +755,7 @@ void Compass::_detect_backends(void)
case AP_BoardConfig::VRX_BOARD_UBRAIN51:
case AP_BoardConfig::VRX_BOARD_UBRAIN52: {
// external i2c bus
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_HMC5843_I2C_ADDR),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(1, HAL_COMPASS_HMC5843_I2C_ADDR),
true, ROTATION_ROLL_180),
AP_Compass_HMC5843::name, true);
}
@ -739,11 +784,11 @@ void Compass::_detect_backends(void)
case AP_BoardConfig::PX4_BOARD_FMUV5:
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(i, HAL_COMPASS_IST8310_I2C_ADDR),
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, GET_I2C_DEVICE(i, HAL_COMPASS_IST8310_I2C_ADDR),
true, ROTATION_ROLL_180_YAW_90), AP_Compass_IST8310::name, true);
}
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(i, HAL_COMPASS_IST8310_I2C_ADDR),
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, GET_I2C_DEVICE(i, HAL_COMPASS_IST8310_I2C_ADDR),
false, ROTATION_ROLL_180_YAW_90), AP_Compass_IST8310::name, false);
}
break;
@ -789,7 +834,7 @@ void Compass::_detect_backends(void)
break;
case AP_BoardConfig::PX4_BOARD_MINDPXV2:
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_HMC5843_I2C_ADDR),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(0, HAL_COMPASS_HMC5843_I2C_ADDR),
false, ROTATION_YAW_90),
AP_Compass_HMC5843::name, false);
ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), ROTATION_PITCH_180_YAW_270),
@ -801,11 +846,11 @@ void Compass::_detect_backends(void)
}
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BH
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
AP_Compass_HMC5843::name, false);
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0), AP_Compass_AK8963::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BBBMINI
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true);
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false);
@ -816,37 +861,37 @@ void Compass::_detect_backends(void)
AP_Compass_LSM9DS1::name, false);
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NAVIO
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_OCPOC_ZYNQ
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
AP_Compass_HMC5843::name, true);
ADD_BACKEND(DRIVER_AK8963,AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_EDGE
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)),
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, GET_I2C_DEVICE(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_POCKET
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_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(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0),
@ -855,54 +900,54 @@ void Compass::_detect_backends(void)
#ifndef HAL_COMPASS_HMC5843_ROTATION
# define HAL_COMPASS_HMC5843_ROTATION ROTATION_NONE
#endif
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR),
false, HAL_COMPASS_HMC5843_ROTATION),
AP_Compass_HMC5843::name, false);
#if defined(HAL_COMPASS_HMC5843_I2C_EXT_BUS) && CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_EXT_BUS, HAL_COMPASS_HMC5843_I2C_ADDR),true),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(HAL_COMPASS_HMC5843_I2C_EXT_BUS, HAL_COMPASS_HMC5843_I2C_ADDR),true),
AP_Compass_HMC5843::name, true);
#endif
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_HMC5843_MPU6000
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe_mpu6000(*this),
AP_Compass_HMC5843::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_I2C
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)),
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe(*this, GET_I2C_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(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, hal.i2c_mgr->get_device(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)),
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, GET_I2C_DEVICE(HAL_COMPASS_AK8963_I2C_BUS, HAL_COMPASS_AK8963_I2C_ADDR)),
AP_Compass_AK8963::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AERO
ADD_BACKEND(DRIVER_BMM150, AP_Compass_BMM150::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_BMM150_I2C_BUS, HAL_COMPASS_BMM150_I2C_ADDR)),
ADD_BACKEND(DRIVER_BMM150, AP_Compass_BMM150::probe(*this, GET_I2C_DEVICE(HAL_COMPASS_BMM150_I2C_BUS, HAL_COMPASS_BMM150_I2C_ADDR)),
AP_Compass_BMM150::name, false);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR), true),
AP_Compass_HMC5843::name, true);
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_IST8310_I2C_BUS, HAL_COMPASS_IST8310_I2C_ADDR),
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, GET_I2C_DEVICE(HAL_COMPASS_IST8310_I2C_BUS, HAL_COMPASS_IST8310_I2C_ADDR),
true, ROTATION_PITCH_180_YAW_90), AP_Compass_IST8310::name, true);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_LIS3MDL
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, hal.spi->get_device(HAL_COMPASS_LIS3MDL_NAME), false, ROTATION_ROLL_180_YAW_90),
AP_Compass_LIS3MDL::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_MAG3110
ADD_BACKEND(DRIVER_MAG3110, AP_Compass_MAG3110::probe(*this, hal.i2c_mgr->get_device(HAL_MAG3110_I2C_BUS, HAL_MAG3110_I2C_ADDR), ROTATION_NONE),
ADD_BACKEND(DRIVER_MAG3110, AP_Compass_MAG3110::probe(*this, GET_I2C_DEVICE(HAL_MAG3110_I2C_BUS, HAL_MAG3110_I2C_ADDR), ROTATION_NONE),
AP_Compass_MAG3110::name, true);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_IST8310
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_IST8310_I2C_BUS, HAL_COMPASS_IST8310_I2C_ADDR),
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, GET_I2C_DEVICE(HAL_COMPASS_IST8310_I2C_BUS, HAL_COMPASS_IST8310_I2C_ADDR),
true, ROTATION_PITCH_180_YAW_90), AP_Compass_IST8310::name, true);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QMC5883L
ADD_BACKEND(DRIVER_QMC5883, AP_Compass_QMC5883L::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_QMC5883L_I2C_ADDR),
true,ROTATION_ROLL_180),
ADD_BACKEND(DRIVER_QMC5883, AP_Compass_QMC5883L::probe(*this, GET_I2C_DEVICE(1, HAL_COMPASS_QMC5883L_I2C_ADDR),
true, HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL),
AP_Compass_QMC5883L::name, true);
ADD_BACKEND(DRIVER_QMC5883, AP_Compass_QMC5883L::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_QMC5883L_I2C_ADDR),
false,ROTATION_PITCH_180_YAW_270),
ADD_BACKEND(DRIVER_QMC5883, AP_Compass_QMC5883L::probe(*this, GET_I2C_DEVICE(0, HAL_COMPASS_QMC5883L_I2C_ADDR),
false, HAL_COMPASS_QMC5883L_ORIENTATION_INTERNAL),
AP_Compass_QMC5883L::name, false);
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR)),
AP_Compass_HMC5843::name, false);
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(*this, 0),
AP_Compass_AK8963::name, false);
ADD_BACKEND(DRIVER_LSM9DS1, AP_Compass_LSM9DS1::probe(*this, hal.spi->get_device("lsm9ds1_m")),
AP_Compass_LSM9DS1::name, false);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BMM150_I2C
ADD_BACKEND(DRIVER_BMM150, AP_Compass_BMM150::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_BMM150_I2C_BUS, HAL_COMPASS_BMM150_I2C_ADDR)),
ADD_BACKEND(DRIVER_BMM150, AP_Compass_BMM150::probe(*this, GET_I2C_DEVICE(HAL_COMPASS_BMM150_I2C_BUS, HAL_COMPASS_BMM150_I2C_ADDR)),
AP_Compass_BMM150::name, true);
#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_NONE
// no compass
@ -913,43 +958,43 @@ void Compass::_detect_backends(void)
#if defined(BOARD_I2C_BUS_EXT) && CONFIG_HAL_BOARD == HAL_BOARD_F4LIGHT
// autodetect external i2c bus
ADD_BACKEND(DRIVER_QMC5883, AP_Compass_QMC5883L::probe(*this, hal.i2c_mgr->get_device(BOARD_I2C_BUS_EXT, HAL_COMPASS_QMC5883L_I2C_ADDR),
ADD_BACKEND(DRIVER_QMC5883, AP_Compass_QMC5883L::probe(*this, GET_I2C_DEVICE(BOARD_I2C_BUS_EXT, HAL_COMPASS_QMC5883L_I2C_ADDR),
true,ROTATION_NONE),
AP_Compass_QMC5883L::name, true);
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(BOARD_I2C_BUS_EXT, HAL_COMPASS_HMC5843_I2C_ADDR)),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(BOARD_I2C_BUS_EXT, HAL_COMPASS_HMC5843_I2C_ADDR)),
AP_Compass_HMC5843::name, true);
// lis3mdl
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, hal.i2c_mgr->get_device(BOARD_I2C_BUS_EXT, HAL_COMPASS_LIS3MDL_I2C_ADDR),
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(*this, GET_I2C_DEVICE(BOARD_I2C_BUS_EXT, HAL_COMPASS_LIS3MDL_I2C_ADDR),
true, ROTATION_NONE),
AP_Compass_LIS3MDL::name, true);
// AK09916
ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(*this, hal.i2c_mgr->get_device(BOARD_I2C_BUS_EXT, HAL_COMPASS_AK09916_I2C_ADDR),
ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe(*this, GET_I2C_DEVICE(BOARD_I2C_BUS_EXT, HAL_COMPASS_AK09916_I2C_ADDR),
true, ROTATION_NONE),
AP_Compass_AK09916::name, true);
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, hal.i2c_mgr->get_device(BOARD_I2C_BUS_EXT, HAL_COMPASS_IST8310_I2C_ADDR),
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(*this, GET_I2C_DEVICE(BOARD_I2C_BUS_EXT, HAL_COMPASS_IST8310_I2C_ADDR),
ROTATION_NONE),
AP_Compass_IST8310::name, true);
#ifdef HAL_COMPASS_BMM150_I2C_ADDR
ADD_BACKEND(DRIVER_BMM150, AP_Compass_BMM150::probe(*this, hal.i2c_mgr->get_device(BOARD_I2C_BUS_EXT, HAL_COMPASS_BMM150_I2C_ADDR)),
ADD_BACKEND(DRIVER_BMM150, AP_Compass_BMM150::probe(*this, GET_I2C_DEVICE(BOARD_I2C_BUS_EXT, HAL_COMPASS_BMM150_I2C_ADDR)),
AP_Compass_BMM150::name, true);
#endif
ADD_BACKEND(DRIVER_MAG3110, AP_Compass_MAG3110::probe(*this, hal.i2c_mgr->get_device(BOARD_I2C_BUS_EXT, HAL_MAG3110_I2C_ADDR), ROTATION_NONE),
ADD_BACKEND(DRIVER_MAG3110, AP_Compass_MAG3110::probe(*this, GET_I2C_DEVICE(BOARD_I2C_BUS_EXT, HAL_MAG3110_I2C_ADDR), ROTATION_NONE),
AP_Compass_MAG3110::name, true);
ADD_BACKEND(DRIVER_QMC5883, AP_Compass_QMC5883L::probe(*this, hal.i2c_mgr->get_device(BOARD_I2C_BUS_EXT, HAL_COMPASS_QMC5883L_I2C_ADDR), ROTATION_NONE),
ADD_BACKEND(DRIVER_QMC5883, AP_Compass_QMC5883L::probe(*this, GET_I2C_DEVICE(BOARD_I2C_BUS_EXT, HAL_COMPASS_QMC5883L_I2C_ADDR), ROTATION_NONE),
AP_Compass_QMC5883L::name, true);
#endif
/* for chibios external board coniguration */
#ifdef HAL_EXT_COMPASS_HMC5843_I2C_BUS
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_EXT_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR),
ADD_BACKEND(DRIVER_HMC5883, AP_Compass_HMC5843::probe(*this, GET_I2C_DEVICE(HAL_EXT_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR),
true, ROTATION_ROLL_180),
AP_Compass_HMC5843::name, true);
#endif

11
libraries/AP_Compass/AP_Compass.h
View File

@ -140,7 +140,7 @@ public:
void cancel_calibration_all();
bool compass_cal_requires_reboot() { return _cal_complete_requires_reboot; }
bool compass_cal_requires_reboot() const { return _cal_complete_requires_reboot; }
bool is_calibrating() const;
/*
@ -339,6 +339,7 @@ private:
// load backend drivers
bool _add_backend(AP_Compass_Backend *backend, const char *name, bool external);
void _probe_external_i2c_compasses(void);
void _detect_backends(void);
// compass cal
@ -351,9 +352,8 @@ private:
bool _start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot=false);
bool _auto_reboot() { return _compass_cal_autoreboot; }
// see if we already have probed a driver by bus type
bool _have_driver(AP_HAL::Device::BusType bus_type, uint8_t bus_num, uint8_t address, uint8_t devtype) const;
// see if we already have probed a i2c driver by bus number and address
bool _have_i2c_driver(uint8_t bus_num, uint8_t address) const;
//keep track of which calibrators have been saved
bool _cal_saved[COMPASS_MAX_INSTANCES];
@ -417,6 +417,9 @@ private:
// 0 = disabled, 1 = enabled for throttle, 2 = enabled for current
AP_Int8 _motor_comp_type;
// automatic compass orientation on calibration
AP_Int8 _rotate_auto;
// throttle expressed as a percentage from 0 ~ 1.0, used for motor compensation
float _thr;

2
libraries/AP_Compass/AP_Compass_AK09916.cpp
View File

@ -77,7 +77,7 @@ AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(Compass &compass,
bool force_external,
enum Rotation rotation)
{
if (!dev) {
if (!dev || !dev_icm) {
return nullptr;
}
AP_Compass_AK09916 *sensor = new AP_Compass_AK09916(compass, std::move(dev), std::move(dev_icm),

8
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -132,6 +132,14 @@ void AP_Compass_Backend::set_dev_id(uint8_t instance, uint32_t dev_id)
_compass._state[instance].dev_id.set_and_notify(dev_id);
}
/*
save dev_id, used by SITL
*/
void AP_Compass_Backend::save_dev_id(uint8_t instance)
{
_compass._state[instance].dev_id.save();
}
/*
set external for an instance
*/

8
libraries/AP_Compass/AP_Compass_Backend.h
View File

@ -61,8 +61,9 @@ public:
DEVTYPE_IST8310 = 0x0A,
DEVTYPE_ICM20948 = 0x0B,
DEVTYPE_MMC3416 = 0x0C,
DEVTYPE_QMC5883L = 0x0D,
DEVTYPE_MAG3110 = 0x0E,
DEVTYPE_QMC5883L = 0x0D,
DEVTYPE_MAG3110 = 0x0E,
DEVTYPE_SITL = 0x0F,
};
@ -93,6 +94,9 @@ protected:
// set dev_id for an instance
void set_dev_id(uint8_t instance, uint32_t dev_id);
// save dev_id, used by SITL
void save_dev_id(uint8_t instance);
// set external state for an instance
void set_external(uint8_t instance, bool external);

23
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -61,8 +61,14 @@ Compass::_start_calibration(uint8_t i, bool retry, float delay)
// lot noisier
_calibrator[i].set_tolerance(_calibration_threshold*2);
}
if (_rotate_auto) {
enum Rotation r = _state[i].external?(enum Rotation)_state[i].orientation.get():ROTATION_NONE;
if (r != ROTATION_CUSTOM) {
_calibrator[i].set_orientation(r, _state[i].external, _rotate_auto>=2);
}
}
_cal_saved[i] = false;
_calibrator[i].start(retry, delay, get_offsets_max());
_calibrator[i].start(retry, delay, get_offsets_max(), i);
// disable compass learning both for calibration and after completion
_learn.set_and_save(0);
@ -147,6 +153,10 @@ Compass::_accept_calibration(uint8_t i)
set_and_save_diagonals(i,diag);
set_and_save_offdiagonals(i,offdiag);
if (_state[i].external && _rotate_auto >= 2) {
_state[i].orientation.set_and_save_ifchanged(cal.get_orientation());
}
if (!is_calibrating()) {
AP_Notify::events.compass_cal_saved = 1;
}
@ -215,8 +225,9 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan)
}
uint8_t cal_status = _calibrator[compass_id].get_status();
if ((cal_status == COMPASS_CAL_SUCCESS ||
cal_status == COMPASS_CAL_FAILED)) {
if (cal_status == COMPASS_CAL_SUCCESS ||
cal_status == COMPASS_CAL_FAILED ||
cal_status == COMPASS_CAL_BAD_ORIENTATION) {
float fitness = _calibrator[compass_id].get_fitness();
Vector3f ofs, diag, offdiag;
_calibrator[compass_id].get_calibration(ofs, diag, offdiag);
@ -229,7 +240,10 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan)
fitness,
ofs.x, ofs.y, ofs.z,
diag.x, diag.y, diag.z,
offdiag.x, offdiag.y, offdiag.z
offdiag.x, offdiag.y, offdiag.z,
_calibrator[compass_id].get_orientation_confidence(),
_calibrator[compass_id].get_original_orientation(),
_calibrator[compass_id].get_orientation()
);
}
}
@ -243,6 +257,7 @@ Compass::is_calibrating() const
case COMPASS_CAL_NOT_STARTED:
case COMPASS_CAL_SUCCESS:
case COMPASS_CAL_FAILED:
case COMPASS_CAL_BAD_ORIENTATION:
break;
default:
return true;

1
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -168,7 +168,6 @@ bool AP_Compass_HMC5843::init()
}
if (!_check_whoami()) {
hal.console->printf("HMC5843: not a HMC device\n");
goto errout;
}

11
libraries/AP_Compass/AP_Compass_QMC5883L.h
View File

@ -28,6 +28,17 @@
#define HAL_COMPASS_QMC5883L_I2C_ADDR 0x0D
#endif
/*
setup default orientations
*/
#ifndef HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL
#define HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL ROTATION_ROLL_180
#endif
#ifndef HAL_COMPASS_QMC5883L_ORIENTATION_INTERNAL
#define HAL_COMPASS_QMC5883L_ORIENTATION_INTERNAL ROTATION_ROLL_180_YAW_270
#endif
class AP_Compass_QMC5883L : public AP_Compass_Backend
{
public:

68
libraries/AP_Compass/AP_Compass_SITL.cpp
View File

@ -13,12 +13,50 @@ AP_Compass_SITL::AP_Compass_SITL(Compass &compass):
if (_sitl != nullptr) {
_compass._setup_earth_field();
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
// default offsets to correct value
compass.set_offsets(i, _sitl->mag_ofs);
_compass_instance[i] = register_compass();
set_dev_id(_compass_instance[i], AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, i, 0, DEVTYPE_SITL));
// save so the compass always comes up configured in SITL
save_dev_id(_compass_instance[i]);
}
// make first compass external
set_external(_compass_instance[0], true);
hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_Compass_SITL::_timer, void));
}
}
/*
create correction matrix for diagnonals and off-diagonals
*/
void AP_Compass_SITL::_setup_eliptical_correcion(void)
{
Vector3f diag = _sitl->mag_diag.get();
if (diag.is_zero()) {
diag(1,1,1);
}
const Vector3f &diagonals = diag;
const Vector3f &offdiagonals = _sitl->mag_offdiag;
if (diagonals == _last_dia && offdiagonals == _last_odi) {
return;
}
_eliptical_corr = Matrix3f(diagonals.x, offdiagonals.x, offdiagonals.y,
offdiagonals.x, diagonals.y, offdiagonals.z,
offdiagonals.y, offdiagonals.z, diagonals.z);
if (!_eliptical_corr.invert()) {
_eliptical_corr.identity();
}
_last_dia = diag;
_last_odi = offdiagonals;
}
void AP_Compass_SITL::_timer()
{
// TODO: Refactor delay buffer with AP_Baro_SITL.
@ -66,22 +104,33 @@ void AP_Compass_SITL::_timer()
new_mag_data = buffer[best_index].data;
}
_setup_eliptical_correcion();
new_mag_data = _eliptical_corr * new_mag_data;
new_mag_data -= _sitl->mag_ofs.get();
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
rotate_field(new_mag_data, _compass_instance[i]);
publish_raw_field(new_mag_data, _compass_instance[i]);
correct_field(new_mag_data, _compass_instance[i]);
Vector3f f = new_mag_data;
if (i == 0) {
// rotate the first compass, allowing for testing of external compass rotation
f.rotate_inverse((enum Rotation)_sitl->mag_orient.get());
}
rotate_field(f, _compass_instance[i]);
publish_raw_field(f, _compass_instance[i]);
correct_field(f, _compass_instance[i]);
_mag_accum[i] += f;
}
if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return;
}
_mag_accum += new_mag_data;
_accum_count++;
if (_accum_count == 10) {
_mag_accum /= 2;
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
_mag_accum[i] /= 2;
}
_accum_count = 5;
_has_sample = true;
}
@ -96,14 +145,13 @@ void AP_Compass_SITL::read()
return;
}
Vector3f field(_mag_accum);
field /= _accum_count;
_mag_accum.zero();
_accum_count = 0;
for (uint8_t i=0; i<SITL_NUM_COMPASSES; i++) {
Vector3f field(_mag_accum[i]);
field /= _accum_count;
_mag_accum[i].zero();
publish_filtered_field(field, _compass_instance[i]);
}
_accum_count = 0;
_has_sample = false;
_sem->give();

6
libraries/AP_Compass/AP_Compass_SITL.h
View File

@ -35,9 +35,13 @@ private:
bool _has_sample;
uint32_t _last_sample_time;
Vector3f _mag_accum;
Vector3f _mag_accum[SITL_NUM_COMPASSES];
uint32_t _accum_count;
void _setup_eliptical_correcion();
Matrix3f _eliptical_corr;
Vector3f _last_dia;
Vector3f _last_odi;
};
#endif // CONFIG_HAL_BOARD

195
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -60,6 +60,8 @@
#include "CompassCalibrator.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_GeodesicGrid.h>
#include <AP_AHRS/AP_AHRS.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
@ -78,7 +80,8 @@ void CompassCalibrator::clear() {
set_status(COMPASS_CAL_NOT_STARTED);
}
void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max) {
void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx)
{
if(running()) {
return;
}
@ -87,6 +90,7 @@ void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max) {
_retry = retry;
_delay_start_sec = delay;
_start_time_ms = AP_HAL::millis();
_compass_idx = compass_idx;
set_status(COMPASS_CAL_WAITING_TO_START);
}
@ -114,6 +118,7 @@ float CompassCalibrator::get_completion_percent() const {
case COMPASS_CAL_SUCCESS:
return 100.0f;
case COMPASS_CAL_FAILED:
case COMPASS_CAL_BAD_ORIENTATION:
default:
return 0.0f;
};
@ -167,6 +172,7 @@ void CompassCalibrator::new_sample(const Vector3f& sample) {
if(running() && _samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample(sample)) {
update_completion_mask(sample);
_sample_buffer[_samples_collected].set(sample);
_sample_buffer[_samples_collected].att.set_from_ahrs();
_samples_collected++;
}
}
@ -194,7 +200,7 @@ void CompassCalibrator::update(bool &failure) {
}
} else if(_status == COMPASS_CAL_RUNNING_STEP_TWO) {
if (_fit_step >= 35) {
if(fit_acceptable()) {
if(fit_acceptable() && calculate_orientation()) {
set_status(COMPASS_CAL_SUCCESS);
} else {
set_status(COMPASS_CAL_FAILED);
@ -314,6 +320,13 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) {
return true;
case COMPASS_CAL_FAILED:
if (_status == COMPASS_CAL_BAD_ORIENTATION) {
// don't overwrite bad orientation status
return false;
}
FALLTHROUGH;
case COMPASS_CAL_BAD_ORIENTATION:
if(_status == COMPASS_CAL_NOT_STARTED) {
return false;
}
@ -328,7 +341,7 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) {
_sample_buffer = nullptr;
}
_status = COMPASS_CAL_FAILED;
_status = status;
return true;
default:
@ -696,3 +709,179 @@ void CompassCalibrator::CompassSample::set(const Vector3f &in) {
y = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.y);
z = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.z);
}
void CompassCalibrator::AttitudeSample::set_from_ahrs(void) {
const Matrix3f &dcm = AP::ahrs().get_DCM_rotation_body_to_ned();
float roll_rad, pitch_rad, yaw_rad;
dcm.to_euler(&roll_rad, &pitch_rad, &yaw_rad);
roll = constrain_int16(127 * (roll_rad / M_PI), -127, 127);
pitch = constrain_int16(127 * (pitch_rad / M_PI_2), -127, 127);
yaw = constrain_int16(127 * (yaw_rad / M_PI), -127, 127);
}
Matrix3f CompassCalibrator::AttitudeSample::get_rotmat(void) {
float roll_rad, pitch_rad, yaw_rad;
roll_rad = roll * (M_PI / 127);
pitch_rad = pitch * (M_PI_2 / 127);
yaw_rad = yaw * (M_PI / 127);
Matrix3f dcm;
dcm.from_euler(roll_rad, pitch_rad, yaw_rad);
return dcm;
}
/*
calculate the implied earth field for a compass sample and compass
rotation. This is used to check for consistency between
samples.
If the orientation is correct then when rotated the same (or
similar) earth field should be given for all samples.
Note that this earth field uses an arbitrary north reference, so it
may not match the true earth field.
*/
Vector3f CompassCalibrator::calculate_earth_field(CompassSample &sample, enum Rotation r)
{
Vector3f v = sample.get();
// convert the sample back to sensor frame
v.rotate_inverse(_orientation);
// rotate to body frame for this rotation
v.rotate(r);
// apply offsets, rotating them for the orientation we are testing
Vector3f rot_offsets = _params.offset;
rot_offsets.rotate_inverse(_orientation);
rot_offsets.rotate(r);
v += rot_offsets;
// rotate the sample from body frame back to earth frame
Matrix3f rot = sample.att.get_rotmat();
Vector3f efield = rot * v;
// earth field is the mag sample in earth frame
return efield;
}
/*
calculate compass orientation using the attitude estimate associated
with each sample, and fix orientation on external compasses if
the feature is enabled
*/
bool CompassCalibrator::calculate_orientation(void)
{
if (!_check_orientation) {
// we are not checking orientation
return true;
}
float variance[ROTATION_MAX] {};
for (enum Rotation r = ROTATION_NONE; r<ROTATION_MAX; r = (enum Rotation)(r+1)) {
// calculate the average implied earth field across all samples
Vector3f total_ef {};
for (uint32_t i=0; i<_samples_collected; i++) {
Vector3f efield = calculate_earth_field(_sample_buffer[i], r);
total_ef += efield;
}
Vector3f avg_efield = total_ef / _samples_collected;
// now calculate the square error for this rotation against the average earth field
for (uint32_t i=0; i<_samples_collected; i++) {
Vector3f efield = calculate_earth_field(_sample_buffer[i], r);
float err = (efield - avg_efield).length_squared();
// divide by number of samples collected to get the variance
variance[r] += err / _samples_collected;
}
}
// find the rotation with the lowest variance
enum Rotation besti = ROTATION_NONE;
float bestv = variance[0];
for (enum Rotation r = ROTATION_NONE; r<ROTATION_MAX; r = (enum Rotation)(r+1)) {
if (variance[r] < bestv) {
bestv = variance[r];
besti = r;
}
}
// consider this a pass if the best orientation is 2x better
// variance than 2nd best
const float variance_threshold = 2.0;
float second_best = besti==ROTATION_NONE?variance[1]:variance[0];
for (enum Rotation r = ROTATION_NONE; r<ROTATION_MAX; r = (enum Rotation)(r+1)) {
if (r != besti) {
if (variance[r] < second_best) {
second_best = variance[r];
}
}
}
_orientation_confidence = second_best/bestv;
bool pass;
if (besti == _orientation) {
// if the orientation matched then allow for a low threshold
pass = true;
} else {
pass = _orientation_confidence > variance_threshold;
}
if (!pass) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Mag(%u) bad orientation: %u %.1f", _compass_idx, besti, _orientation_confidence);
} else if (besti == _orientation) {
// no orientation change
gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) good orientation: %u %.1f", _compass_idx, besti, _orientation_confidence);
} else if (!_is_external || !_fix_orientation) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Mag(%u) internal bad orientation: %u %.1f", _compass_idx, besti, _orientation_confidence);
} else {
gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) new orientation: %u was %u %.1f", _compass_idx, besti, _orientation, _orientation_confidence);
}
if (!pass) {
set_status(COMPASS_CAL_BAD_ORIENTATION);
return false;
}
if (_orientation == besti) {
// no orientation change
return true;
}
if (!_is_external || !_fix_orientation) {
// we won't change the orientation, but we set _orientation
// for reporting purposes
_orientation = besti;
set_status(COMPASS_CAL_BAD_ORIENTATION);
return false;
}
// correct the offsets for the new orientation
Vector3f rot_offsets = _params.offset;
rot_offsets.rotate_inverse(_orientation);
rot_offsets.rotate(besti);
_params.offset = rot_offsets;
// rotate the samples for the new orientation
for (uint32_t i=0; i<_samples_collected; i++) {
Vector3f s = _sample_buffer[i].get();
s.rotate_inverse(_orientation);
s.rotate(besti);
_sample_buffer[i].set(s);
}
_orientation = besti;
// re-run the fit to get the diagonals and off-diagonals for the
// new orientation
initialize_fit();
run_sphere_fit();
run_ellipsoid_fit();
return fit_acceptable();
}

41
libraries/AP_Compass/CompassCalibrator.h
View File

@ -1,3 +1,5 @@
#pragma once
#include <AP_Math/AP_Math.h>
#define COMPASS_CAL_NUM_SPHERE_PARAMS 4
@ -13,7 +15,8 @@ enum compass_cal_status_t {
COMPASS_CAL_RUNNING_STEP_ONE=2,
COMPASS_CAL_RUNNING_STEP_TWO=3,
COMPASS_CAL_SUCCESS=4,
COMPASS_CAL_FAILED=5
COMPASS_CAL_FAILED=5,
COMPASS_CAL_BAD_ORIENTATION=6,
};
class CompassCalibrator {
@ -22,7 +25,7 @@ public:
CompassCalibrator();
void start(bool retry, float delay, uint16_t offset_max);
void start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx);
void clear();
void update(bool &failure);
@ -32,14 +35,25 @@ public:
bool running() const;
void set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation) {
_check_orientation = true;
_orientation = orientation;
_orig_orientation = orientation;
_is_external = is_external;
_fix_orientation = fix_orientation;
}
void set_tolerance(float tolerance) { _tolerance = tolerance; }
void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals);
enum Rotation get_orientation(void) { return _orientation; }
enum Rotation get_original_orientation(void) { return _orig_orientation; }
float get_completion_percent() const;
completion_mask_t& get_completion_mask();
enum compass_cal_status_t get_status() const { return _status; }
float get_fitness() const { return sqrtf(_fitness); }
float get_orientation_confidence() const { return _orientation_confidence; }
uint8_t get_attempt() const { return _attempt; }
private:
@ -59,17 +73,34 @@ private:
Vector3f offdiag;
};
// compact class for approximate attitude, to save memory
class AttitudeSample {
public:
Matrix3f get_rotmat();
void set_from_ahrs();
private:
int8_t roll;
int8_t pitch;
int8_t yaw;
};
class CompassSample {
public:
Vector3f get() const;
void set(const Vector3f &in);
AttitudeSample att;
private:
int16_t x;
int16_t y;
int16_t z;
};
enum Rotation _orientation;
enum Rotation _orig_orientation;
bool _is_external;
bool _check_orientation;
bool _fix_orientation;
uint8_t _compass_idx;
enum compass_cal_status_t _status;
@ -96,6 +127,7 @@ private:
float _ellipsoid_lambda;
uint16_t _samples_collected;
uint16_t _samples_thinned;
float _orientation_confidence;
bool set_status(compass_cal_status_t status);
@ -136,4 +168,7 @@ private:
* Reset and update #_completion_mask with the current samples.
*/
void update_completion_mask();
Vector3f calculate_earth_field(CompassSample &sample, enum Rotation r);
bool calculate_orientation();
};