ArduPilot开源飞控之AP_Compass

2023-10-20 04:04
文章标签 开源 ardupilot ap 飞控 compass

本文主要是介绍ArduPilot开源飞控之AP_Compass，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

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. 源由

磁力计应该不是一个非常复杂的传感器，但是确实是一个非常重要的感知机头（飞行）方向的部件。

在实际应用过程中需要考虑霍尔的物理原理。因此，该传感 器件周边要减少电磁场，以减小对罗盘的影响。

主要方法：

  1. 避开RF信号发射源
  2. 避开大电流线缆
  3. 尽量原理EMC发射源
  4. 供电电源尽量减少纹波信号
  5. 适当的线缆屏蔽有一定效果

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_270ROTATION_PITCH_180_YAW_270   = 27, // same as ROTATION_ROLL_180_YAW_90ROTATION_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.3ROTATION_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

支持类型：

  1. AP_Compass_ExternalAHRS
  2. AP_Compass_SITL
  3. AP_Compass_MSP
  4. AP_Compass_BMM150
  5. AP_Compass_HMC5843
  6. AP_Compass_LSM303D
  7. AP_Compass_AK8963
  8. AP_Compass_AK09916
  9. AP_Compass_IST8310
  10. AP_Compass_LIS3MDL
  11. AP_Compass_DroneCAN
/*detect available backends for this board*/
void Compass::_detect_backends(void)
{
#if AP_COMPASS_EXTERNALAHRS_ENABLEDconst 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_DETECTif (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_DRIVERSADD_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_ENABLEDfor (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.datHAL_MAG_PROBE_LIST;
#elif AP_FEATURE_BOARD_DETECTswitch (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_ENABLEDADD_BACKEND(DRIVER_BMM150,AP_Compass_BMM150::probe(GET_I2C_DEVICE(0, 0x10), false, ROTATION_NONE));
#endifbreak;case AP_BoardConfig::VRX_BOARD_BRAIN54:case AP_BoardConfig::VRX_BOARD_BRAIN51: {
#if AP_COMPASS_HMC5843_ENABLED// external i2c busADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(GET_I2C_DEVICE(1, HAL_COMPASS_HMC5843_I2C_ADDR),true, ROTATION_ROLL_180));// internal i2c busADD_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 busADD_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_ENABLEDADD_BACKEND(DRIVER_HMC5843, AP_Compass_HMC5843::probe(hal.spi->get_device(HAL_COMPASS_HMC5843_NAME),false, ROTATION_PITCH_180));
#endif
#if AP_COMPASS_LSM303D_ENABLEDADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), ROTATION_NONE));
#endifbreak;case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
#if AP_COMPASS_LSM303D_ENABLEDADD_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 rateADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(1, ROTATION_YAW_270));
#endif
#if AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLEDADD_BACKEND(DRIVER_AK09916, AP_Compass_AK09916::probe_ICM20948(0, ROTATION_ROLL_180_YAW_90));
#endifbreak;case AP_BoardConfig::PX4_BOARD_FMUV5:case AP_BoardConfig::PX4_BOARD_FMUV6:
#if AP_COMPASS_IST8310_ENABLEDFOREACH_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_ENABLEDbreak;case AP_BoardConfig::PX4_BOARD_SP01:
#if AP_COMPASS_AK8963_ENABLEDADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(1, ROTATION_NONE));
#endifbreak;case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO:
#if AP_COMPASS_AK8963_ENABLEDADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(0, ROTATION_ROLL_180_YAW_90));
#endif
#if AP_COMPASS_LIS3MDL_ENABLEDADD_BACKEND(DRIVER_LIS3MDL, AP_Compass_LIS3MDL::probe(hal.spi->get_device(HAL_COMPASS_LIS3MDL_NAME),false, ROTATION_NONE));
#endif  // AP_COMPASS_LIS3MDL_ENABLEDbreak;case AP_BoardConfig::PX4_BOARD_PHMINI:
#if AP_COMPASS_AK8963_ENABLEDADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(0, ROTATION_ROLL_180));
#endifbreak;case AP_BoardConfig::PX4_BOARD_AUAV21:
#if AP_COMPASS_AK8963_ENABLEDADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(0, ROTATION_ROLL_180_YAW_90));
#endifbreak;case AP_BoardConfig::PX4_BOARD_PH2SLIM:
#if AP_COMPASS_AK8963_ENABLEDADD_BACKEND(DRIVER_AK8963, AP_Compass_AK8963::probe_mpu9250(0, ROTATION_YAW_270));
#endifbreak;case AP_BoardConfig::PX4_BOARD_MINDPXV2:
#if AP_COMPASS_HMC5843_ENABLEDADD_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_ENABLEDADD_BACKEND(DRIVER_LSM303D, AP_Compass_LSM303D::probe(hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), ROTATION_PITCH_180_YAW_270));
#endifbreak;default:break;}
#endif#if AP_COMPASS_DRONECAN_ENABLEDif (_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 > 0if (_unreg_compass_count == COMPASS_MAX_UNREG_DEV)  {break;}
#endif}#if COMPASS_MAX_UNREG_DEV > 0if (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 compassfor (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 replacementfor (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 magif (!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 listremove_unreg_dev_id(detected_devid);} else {// the mag has already been allocated,// let's begin the replacementbool 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 dobreak;}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_ENABLEDif (_backend_count == 0 ||_compass_count == 0) {DEV_PRINTF("No Compass backends available\n");}
}

3.4 _probe_external_i2c_compasses

支持类型：

  1. AP_Compass_HMC5843
  2. AP_Compass_QMC5883L
  3. AP_Compass_AK09916
  4. AP_Compass_LIS3MDL
  5. AP_Compass_IST8310
  6. AP_Compass_IST8308
  7. AP_Compass_MMC3416
  8. AP_Compass_RM3100
  9. 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 busFOREACH_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 busFOREACH_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 busFOREACH_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 externalsFOREACH_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_ENABLEDFOREACH_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 addressFOREACH_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 addressFOREACH_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 addressFOREACH_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 thatFOREACH_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 busif (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 addressesconst 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 busFOREACH_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 busFOREACH_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_ADDRconst uint8_t rm3100_addresses[] = { HAL_COMPASS_RM3100_I2C_ADDR };
#else// RM3100 can be on 4 different addressesconst 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 busFOREACH_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 I2CFOREACH_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. 总结

目前，磁力计支持以下类型：

  1. AP_Compass_ExternalAHRS
  2. AP_Compass_SITL
  3. AP_Compass_MSP
  4. AP_Compass_DroneCAN
  5. AP_Compass_BMM150
  6. AP_Compass_HMC5843
  7. AP_Compass_LSM303D
  8. AP_Compass_AK8963
  9. AP_Compass_AK09916
  10. AP_Compass_IST8310
  11. AP_Compass_LIS3MDL
  12. AP_Compass_QMC5883L
  13. AP_Compass_IST8308
  14. AP_Compass_MMC3416
  15. AP_Compass_RM3100

虽然磁力计在航模的应用上就没有这么讲究，但当远航飞行的时候，如果磁罗盘故障或者校准不准时，将会影响RTL返航。

历史上，由于磁力计的使用，导致偏航的灾难！

苏联两度击落韩国客机，背后是否另有隐情

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计

这篇关于ArduPilot开源飞控之AP_Compass的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/244536。

相关文章

阿里开源语音识别SenseVoiceWindows环境部署

阿里开源语音识别SenseVoiceWindows环境部署

SenseVoice介绍 SenseVoice 专注于高精度多语言语音识别、情感辨识和音频事件检测多语言识别： 采用超过 40 万小时数据训练，支持超过 50 种语言，识别效果上优于 Whisper 模型。富文本识别：具备优秀的情感识别，能够在测试数据上达到和超过目前最佳情感识别模型的效果。支持声音事件检测能力，支持音乐、掌声、笑声、哭声、咳嗽、喷嚏等多种常见人机交互事件进行检测。高效推
阅读更多...
金融业开源技术 术语

金融业开源技术 术语

金融业开源技术  术语 1  范围 本文件界定了金融业开源技术的常用术语。 本文件适用于金融业中涉及开源技术的相关标准及规范性文件制定和信息沟通等活动。
阅读更多...
安全管理体系化的智慧油站开源了。

安全管理体系化的智慧油站开源了。

AI视频监控平台简介 AI视频监控平台是一款功能强大且简单易用的实时算法视频监控系统。它的愿景是最底层打通各大芯片厂商相互间的壁垒，省去繁琐重复的适配流程，实现芯片、算法、应用的全流程组合，从而大大减少企业级应用约95%的开发成本。用户只需在界面上进行简单的操作，就可以实现全视频的接入及布控。摄像头管理模块用于多种终端设备、智能设备的接入及管理。平台支持包括摄像头等终端感知设备接入，为整个平台提
阅读更多...
K8S（Kubernetes）开源的容器编排平台安装步骤详解

K8S（Kubernetes）开源的容器编排平台安装步骤详解

K8S（Kubernetes）是一个开源的容器编排平台，用于自动化部署、扩展和管理容器化应用程序。以下是K8S容器编排平台的安装步骤、使用方式及特点的概述： 安装步骤： 安装Docker：K8S需要基于Docker来运行容器化应用程序。首先要在所有节点上安装Docker引擎。 安装Kubernetes Master：在集群中选择一台主机作为Master节点，安装K8S的控制平面组件，如AP
阅读更多...
MiniGPT-3D, 首个高效的3D点云大语言模型，仅需一张RTX3090显卡，训练一天时间，已开源

MiniGPT-3D, 首个高效的3D点云大语言模型，仅需一张RTX3090显卡，训练一天时间，已开源

项目主页：https://tangyuan96.github.io/minigpt_3d_project_page/ 代码：https://github.com/TangYuan96/MiniGPT-3D 论文：https://arxiv.org/pdf/2405.01413 MiniGPT-3D在多个任务上取得了SoTA，被ACM MM2024接收，只拥有47.8M的可训练参数，在一张RTX
阅读更多...
HomeBank：开源免费的个人财务管理软件

HomeBank：开源免费的个人财务管理软件

在个人财务管理领域，找到一个既免费又开源的解决方案并非易事。HomeBank&nbsp;正是这样一个项目，它不仅提供了强大的功能，还拥有一个活跃的社区，不断推动其发展和完善。 开源免费：HomeBank 是一个完全开源的项目，用户可以自由地使用、修改和分发。用户友好的界面：提供直观的图形用户界面，使得非技术用户也能轻松上手。数据导入支持：支持从 Quicken、Microsoft Money
阅读更多...
开源分布式数据库中间件

开源分布式数据库中间件

转自：https://www.csdn.net/article/2015-07-16/2825228 MyCat：开源分布式数据库中间件 为什么需要MyCat？ 虽然云计算时代，传统数据库存在着先天性的弊端，但是NoSQL数据库又无法将其替代。如果传统数据易于扩展，可切分，就可以避免单机（单库）的性能缺陷。 MyCat的目标就是：低成本地将现有的单机数据库和应用平滑迁移到“云”端
阅读更多...
LLM系列 | 38：解读阿里开源语音多模态模型Qwen2-Audio

LLM系列 | 38：解读阿里开源语音多模态模型Qwen2-Audio

引言 模型概述 模型架构 训练方法 性能评估 实战演示 总结 引言 金山挂月窥禅径，沙鸟听经恋法门。 小伙伴们好，我是微信公众号《小窗幽记机器学习》的小编：卖铁观音的小男孩，今天这篇小作文主要是介绍阿里巴巴的语音多模态大模型Qwen2-Audio。近日，阿里巴巴Qwen团队发布了最新的大规模音频-语言模型Qwen2-Audio及其技术报告。该模型在音频理解和多模态交互
阅读更多...
开源Apache服务器安全防护技术精要及实战

开源Apache服务器安全防护技术精要及实战

Apache 服务简介 　　Web服务器也称为WWW服务器或HTTP服务器(HTTPServer)，它是Internet上最常见也是使用最频繁的服务器之一，Web服务器能够为用户提供网页浏览、论坛访问等等服务。 　　由于用户在通过Web浏览器访问信息资源的过程中，无须再关心一些技术性的细节，而且界面非常友好，因而Web在Internet上一推出就得到了爆炸性的发展。现在Web服务器已
阅读更多...
数据集 3DPW-开源户外三维人体建模-姿态估计-人体关键点-人体mesh建模 ＞＞ DataBall

数据集 3DPW-开源户外三维人体建模-姿态估计-人体关键点-人体mesh建模 ＞＞ DataBall

3DPW 3DPW-开源户外三维人体建模数据集-姿态估计-人体关键点-人体mesh建模 开源户外三维人体数据集 @inproceedings{vonMarcard2018, title = {Recovering Accurate 3D Human Pose in The Wild Using IMUs and a Moving Camera}, author = {von Marc
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2