ArduPilot开源飞控之AP_Compass
- 1. 源由
- 2. 框架设计
- 2.1 启动代码
- 2.2 任务代码
- 3. 重要例程
- 3.1 init
- 3.2 read
- 3.3 _detect_backends
- 3.4 _probe_external_i2c_compasses
- 4. 总结
- 5. 参考资料
1. 源由
磁力计应该不是一个非常复杂的传感器，但是确实是一个非常重要的感知机头（飞行）方向的部件。
在实际应用过程中需要考虑霍尔的物理原理。因此，该传感 器件周边要减少电磁场，以减小对罗盘的影响。
主要方法：
- 避开RF信号发射源
- 避开大电流线缆
- 尽量原理EMC发射源
- 供电电源尽量减少纹波信号
- 适当的线缆屏蔽有一定效果
APM在校准磁力计有时确实存在一定的难度，始终无法得到好的效果，可以借鉴：ArduPilot开源飞控之磁力计校准
2. 框架设计
2.1 启动代码
Copter::init_ardupilot
└──> Compass::init
└──> Compass::_detect_backends
└──> Compass::_probe_external_i2c_compasses
2.2 任务代码
FAST_TASK(read_AHRS)
└──> Copter::read_AHRS
└──> AP_AHRS::update
└──> AP_AHRS::update_DCM
└──> AP_AHRS_DCM::update
└──> AP_AHRS_DCM::drift_correction
└──> AP_AHRS_DCM::drift_correction_yaw
└──> Compass::read
3. 重要例程
3.1 init
Ardupilot关于芯片方向的配置有以下几种：
// these rotations form a full set - every rotation in the following
// list when combined with another in the list forms an entry which is
// also in the list. This is an important property. Please run the
// rotations test suite if you add to the list.
// NOTE!! these rotation values are stored to EEPROM, so be careful not to
// change the numbering of any existing entry when adding a new entry.
enum Rotation : uint8_t {
ROTATION_NONE = 0,
ROTATION_YAW_45 = 1,
ROTATION_YAW_90 = 2,
ROTATION_YAW_135 = 3,
ROTATION_YAW_180 = 4,
ROTATION_YAW_225 = 5,
ROTATION_YAW_270 = 6,
ROTATION_YAW_315 = 7,
ROTATION_ROLL_180 = 8,
ROTATION_ROLL_180_YAW_45 = 9,
ROTATION_ROLL_180_YAW_90 = 10,
ROTATION_ROLL_180_YAW_135 = 11,
ROTATION_PITCH_180 = 12,
ROTATION_ROLL_180_YAW_225 = 13,
ROTATION_ROLL_180_YAW_270 = 14,
ROTATION_ROLL_180_YAW_315 = 15,
ROTATION_ROLL_90 = 16,
ROTATION_ROLL_90_YAW_45 = 17,
ROTATION_ROLL_90_YAW_90 = 18,
ROTATION_ROLL_90_YAW_135 = 19,
ROTATION_ROLL_270 = 20,
ROTATION_ROLL_270_YAW_45 = 21,
ROTATION_ROLL_270_YAW_90 = 22,
ROTATION_ROLL_270_YAW_135 = 23,
ROTATION_PITCH_90 = 24,
ROTATION_PITCH_270 = 25,
ROTATION_PITCH_180_YAW_90 = 26, // same as ROTATION_ROLL_180_YAW_270
ROTATION_PITCH_180_YAW_270 = 27, // same as ROTATION_ROLL_180_YAW_90
ROTATION_ROLL_90_PITCH_90 = 28,
ROTATION_ROLL_180_PITCH_90 = 29,
ROTATION_ROLL_270_PITCH_90 = 30,
ROTATION_ROLL_90_PITCH_180 = 31,
ROTATION_ROLL_270_PITCH_180 = 32,
ROTATION_ROLL_90_PITCH_270 = 33,
ROTATION_ROLL_180_PITCH_270 = 34,
ROTATION_ROLL_270_PITCH_270 = 35,
ROTATION_ROLL_90_PITCH_180_YAW_90 = 36,
ROTATION_ROLL_90_YAW_270 = 37,
ROTATION_ROLL_90_PITCH_68_YAW_293 = 38, // this is actually, roll 90, pitch 68.8, yaw 293.3
ROTATION_PITCH_315 = 39,
ROTATION_ROLL_90_PITCH_315 = 40,
ROTATION_PITCH_7 = 41,
ROTATION_ROLL_45 = 42,
ROTATION_ROLL_315 = 43,
///
// Do not add more rotations without checking that there is not a conflict
// with the MAVLink spec. MAV_SENSOR_ORIENTATION is expected to match our
// list of rotations here. If a new rotation is added it needs to be added
// to the MAVLink messages as well.
///
ROTATION_MAX,
ROTATION_CUSTOM_OLD = 100,
ROTATION_CUSTOM_1 = 101,
ROTATION_CUSTOM_2 = 102,
ROTATION_CUSTOM_END,
};
整个初始化过程涉及: 芯片方向，主芯片选择，驱动初始化等。
Compass::init
│
│ /********************************************************************************
│ * Pre-check: Ability Configuration *
│ ********************************************************************************/
├──> <!_enabled>
│ └──> return;
│
│ /********************************************************************************
│ * convert to new custom rotation method. *
│ ********************************************************************************/
├──> <!APM_BUILD_TYPE(APM_BUILD_AP_Periph)> <for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++)>
│ ├──> <_state[i].orientation != ROTATION_CUSTOM_OLD>
│ │ └──> continue;
│ │
│ │ // ROTATION_CUSTOM_2 for compass, ROTATION_CUSTOM_1 for board orientation
│ ├──> _state[i].orientation.set_and_save(ROTATION_CUSTOM_2);
│ │
│ │ // Get rotation configuration and custom rotation convert
│ ├──> AP_Param::ConversionInfo info;
│ ├──> <AP_Param::find_top_level_key_by_pointer(this, info.old_key)>
│ │ ├──> info.type = AP_PARAM_FLOAT;
│ │ ├──> float rpy[3] = {};
│ │ ├──> AP_Float rpy_param;
│ │ ├──> <for (info.old_group_element=49; info.old_group_element<=51; info.old_group_element++)>
│ │ │ └──> <AP_Param::find_old_parameter(&info, &rpy_param)>
│ │ │ └──> rpy[info.old_group_element-49] = rpy_param.get();
│ │ └──> AP::custom_rotations().convert(ROTATION_CUSTOM_2, rpy[0], rpy[1], rpy[2]);
│ └──> break;
│
│ /********************************************************************************
│ * select primary compass for multi-compass *
│ ********************************************************************************/
├──> <COMPASS_MAX_INSTANCES > 1>
│ │
│ │ // Look if there was a primary compass setup in previous version
│ │ // if so and the primary compass is not set in current setup
│ │ // make the devid as primary.
│ ├──> <_priority_did_stored_list[Priority(0)] == 0>
│ │ ├──> uint16_t k_param_compass;
│ │ └──> <AP_Param::find_top_level_key_by_pointer(this, k_param_compass)>
│ │ ├──> const AP_Param::ConversionInfo primary_compass_old_param = {k_param_compass, 12, AP_PARAM_INT8, ""};
│ │ ├──> AP_Int8 value;
│ │ ├──> value.set(0);
│ │ ├──> bool primary_param_exists = AP_Param::find_old_parameter(&primary_compass_old_param, &value);
│ │ ├──> int8_t oldvalue = value.get();
│ │ └──> <(oldvalue!=0) && (oldvalue<COMPASS_MAX_INSTANCES) && primary_param_exists>
│ │ └──> _priority_did_stored_list[Priority(0)].set_and_save_ifchanged(_state[StateIndex(oldvalue)].dev_id);
│ │
│ │ // Load priority list from storage, the changes to priority list
│ │ // by user only take effect post reboot, after this
│ └──> <for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++)>
│ ├──> <_priority_did_stored_list[i] != 0>
│ │ └──> _priority_did_list[i] = _priority_did_stored_list[i];
│ └──> < else > <for (Priority j(i+1); j<COMPASS_MAX_INSTANCES; j++)> // Maintain a list without gaps and duplicates
│ ├──> int32_t temp;
│ ├──> if (_priority_did_stored_list[j] == _priority_did_stored_list[i]) {
│ │ └──> __priority_did_stored_list[j].set_and_save_ifchanged(0);
│ ├──> if (_priority_did_stored_list[j] == 0) {
│ │ └──> _continue;
│ ├──> temp = _priority_did_stored_list[j];
│ ├──> _priority_did_stored_list[j].set_and_save_ifchanged(0);
│ ├──> _priority_did_list[i] = temp;
│ ├──> _priority_did_stored_list[i].set_and_save_ifchanged(temp);
│ └──> break;
│
│ // cache expected dev ids for use during runtime detection
├──> <for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++) >
│ └──> _state[i].expected_dev_id = _state[i].dev_id;
├──> <COMPASS_MAX_UNREG_DEV>
│ │ // set the dev_id to 0 for undetected compasses. extra_dev_id is just an
│ │ // interface for users to see unreg compasses, we actually never store it
│ │ // in storage.
│ └──> <for (uint8_t i=_unreg_compass_count; i<COMPASS_MAX_UNREG_DEV; i++) >
│ │ // cache the extra devices detected in last boot
│ │ // for detecting replacement mag
│ ├──> _previously_unreg_mag[i] = extra_dev_id[i];
│ └──> extra_dev_id[i].set_and_save(0);
│
├──> <COMPASS_MAX_INSTANCES > 1
│ │ // This method calls set_and_save_ifchanged on parameters
│ │ // which are set() but not saved() during normal runtime,
│ │ // do not move this call without ensuring that is not happening
│ │ // read comments under set_and_save_ifchanged for details
│ └──> _reorder_compass_params();
│
│ /********************************************************************************
│ * Detect sensors *
│ ********************************************************************************/
├──> <_compass_count == 0> // detect available backends. Only called once
│ └──> _detect_backends();
│
├──> <COMPASS_MAX_UNREG_DEV>
│ │ // We store the list of unregistered mags detected here,
│ │ // We don't do this during runtime, as we don't want to detect
│ │ // compasses connected by user as a replacement while the system
│ │ // is running
│ └──> <for (uint8_t i=0; i<COMPASS_MAX_UNREG_DEV; i++)>
│ └──> extra_dev_id[i].save();
│
├──> <_compass_count != 0> // get initial health status
│ ├──> hal.scheduler->delay(100);
│ └──> read();
│
│ // set the dev_id to 0 for undetected compasses, to make it easier
│ // for users to see how many compasses are detected. We don't do a
│ // set_and_save() as the user may have temporarily removed the
│ // compass, and we don't want to force a re-cal if they plug it
│ // back in again
├──> <for (StateIndex i(0); i<COMPASS_MAX_INSTANCES; i++)> <!_state[i].registered>
│ └──> _state[i].dev_id.set(0);
│
├──> <HAL_BUILD_AP_PERIPH> // updating the AHRS orientation updates our own orientation:
│ └──> _AP::ahrs().update_orientation();
└──> init_done = true;
3.2 read
Compass::read
│
│ /********************************************************************************
│ * Pre-check: Availability *
│ ********************************************************************************/
├──> <!available()>
│ └──> return false;
│
├──> <HAL_LOGGING_ENABLED>
│ └──> const bool old_healthy = healthy();
│
├──> <HAL_BUILD_AP_PERIPH> <!_initial_location_set>
│ └──> try_set_initial_location();
├──> _detect_runtime();
│
│ /********************************************************************************
│ * Compass data read *
│ ********************************************************************************/
├──> <for (uint8_t i=0; i< _backend_count; i++)>
│ └──> _backends[i]->read(); // call read on each of the backend. This call updates field[i]
│
│ // health status update
├──> uint32_t time = AP_HAL::millis();
├──> bool any_healthy = false;
├──> <for (StateIndex i(0); i < COMPASS_MAX_INSTANCES; i++)>
│ ├──> _state[i].healthy = (time - _state[i].last_update_ms < 500);
│ └──> any_healthy |= _state[i].healthy;
│
│ // health status update
├──> <COMPASS_LEARN_ENABLED>
│ ├──> <_learn == LEARN_INFLIGHT && !learn_allocated>
│ │ ├──> learn_allocated = true;
│ │ └──> learn = new CompassLearn(*this);
│ └──> <_learn == LEARN_INFLIGHT && learn != nullptr>
│ └──> learn->update();
│
├──> <HAL_LOGGING_ENABLED> <any_healthy && _log_bit != (uint32_t)-1 && AP::logger().should_log(_log_bit)>
│ └──> AP::logger().Write_Compass();
│
│ // Set _first_usable parameter
├──> <for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++)>
│ └──> <_use_for_yaw[i]>
│ ├──> _first_usable = uint8_t(i);
│ └──> break;
├──> const bool new_healthy = healthy();
│
├──> <HAL_LOGGING_ENABLED>
│ ├──> #define MASK_LOG_ANY 0xFFFF
│ └──> <new_healthy != old_healthy> <AP::logger().should_log(MASK_LOG_ANY)>
│ ├──> const LogErrorCode code = new_healthy ? LogErrorCode::ERROR_RESOLVED : LogErrorCode::UNHEALTHY;
│ └──> AP::logger().Write_Error(LogErrorSubsystem::COMPASS, code);
└──> return new_healthy;
3.3 _detect_backends
支持类型：
- AP_Compass_ExternalAHRS
- AP_Compass_SITL
- AP_Compass_MSP
- AP_Compass_BMM150
- AP_Compass_HMC5843
- AP_Compass_LSM303D
- AP_Compass_AK8963
- AP_Compass_AK09916
- AP_Compass_IST8310
- AP_Compass_LIS3MDL
- AP_Compass_DroneCAN
/*
detect available backends for this board
*/
void Compass::_detect_backends(void)
{
#if AP_COMPASS_EXTERNALAHRS_ENABLED
const int8_t serial_port = AP::externalAHRS().get_port(AP_ExternalAHRS::AvailableSensor::COMPASS);
if (serial_port >= 0) {
ADD_BACKEND(DRIVER_EXTERNALAHRS, new AP_Compass_ExternalAHRS(serial_port));
}
#endif
#if AP_FEATURE_BOARD_DETECT
if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2) {
// default to disabling LIS3MDL on pixhawk2 due to hardware issue
#if AP_COMPASS_LIS3MDL_ENABLED
_driver_type_mask.set_default(1U<<DRIVER_LIS3MDL);
#endif
}
#endif
#if AP_COMPASS_SITL_ENABLED && !AP_TEST_DRONECAN_DRIVERS
ADD_BACKEND(DRIVER_SITL, new AP_Compass_SITL());
#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();
CHECK_UNREG_LIMIT_RETURN;
#endif
#if AP_COMPASS_MSP_ENABLED
for (uint8_t i=0; i<8; i++) {
if (msp_instance_mask & (1U<<i)) {
ADD_BACKEND(DRIVER_MSP, new AP_Compass_MSP(i));
}
}
#endif
#if defined(HAL_MAG_PROBE_LIST)
// driver probes defined by COMPASS lines in hwdef.dat
HAL_MAG_PROBE_LIST;
#elif AP_FEATURE_BOARD_DETECT
switch (AP_BoardConfig::get_board_type()) {
case AP_BoardConfig::PX4_BOARD_PX4V1:
case AP_BoardConfig::PX4_BOARD_PIXHAWK:
case AP_BoardConfig::PX4_BOARD_PHMINI:
case AP_BoardConfig::PX4_BOARD_AUAV21:
case AP_BoardConfig::PX4_BOARD_PH2SLIM:
case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
case AP_BoardConfig::PX4_BOARD_MINDPXV2:
case AP_BoardConfig::PX4_BOARD_FMUV5:
case AP_BoardConfig::PX4_BOARD_FMUV6:
case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO:
case AP_BoardConfig::PX4_BOARD_AEROFC:
_probe_external_i2c_compasses();
CHECK_UNREG_LIMIT_RETURN;
break;
case AP_BoardConfig::PX4_BOARD_PCNC1:
#if AP_COMPASS_BMM150_ENABLED
ADD_BACKEND(DRIVER_BMM150,
AP_Compass_BMM150::probe(GET_I2C_DEVICE(0, 0x10), false, ROTATION_NONE));
#endif
break;
case AP_BoardConfig::VRX_BOARD_BRAIN54:
case AP_BoardConfig::VRX_BOARD_BRAIN51: {
#if AP_COMPASS_HMC5843_ENABLED
// external i2c bus
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(1, HAL_COMPASS_HMC5843_I2C_ADDR),
true, ROTATION_ROLL_180));
// internal i2c bus
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(0, HAL_COMPASS_HMC5843_I2C_ADDR),
false, ROTATION_YAW_270));
#endif // AP_COMPASS_HMC5843_ENABLED
}
break;
case AP_BoardConfig::VRX_BOARD_BRAIN52:
case AP_BoardConfig::VRX_BOARD_BRAIN52E:
case AP_BoardConfig::VRX_BOARD_CORE10:
case AP_BoardConfig::VRX_BOARD_UBRAIN51:
case AP_BoardConfig::VRX_BOARD_UBRAIN52: {
#if AP_COMPASS_HMC5843_ENABLED
// external i2c bus
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(1, HAL_COMPASS_HMC5843_I2C_ADDR),
true, ROTATION_ROLL_180));
#endif // AP_COMPASS_HMC5843_ENABLED
}
break;
default:
break;
}
switch (AP_BoardConfig::get_board_type()) {
case AP_BoardConfig::PX4_BOARD_PIXHAWK:
#if AP_COMPASS_HMC5843_ENABLED
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(hal.spi->get_device(HAL_COMPASS_HMC5843_NAME),
false, ROTATION_PITCH_180));
#endif
#if AP_COMPASS_LSM303D_ENABLED
ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), ROTATION_NONE));
#endif
break;
case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
#if AP_COMPASS_LSM303D_ENABLED
ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME), ROTATION_YAW_270));
#endif
#if AP_COMPASS_AK8963_ENABLED
// we run the AK8963 only on the 2nd MPU9250, which leaves the
// first MPU9250 to run without disturbance at high rate
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(1, ROTATION_YAW_270));
#endif
#if AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED
ADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe_ICM20948(0, ROTATION_ROLL_180_YAW_90));
#endif
break;
case AP_BoardConfig::PX4_BOARD_FMUV5:
case AP_BoardConfig::PX4_BOARD_FMUV6:
#if AP_COMPASS_IST8310_ENABLED
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(GET_I2C_DEVICE(i, HAL_COMPASS_IST8310_I2C_ADDR),
true, ROTATION_ROLL_180_YAW_90));
}
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(GET_I2C_DEVICE(i, HAL_COMPASS_IST8310_I2C_ADDR),
false, ROTATION_ROLL_180_YAW_90));
}
#endif // AP_COMPASS_IST8310_ENABLED
break;
case AP_BoardConfig::PX4_BOARD_SP01:
#if AP_COMPASS_AK8963_ENABLED
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(1, ROTATION_NONE));
#endif
break;
case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO:
#if AP_COMPASS_AK8963_ENABLED
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(0, ROTATION_ROLL_180_YAW_90));
#endif
#if AP_COMPASS_LIS3MDL_ENABLED
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(hal.spi->get_device(HAL_COMPASS_LIS3MDL_NAME),
false, ROTATION_NONE));
#endif // AP_COMPASS_LIS3MDL_ENABLED
break;
case AP_BoardConfig::PX4_BOARD_PHMINI:
#if AP_COMPASS_AK8963_ENABLED
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(0, ROTATION_ROLL_180));
#endif
break;
case AP_BoardConfig::PX4_BOARD_AUAV21:
#if AP_COMPASS_AK8963_ENABLED
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(0, ROTATION_ROLL_180_YAW_90));
#endif
break;
case AP_BoardConfig::PX4_BOARD_PH2SLIM:
#if AP_COMPASS_AK8963_ENABLED
ADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(0, ROTATION_YAW_270));
#endif
break;
case AP_BoardConfig::PX4_BOARD_MINDPXV2:
#if AP_COMPASS_HMC5843_ENABLED
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(0, HAL_COMPASS_HMC5843_I2C_ADDR),
false, ROTATION_YAW_90));
#endif
#if AP_COMPASS_LSM303D_ENABLED
ADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), ROTATION_PITCH_180_YAW_270));
#endif
break;
default:
break;
}
#endif
#if AP_COMPASS_DRONECAN_ENABLED
if (_driver_enabled(DRIVER_UAVCAN)) {
for (uint8_t i=0; i<COMPASS_MAX_BACKEND; i++) {
AP_Compass_Backend* _uavcan_backend = AP_Compass_DroneCAN::probe(i);
if (_uavcan_backend) {
_add_backend(_uavcan_backend);
}
#if COMPASS_MAX_UNREG_DEV > 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)) {
// 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<COMPASS_MAX_INSTANCES; i++) {
if (AP_HAL::Device::devid_get_bus_type(_priority_did_list[i]) != AP_HAL::Device::BUS_TYPE_UAVCAN
|| _get_state(i).registered) {
continue;
}
// There's a UAVCAN compass missing
// Let's check if there's a replacement
for (uint8_t j=0; j<COMPASS_MAX_INSTANCES; j++) {
uint32_t detected_devid = AP_Compass_DroneCAN::get_detected_devid(j);
// Check if this is a potential replacement mag
if (!is_replacement_mag(detected_devid)) {
continue;
}
// We have found a replacement mag, let's replace the existing one
// with this by setting the priority to zero and calling uavcan probe
gcs().send_text(MAV_SEVERITY_ALERT, "Mag: Compass #%d with DEVID %lu replaced", uint8_t(i), (unsigned long)_priority_did_list[i]);
_priority_did_stored_list[i].set_and_save(0);
_priority_did_list[i] = 0;
AP_Compass_Backend* _uavcan_backend = AP_Compass_DroneCAN::probe(j);
if (_uavcan_backend) {
_add_backend(_uavcan_backend);
// we also need to remove the id from unreg list
remove_unreg_dev_id(detected_devid);
} else {
// the mag has already been allocated,
// let's begin the replacement
bool found_replacement = false;
for (StateIndex k(0); k<COMPASS_MAX_INSTANCES; k++) {
if ((uint32_t)_state[k].dev_id == detected_devid) {
if (_state[k].priority <= uint8_t(i)) {
// we are already on higher priority
// nothing to do
break;
}
found_replacement = true;
// reset old priority of replacement mag
_priority_did_stored_list[_state[k].priority].set_and_save(0);
_priority_did_list[_state[k].priority] = 0;
// update new priority
_state[k].priority = i;
}
}
if (!found_replacement) {
continue;
}
_priority_did_stored_list[i].set_and_save(detected_devid);
_priority_did_list[i] = detected_devid;
}
}
}
}
#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");
}
}
3.4 _probe_external_i2c_compasses
支持类型：
- AP_Compass_HMC5843
- AP_Compass_QMC5883L
- AP_Compass_AK09916
- AP_Compass_LIS3MDL
- AP_Compass_IST8310
- AP_Compass_IST8308
- AP_Compass_MMC3416
- AP_Compass_RM3100
- AP_Compass_BMM150
/*
look for compasses on external i2c buses
*/
void Compass::_probe_external_i2c_compasses(void)
{
#if !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)
bool all_external = (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2);
(void)all_external; // in case all backends using this are compiled out
#endif
#if AP_COMPASS_HMC5843_ENABLED
// external i2c bus
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(i, HAL_COMPASS_HMC5843_I2C_ADDR),
true, ROTATION_ROLL_180));
}
#if !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)
if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_MINDPXV2 &&
AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_AEROFC) {
// internal i2c bus
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(i, HAL_COMPASS_HMC5843_I2C_ADDR),
all_external, all_external?ROTATION_ROLL_180:ROTATION_YAW_270));
}
}
#endif // !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)
#endif // AP_COMPASS_HMC5843_ENABLED
#if AP_COMPASS_QMC5883L_ENABLED
//external i2c bus
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_QMC5883L, AP_Compass_QMC5883L::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883L_I2C_ADDR),
true, HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL));
}
// internal i2c bus
#if !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)
if (all_external) {
// only probe QMC5883L on internal if we are treating internals as externals
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_QMC5883L, AP_Compass_QMC5883L::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883L_I2C_ADDR),
all_external,
all_external?HAL_COMPASS_QMC5883L_ORIENTATION_EXTERNAL:HAL_COMPASS_QMC5883L_ORIENTATION_INTERNAL));
}
}
#endif
#endif // AP_COMPASS_QMC5883L_ENABLED
#ifndef HAL_BUILD_AP_PERIPH
// AK09916 on ICM20948
#if AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR),
GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR),
true, ROTATION_PITCH_180_YAW_90));
ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR),
GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR2),
true, ROTATION_PITCH_180_YAW_90));
}
#if !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR),
GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR),
all_external, ROTATION_PITCH_180_YAW_90));
ADD_BACKEND(DRIVER_ICM20948, AP_Compass_AK09916::probe_ICM20948(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR),
GET_I2C_DEVICE(i, HAL_COMPASS_ICM20948_I2C_ADDR2),
all_external, ROTATION_PITCH_180_YAW_90));
}
#endif
#endif // AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED
#endif // HAL_BUILD_AP_PERIPH
#if AP_COMPASS_LIS3MDL_ENABLED
// lis3mdl on bus 0 with default address
#if !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR),
all_external, all_external?ROTATION_YAW_90:ROTATION_NONE));
}
// lis3mdl on bus 0 with alternate address
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR2),
all_external, all_external?ROTATION_YAW_90:ROTATION_NONE));
}
#endif
// external lis3mdl on bus 1 with default address
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR),
true, ROTATION_YAW_90));
}
// external lis3mdl on bus 1 with alternate address
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(GET_I2C_DEVICE(i, HAL_COMPASS_LIS3MDL_I2C_ADDR2),
true, ROTATION_YAW_90));
}
#endif // AP_COMPASS_LIS3MDL_ENABLED
#if AP_COMPASS_AK09916_ENABLED
// 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(GET_I2C_DEVICE(i, HAL_COMPASS_AK09916_I2C_ADDR),
true, ROTATION_YAW_270));
}
#if !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)
FOREACH_I2C_INTERNAL(i) {
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));
}
#endif
#endif // AP_COMPASS_AK09916_ENABLED
#if AP_COMPASS_IST8310_ENABLED
// IST8310 on external and internal bus
if (AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_FMUV5 &&
AP_BoardConfig::get_board_type() != AP_BoardConfig::PX4_BOARD_FMUV6) {
enum Rotation default_rotation = AP_COMPASS_IST8310_DEFAULT_ROTATION;
if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_AEROFC) {
default_rotation = ROTATION_PITCH_180_YAW_90;
}
// probe all 4 possible addresses
const uint8_t ist8310_addr[] = { 0x0C, 0x0D, 0x0E, 0x0F };
for (uint8_t a=0; a<ARRAY_SIZE(ist8310_addr); a++) {
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(GET_I2C_DEVICE(i, ist8310_addr[a]),
true, default_rotation));
}
#if !defined(HAL_COMPASS_DISABLE_IST8310_INTERNAL_PROBE) && !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_IST8310, AP_Compass_IST8310::probe(GET_I2C_DEVICE(i, ist8310_addr[a]),
all_external, default_rotation));
}
#endif
}
}
#endif // AP_COMPASS_IST8310_ENABLED
#if AP_COMPASS_IST8308_ENABLED
// external i2c bus
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_IST8308, AP_Compass_IST8308::probe(GET_I2C_DEVICE(i, HAL_COMPASS_IST8308_I2C_ADDR),
true, ROTATION_NONE));
}
#if !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_IST8308, AP_Compass_IST8308::probe(GET_I2C_DEVICE(i, HAL_COMPASS_IST8308_I2C_ADDR),
all_external, ROTATION_NONE));
}
#endif
#endif // AP_COMPASS_IST8308_ENABLED
#if AP_COMPASS_MMC3416_ENABLED
// external i2c bus
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_MMC3416, AP_Compass_MMC3416::probe(GET_I2C_DEVICE(i, HAL_COMPASS_MMC3416_I2C_ADDR),
true, ROTATION_NONE));
}
#if !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)
FOREACH_I2C_INTERNAL(i) {
ADD_BACKEND(DRIVER_MMC3416, AP_Compass_MMC3416::probe(GET_I2C_DEVICE(i, HAL_COMPASS_MMC3416_I2C_ADDR),
all_external, ROTATION_NONE));
}
#endif
#endif // AP_COMPASS_MMC3416_ENABLED
#if AP_COMPASS_RM3100_ENABLED
#ifdef HAL_COMPASS_RM3100_I2C_ADDR
const uint8_t rm3100_addresses[] = { HAL_COMPASS_RM3100_I2C_ADDR };
#else
// RM3100 can be on 4 different addresses
const uint8_t rm3100_addresses[] = { HAL_COMPASS_RM3100_I2C_ADDR1,
HAL_COMPASS_RM3100_I2C_ADDR2,
HAL_COMPASS_RM3100_I2C_ADDR3,
HAL_COMPASS_RM3100_I2C_ADDR4 };
#endif
// external i2c bus
FOREACH_I2C_EXTERNAL(i) {
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]), true, ROTATION_NONE));
}
}
#if !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)
FOREACH_I2C_INTERNAL(i) {
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));
}
}
#endif
#endif // AP_COMPASS_RM3100_ENABLED
#if AP_COMPASS_BMM150_DETECT_BACKENDS_ENABLED
// BMM150 on I2C
FOREACH_I2C_EXTERNAL(i) {
for (uint8_t addr=BMM150_I2C_ADDR_MIN; addr <= BMM150_I2C_ADDR_MAX; addr++) {
ADD_BACKEND(DRIVER_BMM150,
AP_Compass_BMM150::probe(GET_I2C_DEVICE(i, addr), true, ROTATION_NONE));
}
}
#endif // AP_COMPASS_BMM150_ENABLED
}
4. 总结
目前，磁力计支持以下类型：
- AP_Compass_ExternalAHRS
- AP_Compass_SITL
- AP_Compass_MSP
- AP_Compass_DroneCAN
- AP_Compass_BMM150
- AP_Compass_HMC5843
- AP_Compass_LSM303D
- AP_Compass_AK8963
- AP_Compass_AK09916
- AP_Compass_IST8310
- AP_Compass_LIS3MDL
- AP_Compass_QMC5883L
- AP_Compass_IST8308
- AP_Compass_MMC3416
- AP_Compass_RM3100
虽然磁力计在航模的应用上就没有这么讲究，但当远航飞行的时候，如果磁罗盘故障或者校准不准时，将会影响RTL返航。
历史上，由于磁力计的使用，导致偏航的灾难！
苏联两度击落韩国客机，背后是否另有隐情
5. 参考资料
【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计
