我是靠谱客的博主 落后衬衫，最近开发中收集的这篇文章主要介绍APM_ArduCopter源码解析学习（四）——IMU前言一、system.cpp二、库内传感器配置及前后端三、Copter.cpp，觉得挺不错的，现在分享给大家，希望可以做个参考。
概述
APM_ArduCopter源码解析学习（四）——IMU
- 前言
- 一、system.cpp
-
- 1.1 无人机内部初始化
- 1.2 Copter::init_ardupilot()
- 1.3 Copter::startup_INS_ground()
- 二、库内传感器配置及前后端
-
- backend
- 三、Copter.cpp
前言
感谢博主：https://blog.csdn.net/moumde/article/details/108943516。本文打算来写一下Copter下面IMU的具体驱动，当然对于其他类型的无人机实际上原理也是大同小异的，也可用作参考。在具体开始之前，想先从初始化方式开始，捋清楚了初始化顺序，到后面才不会觉得混乱。如有错误，请务必留言指出。一、system.cpp
上一篇博文已经说过了，在init_ardupilot()内部无人机将完成大部分设备的初始化工作。ArduCopter的init_ardupilot()这个函数还是很好找的，就实现在ardupilot/ArduCopter/system.cpp文件内部。但是在开始讲解这个函数之前，想先把无人机内部的初始化流程先说一下。
1.1 无人机内部初始化
关于无人机类型及其继承方式，详见我的上一篇博文：APM_ArduCopter源码解析学习（三）——无人机类型
继承关系如下：
AP_HAL::HAL::Callbacks
|---- AP_Vechile
|---- Copter
之前已经说过，AP_Vehicle类在公有部分内部声明有两个函数setup()和loop()，并且在对应的.cpp文件中得到实现；在protected部分提供了一个纯虚的接口函数init_ardupilot()，并且未在AP_Vehicle.cpp中对其进行实现（具体查看我的上一篇博文）。而实际上，在AP_Vehicle.cpp中对setup()进行具体实现的时候调用了init_ardupilot()。
Copter类继承自AP_Vehicle类，并且并未对setup()和loop()进行修改或重写。相对应的，其在内部以private方式继承了fast_loop()和init_ardupilot()，fast_loop()的具体实现在第一篇博文里面已经介绍过了，这边主要来说一下init_ardupilot()。具体路径位于Copter.h。
private:
...
void fast_loop() override;
...
void init_ardupilot() override;
- 1
- 2
- 3
- 4
- 5
已知Copter类继承了AP_Vehicle类所实现的函数，那么Copter::setup()函数在运行过程中便会调用到Copter::init_ardupilot()函数，而Copter::init_ardupilot则正好实现在system.cpp内部，由此引出我们本次的主要内容。
1.2 Copter::init_ardupilot()
如下内容是针对于Copter的初始化函数。部分内容已备注。
void Copter::init_ardupilot()
{
#if STATS_ENABLED == ENABLED
// initialise stats module
g2.stats.init();
#endif
BoardConfig.init();
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
can_mgr.init();
#endif
// init cargo gripper
#if GRIPPER_ENABLED == ENABLED
g2.gripper.init();
#endif
#if AC_FENCE == ENABLED
fence.init();
#endif
// init winch
#if WINCH_ENABLED == ENABLED
g2.winch.init();
#endif
// initialise notify system
notify.init();
notify_flight_mode();
// initialise battery monitor
battery.init();
// Init RSSI
rssi.init();
barometer.init();
// setup telem slots with serial ports
gcs().setup_uarts();
#if GENERATOR_ENABLED
generator.init();
#endif
#if OSD_ENABLED == ENABLED
osd.init();
#endif
#if LOGGING_ENABLED == ENABLED
log_init();
#endif
// update motor interlock state
update_using_interlock();
#if FRAME_CONFIG == HELI_FRAME
// trad heli specific initialisation
heli_init();
#endif
#if FRAME_CONFIG == HELI_FRAME
input_manager.set_loop_rate(scheduler.get_loop_rate_hz());
#endif
init_rc_in(); // sets up rc channels from radio
// allocate the motors class
allocate_motors();
// initialise rc channels including setting mode
rc().init();
// sets up motors and output to escs
init_rc_out();
// check if we should enter esc calibration mode
esc_calibration_startup_check();
// motors initialised so parameters can be sent
ap.initialised_params = true;
relay.init();
/*
* setup the 'main loop is dead' check. Note that this relies on
* the RC library being initialised.
*/
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
// Do GPS init
gps.set_log_gps_bit(MASK_LOG_GPS);
gps.init(serial_manager);
AP::compass().set_log_bit(MASK_LOG_COMPASS);
AP::compass().init();
#if OPTFLOW == ENABLED
// make optflow available to AHRS
ahrs.set_optflow(&optflow);
#endif
// init Location class
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
Location::set_terrain(&terrain);
wp_nav->set_terrain(&terrain);
#endif
#if AC_OAPATHPLANNER_ENABLED == ENABLED
g2.oa.init();
#endif
attitude_control->parameter_sanity_check();
pos_control->set_dt(scheduler.get_loop_period_s());
// init the optical flow sensor
init_optflow();
#if HAL_MOUNT_ENABLED
// initialise camera mount
camera_mount.init();
#endif
#if PRECISION_LANDING == ENABLED
// initialise precision landing
init_precland();
#endif
// initialise landing gear position
landinggear.init();
#ifdef USERHOOK_INIT
USERHOOK_INIT
#endif
#if HIL_MODE != HIL_MODE_DISABLED
while (barometer.get_last_update() == 0) {
// the barometer begins updating when we get the first
// HIL_STATE message
gcs().send_text(MAV_SEVERITY_WARNING, "Waiting for first HIL_STATE message");
delay(1000);
}
// set INS to HIL mode
ins.set_hil_mode();
#endif
// read Baro pressure at ground
//-----------------------------
barometer.set_log_baro_bit(MASK_LOG_IMU);
barometer.calibrate();
// initialise rangefinder
init_rangefinder();
// init proximity sensor
init_proximity();
#if BEACON_ENABLED == ENABLED
// init beacons used for non-gps position estimation
g2.beacon.init();
#endif
#if RPM_ENABLED == ENABLED
// initialise AP_RPM library
rpm_sensor.init();
#endif
#if MODE_AUTO_ENABLED == ENABLED
// initialise mission library
mode_auto.mission.init();
#endif
#if MODE_SMARTRTL_ENABLED == ENABLED
// initialize SmartRTL
g2.smart_rtl.init();
#endif
// initialise AP_Logger library
logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&copter, &Copter::Log_Write_Vehicle_Startup_Messages, void));
startup_INS_ground();
#ifdef ENABLE_SCRIPTING
g2.scripting.init();
#endif // ENABLE_SCRIPTING
// set landed flags
set_land_complete(true);
set_land_complete_maybe(true);
// we don't want writes to the serial port to cause us to pause
// mid-flight, so set the serial ports non-blocking once we are
// ready to fly
serial_manager.set_blocking_writes_all(false);
// enable CPU failsafe
failsafe_enable();
ins.set_log_raw_bit(MASK_LOG_IMU_RAW);
// enable output to motors
if (arming.rc_calibration_checks(true)) {
enable_motor_output();
}
// disable safety if requested
BoardConfig.init_safety();
// flag that initialisation has completed
ap.initialised = true;
}
无论如何请在上述程序段中找到下面这段，该函数可进行地面启动过程中需要的所有惯性传感器校准等工作。
startup_INS_ground();
- 1
1.3 Copter::startup_INS_ground()
由此我们跳转到位于同一文件下的该函数内部
//******************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//******************************************************************************
void Copter::startup_INS_ground()
{
// initialise ahrs (may push imu calibration into the mpu6000 if using that device).
ahrs.init();
ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
// Warm up and calibrate gyro offsets
ins.init(scheduler.get_loop_rate_hz());
// reset ahrs including gyro bias
ahrs.reset();
}
注意只有ins.init()这个函数是用于传感器的。
二、库内传感器配置及前后端
跳转到 init() 这个函数里面，其路径是在ardupilotlibrariesAP_InertialSensorAP_InertialSensor.cpp中。在开始研究之前需要说明一下AP_InertialSensor这个库中的相关结构。
对于这个库，官方的描述是：读取陀螺仪和加速度计数据，执行校准并将数据以标准单位（度/秒，米/秒）提供给主代码和其他库。
其中AP_InertialSensor.cpp/.h文件是用于和APM上层进行连接通讯的前端。而AP_InertialSensor_Backend.cpp/.h则是中间层文件，是IMU驱动程序后端类。 每种支持的陀螺/加速度传感器类型需要具有一个派生自此类的对象，简单来说就是用于派生具体IMU传感器类型的基类。诸如AP_InertialSensor_XXX形式的都是内部提供的派生自AP_InertialSensor_Backend类的各种IMU传感器类型，如ADIS1647、BMI055等等，其中AP_InertialSensor_Invensense用于MPU9250、MPU6000等。
再来回看这个init()函数：
void
AP_InertialSensor::init(uint16_t loop_rate)
{
// remember the sample rate
_loop_rate = loop_rate;
_loop_delta_t = 1.0f / loop_rate;
// we don't allow deltat values greater than 10x the normal loop
// time to be exposed outside of INS. Large deltat values can
// cause divergence of state estimators
_loop_delta_t_max = 10 * _loop_delta_t;
if (_gyro_count == 0 && _accel_count == 0) {
_start_backends();
}
// initialise accel scale if need be. This is needed as we can't
// give non-zero default values for vectors in AP_Param
for (uint8_t i=0; i<get_accel_count(); i++) {
if (_accel_scale[i].get().is_zero()) {
_accel_scale[i].set(Vector3f(1,1,1));
}
}
// calibrate gyros unless gyro calibration has been disabled
if (gyro_calibration_timing() != GYRO_CAL_NEVER) {
init_gyro();
}
_sample_period_usec = 1000*1000UL / _loop_rate;
// establish the baseline time between samples
_delta_time = 0;
_next_sample_usec = 0;
_last_sample_usec = 0;
_have_sample = false;
// initialise IMU batch logging
batchsampler.init();
// the center frequency of the harmonic notch is always taken from the calculated value so that it can be updated
// dynamically, the calculated value is always some multiple of the configured center frequency, so start with the
// configured value
_calculated_harmonic_notch_freq_hz[0] = _harmonic_notch_filter.center_freq_hz();
_num_calculated_harmonic_notch_frequencies = 1;
for (uint8_t i=0; i<get_gyro_count(); i++) {
_gyro_harmonic_notch_filter[i].allocate_filters(_harmonic_notch_filter.harmonics(), _harmonic_notch_filter.hasOption(HarmonicNotchFilterParams::Options::DoubleNotch));
// initialise default settings, these will be subsequently changed in AP_InertialSensor_Backend::update_gyro()
_gyro_harmonic_notch_filter[i].init(_gyro_raw_sample_rates[i], _calculated_harmonic_notch_freq_hz[0],
_harmonic_notch_filter.bandwidth_hz(), _harmonic_notch_filter.attenuation_dB());
}
}
其他内容大致看看了解一下，这边需要关注的是_start_backends()和init_gyro()这两个函数。_start_backends()用于配置并且开启后端，而init_gyro()完成陀螺仪的校准工作。
backend
这边重点来讲一下IMU传感器在APM后端的配置过程。关于init_gyro()建议阅读源代码学习。
/*
* Start all backends for gyro and accel measurements. It automatically calls
* detect_backends() if it has not been called already.
*/
void AP_InertialSensor::_start_backends()
{
detect_backends();
for (uint8_t i = 0; i < _backend_count; i++) {
_backends[i]->start();
}
if (_gyro_count == 0 || _accel_count == 0) {
AP_HAL::panic("INS needs at least 1 gyro and 1 accel");
}
// clear IDs for unused sensor instances
for (uint8_t i=get_accel_count(); i<INS_MAX_INSTANCES; i++) {
_accel_id[i].set(0);
}
for (uint8_t i=get_gyro_count(); i<INS_MAX_INSTANCES; i++) {
_gyro_id[i].set(0);
}
}
在_start_backends()中，首先调用detect_backends()函数，该函数的作用就是检测飞控板上可用的IMU传感器。在detect_backends()函数内部，需要注意的是在检测到功能板之后，该函数会对具体的板子类型进行判断，并且将对应的IMU加入到后端，方便前端调用。原始代码中并未直接定义AP_FEATURE_BOARD_DETECT。具体路径在libraries/AP_BoardConfig/AP_BoardConfig.h中。
AP_BoardConfig.h宏定义部分：
#ifndef AP_FEATURE_BOARD_DETECT
#if defined(HAL_CHIBIOS_ARCH_FMUV3) || defined(HAL_CHIBIOS_ARCH_FMUV4) || defined(HAL_CHIBIOS_ARCH_FMUV5) || defined(HAL_CHIBIOS_ARCH_MINDPXV2) || defined(HAL_CHIBIOS_ARCH_FMUV4PRO) || defined(HAL_CHIBIOS_ARCH_BRAINV51) || defined(HAL_CHIBIOS_ARCH_BRAINV52) || defined(HAL_CHIBIOS_ARCH_UBRAINV51) || defined(HAL_CHIBIOS_ARCH_COREV10) || defined(HAL_CHIBIOS_ARCH_BRAINV54)
#define AP_FEATURE_BOARD_DETECT 1
#else
#define AP_FEATURE_BOARD_DETECT 0
#endif
#endif
- 1
- 2
- 3
- 4
- 5
- 6
- 7
detect_backends()：
#elif AP_FEATURE_BOARD_DETECT
switch (AP_BoardConfig::get_board_type()) {
case AP_BoardConfig::PX4_BOARD_PX4V1:
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_NONE));
break;
case AP_BoardConfig::PX4_BOARD_PIXHAWK:
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180));
ADD_BACKEND(AP_InertialSensor_LSM9DS0::probe(*this,
hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME),
ROTATION_ROLL_180,
ROTATION_ROLL_180_YAW_270,
ROTATION_PITCH_180));
break;
case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
// older Pixhawk2 boards have the MPU6000 instead of MPU9250
_fast_sampling_mask.set_default(1);
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), ROTATION_PITCH_180));
ADD_BACKEND(AP_InertialSensor_LSM9DS0::probe(*this,
hal.spi->get_device(HAL_INS_LSM9DS0_EXT_G_NAME),
hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME),
ROTATION_ROLL_180_YAW_270,
ROTATION_ROLL_180_YAW_90,
ROTATION_ROLL_180_YAW_90));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
// new cubes have ICM20602, ICM20948, ICM20649
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device("icm20602_ext"), ROTATION_ROLL_180_YAW_270));
ADD_BACKEND(AP_InertialSensor_Invensensev2::probe(*this, hal.spi->get_device("icm20948_ext"), ROTATION_PITCH_180));
ADD_BACKEND(AP_InertialSensor_Invensensev2::probe(*this, hal.spi->get_device("icm20948"), ROTATION_YAW_270));
break;
case AP_BoardConfig::PX4_BOARD_FMUV5:
case AP_BoardConfig::PX4_BOARD_FMUV6:
_fast_sampling_mask.set_default(1);
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device("icm20689"), ROTATION_NONE));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device("icm20602"), ROTATION_NONE));
// allow for either BMI055 or BMI088
ADD_BACKEND(AP_InertialSensor_BMI055::probe(*this,
hal.spi->get_device("bmi055_a"),
hal.spi->get_device("bmi055_g"),
ROTATION_ROLL_180_YAW_90));
ADD_BACKEND(AP_InertialSensor_BMI088::probe(*this,
hal.spi->get_device("bmi055_a"),
hal.spi->get_device("bmi055_g"),
ROTATION_ROLL_180_YAW_90));
break;
case AP_BoardConfig::PX4_BOARD_SP01:
_fast_sampling_mask.set_default(1);
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), ROTATION_NONE));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_NONE));
break;
case AP_BoardConfig::PX4_BOARD_PIXHAWK_PRO:
_fast_sampling_mask.set_default(3);
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_NAME), ROTATION_ROLL_180_YAW_90));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180_YAW_90));
break;
case AP_BoardConfig::PX4_BOARD_PHMINI:
// PHMINI uses ICM20608 on the ACCEL_MAG device and a MPU9250 on the old MPU6000 CS line
_fast_sampling_mask.set_default(3);
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_ROLL_180));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180));
break;
case AP_BoardConfig::PX4_BOARD_AUAV21:
// AUAV2.1 uses ICM20608 on the ACCEL_MAG device and a MPU9250 on the old MPU6000 CS line
_fast_sampling_mask.set_default(3);
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_ROLL_180_YAW_90));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180_YAW_90));
break;
case AP_BoardConfig::PX4_BOARD_PH2SLIM:
_fast_sampling_mask.set_default(1);
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
break;
case AP_BoardConfig::PX4_BOARD_AEROFC:
_fast_sampling_mask.set_default(1);
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU6500_NAME), ROTATION_YAW_270));
break;
case AP_BoardConfig::PX4_BOARD_MINDPXV2:
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU6500_NAME), ROTATION_NONE));
ADD_BACKEND(AP_InertialSensor_LSM9DS0::probe(*this,
hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME),
hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME),
ROTATION_YAW_90,
ROTATION_YAW_90,
ROTATION_YAW_90));
break;
case AP_BoardConfig::VRX_BOARD_BRAIN54:
_fast_sampling_mask.set_default(7);
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_YAW_180));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_EXT_NAME), ROTATION_YAW_180));
在代码段内，会对检测到的板子类型加入对应的IMU传感器到后端，以方便前端的调用。以PX4_BOARD_PIXHAWK为例，该case语句里面包含了两个ADD_BACKEND()函数，AP_InertialSensor_Invensense::probe()函数如下。函数接收3个参数，在原来的detect_backends()中，this指代的就是传感器，hal.spi->get_device()获取由spi驱动的IMU设备，ROTATION_ROLL_180顾名思义就是设置为将传感器默认为旋转180°。
AP_InertialSensor_Backend *AP_InertialSensor_Invensense::probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
enum Rotation rotation)
{
if (!dev) {
return nullptr;
}
AP_InertialSensor_Invensense *sensor;
dev->set_read_flag(0x80);
sensor = new AP_InertialSensor_Invensense(imu, std::move(dev), rotation);
if (!sensor || !sensor->_init()) {
delete sensor;
return nullptr;
}
if (sensor->_mpu_type == Invensense_MPU9250) {
sensor->_id = HAL_INS_MPU9250_SPI;
} else if (sensor->_mpu_type == Invensense_MPU6500) {
sensor->_id = HAL_INS_MPU6500;
} else {
sensor->_id = HAL_INS_MPU60XX_SPI;
}
return sensor;
}
AP_InertialSensor_Invensense::probe()函数代码内部，由AP_InertialSensor_Invensense新建了一个sensor对象，并通过sensor->_init()完成对应传感器的初始化工作，sensor->_init()内部会调用_hardware_init()（_hardware_init()建议仔细看一下，这里由于太长就不放进来了）对寄存器进行配置。再对sensor对象的mpu类型进行判断，获取id号，最后返回sensor对象。
20201207增加_hardware_init()的功能介绍bool AP_InertialSensor_Invensense::_hardware_init(void)
{
_dev->get_semaphore()->take_blocking();
// setup for register checking. We check much less often on I2C
// where the cost of the checks is higher
_dev->setup_checked_registers(7, _dev->bus_type() == AP_HAL::Device::BUS_TYPE_I2C?200:20);
// initially run the bus at low speed
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
if (!_check_whoami()) {
_dev->get_semaphore()->give();
return false;
}
// Chip reset
uint8_t tries;
for (tries = 0; tries < 5; tries++) {
_last_stat_user_ctrl = _register_read(MPUREG_USER_CTRL);
/* First disable the master I2C to avoid hanging the slaves on the
* aulixiliar I2C bus - it will be enabled again if the AuxiliaryBus
* is used */
if (_last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN) {
_last_stat_user_ctrl &= ~BIT_USER_CTRL_I2C_MST_EN;
_register_write(MPUREG_USER_CTRL, _last_stat_user_ctrl);
hal.scheduler->delay(10);
}
/* reset device */
_register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
hal.scheduler->delay(100);
/* bus-dependent initialization */
if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
/* Disable I2C bus if SPI selected (Recommended in Datasheet to be
* done just after the device is reset) */
_last_stat_user_ctrl |= BIT_USER_CTRL_I2C_IF_DIS;
_register_write(MPUREG_USER_CTRL, _last_stat_user_ctrl);
}
/* bus-dependent initialization */
if ((_dev->bus_type() == AP_HAL::Device::BUS_TYPE_I2C) && (_mpu_type == Invensense_MPU9250 || _mpu_type == Invensense_ICM20789)) {
/* Enable I2C bypass to access internal device */
_register_write(MPUREG_INT_PIN_CFG, BIT_BYPASS_EN);
}
// Wake up device and select GyroZ clock. Note that the
// Invensense starts up in sleep mode, and it can take some time
// for it to come out of sleep
_register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_ZGYRO);
hal.scheduler->delay(5);
// check it has woken up
if (_register_read(MPUREG_PWR_MGMT_1) == BIT_PWR_MGMT_1_CLK_ZGYRO) {
break;
}
hal.scheduler->delay(10);
if (_data_ready()) {
break;
}
}
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
if (tries == 5) {
hal.console->printf("Failed to boot Invensense 5 timesn");
_dev->get_semaphore()->give();
return false;
}
if (_mpu_type == Invensense_ICM20608 ||
_mpu_type == Invensense_ICM20602 ||
_mpu_type == Invensense_ICM20601) {
// this avoids a sensor bug, see description above
_register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true);
}
_dev->get_semaphore()->give();
return true;
}
上述代码中，可以看出，在进行传感器的配置过程中，通过一个for循环语句实现了多次传感器的配置工作，从而避免配置一次而导致配置不成功，异常报错，无法使用IMU的问题。for语句如下面所示：
// Chip reset
uint8_t tries;
for (tries = 0; tries < 5; tries++) {
_last_stat_user_ctrl = _register_read(MPUREG_USER_CTRL);
/* First disable the master I2C to avoid hanging the slaves on the
* aulixiliar I2C bus - it will be enabled again if the AuxiliaryBus
* is used */
if (_last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN) {
_last_stat_user_ctrl &= ~BIT_USER_CTRL_I2C_MST_EN;
_register_write(MPUREG_USER_CTRL, _last_stat_user_ctrl);
hal.scheduler->delay(10);
}
/* reset device */
_register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
hal.scheduler->delay(100);
/* bus-dependent initialization */
if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
/* Disable I2C bus if SPI selected (Recommended in Datasheet to be
* done just after the device is reset) */
_last_stat_user_ctrl |= BIT_USER_CTRL_I2C_IF_DIS;
_register_write(MPUREG_USER_CTRL, _last_stat_user_ctrl);
}
/* bus-dependent initialization */
if ((_dev->bus_type() == AP_HAL::Device::BUS_TYPE_I2C) && (_mpu_type == Invensense_MPU9250 || _mpu_type == Invensense_ICM20789)) {
/* Enable I2C bypass to access internal device */
_register_write(MPUREG_INT_PIN_CFG, BIT_BYPASS_EN);
}
// Wake up device and select GyroZ clock. Note that the
// Invensense starts up in sleep mode, and it can take some time
// for it to come out of sleep
_register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_ZGYRO);
hal.scheduler->delay(5);
// check it has woken up
if (_register_read(MPUREG_PWR_MGMT_1) == BIT_PWR_MGMT_1_CLK_ZGYRO) {
break;
}
hal.scheduler->delay(10);
if (_data_ready()) {
break;
}
}
针对于原来的语句
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180));
- 1
我们观察ADD_BACKEND()这个宏，具体路径在AP_InertialSensor.cpp中。
/*
use ADD_BACKEND() macro to allow for INS_ENABLE_MASK for enabling/disabling INS backends
*/
#define ADD_BACKEND(x) do {
if (((1U<<probe_count)&enable_mask) && _add_backend(x)) {
found_mask |= (1U<<probe_count);
}
probe_count++;
} while (0)
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
可知最主要的是其中的_add_backend()
bool AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *backend)
{
if (!backend) {
return false;
}
if (_backend_count == INS_MAX_BACKENDS) {
AP_HAL::panic("Too many INS backends");
}
_backends[_backend_count++] = backend;
return true;
}
_add_backend()接收AP_InertialSensor_Backend类型的指针backend，并且将其放入后端对象的数组_backends[]中，方便后续调用。
然后我们再次回到_start_backends()函数（小节开始的那段代码）中，继续向下。可以很清楚地看出，_backends[]数组对于其中的每一个成员调用了start()方法，用于配置并启动所有传感器。
for (uint8_t i = 0; i < _backend_count; i++) {
_backends[i]->start();
}
- 1
- 2
- 3
对于_backends[]数组中的成员，我们刚刚获取到的是AP_InertialSensor_Invensense中的mpu，因此于对应的.cpp文件中查找到对应的函数。这边由于函数内容过长就不全部放上来了，内部具体的实现内容简单说了就是配置一下芯片的参数及量程什么的（不太准确，建议自己去阅读一下）。具体路径为： AP_InertialSensor_Invensense.cpp中void AP_InertialSensor_Invensense::start()函数，然后这个函数最关键的地方就是在最后面：
// start the timer process to read samples, using the fastest rate avilable
_dev->register_periodic_callback(1000000UL / _gyro_backend_rate_hz, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensense::_poll_data, void));
- 1
- 2
这段程序开启了一个新的读取线程，线程运行时间为1000000UL / _gyro_backend_rate_hz，其中_gyro_backend_rate_hz 表示的是the rate we generating samples into the backend for gyros
_gyro_backend_rate_hz = _accel_backend_rate_hz = 1000;
- 1
由此可知该读线程运行时间为1000us，即1ms运行一次，通过_poll_data()函数内部调用的_read_fifo()读取mpu内部的数据，因此我们在Copter.cpp中找不到对于imu读取的相关函数，因为在start()函数内部以及实现。
为了方便理清关系，这边把对应的逻辑表示一下
Copter::init_ardupilot()
|---- startup_INS_ground();
|---- ins.init();
|---- _start_backends();
|----detect_backends(); |----_backends[i]->start();
|----ADD_BACKEND(); |----_dev->register_periodic_callback();
三、Copter.cpp
回到最初的Copter.cpp下面的fast_loop()，里面的ins.update()完成了立即更新INS以获取当前的陀螺仪数据的功能。内部具体代码如下
/*
update gyro and accel values from backends
*/
void AP_InertialSensor::update(void)
{
// during initialisation update() may be called without
// wait_for_sample(), and a wait is implied
wait_for_sample();
if (!_hil_mode) {
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
// mark sensors unhealthy and let update() in each backend
// mark them healthy via _publish_gyro() and
// _publish_accel()
_gyro_healthy[i] = false;
_accel_healthy[i] = false;
_delta_velocity_valid[i] = false;
_delta_angle_valid[i] = false;
}
for (uint8_t i=0; i<_backend_count; i++) {
_backends[i]->update();
}
// clear accumulators
for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
_delta_velocity_acc[i].zero();
_delta_velocity_acc_dt[i] = 0;
_delta_angle_acc[i].zero();
_delta_angle_acc_dt[i] = 0;
}
if (!_startup_error_counts_set) {
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
_accel_startup_error_count[i] = _accel_error_count[i];
_gyro_startup_error_count[i] = _gyro_error_count[i];
}
if (_startup_ms == 0) {
_startup_ms = AP_HAL::millis();
} else if (AP_HAL::millis()-_startup_ms > 2000) {
_startup_error_counts_set = true;
}
}
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_error_count[i] < _accel_startup_error_count[i]) {
_accel_startup_error_count[i] = _accel_error_count[i];
}
if (_gyro_error_count[i] < _gyro_startup_error_count[i]) {
_gyro_startup_error_count[i] = _gyro_error_count[i];
}
}
// adjust health status if a sensor has a non-zero error count
// but another sensor doesn't.
bool have_zero_accel_error_count = false;
bool have_zero_gyro_error_count = false;
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_healthy[i] && _accel_error_count[i] <= _accel_startup_error_count[i]) {
have_zero_accel_error_count = true;
}
if (_gyro_healthy[i] && _gyro_error_count[i] <= _gyro_startup_error_count[i]) {
have_zero_gyro_error_count = true;
}
}
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_gyro_healthy[i] && _gyro_error_count[i] > _gyro_startup_error_count[i] && have_zero_gyro_error_count) {
// we prefer not to use a gyro that has had errors
_gyro_healthy[i] = false;
}
if (_accel_healthy[i] && _accel_error_count[i] > _accel_startup_error_count[i] && have_zero_accel_error_count) {
// we prefer not to use a accel that has had errors
_accel_healthy[i] = false;
}
}
// set primary to first healthy accel and gyro
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_gyro_healthy[i] && _use[i]) {
_primary_gyro = i;
break;
}
}
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (_accel_healthy[i] && _use[i]) {
_primary_accel = i;
break;
}
}
}
_last_update_usec = AP_HAL::micros();
_have_sample = false;
}
注意到内部的后端更新函数
for (uint8_t i=0; i<_backend_count; i++) {
_backends[i]->update();
}
- 1
- 2
- 3
类似于_backends[i]->start()的调用方式，该函数内部如下。
/*
publish any pending data
*/
bool AP_InertialSensor_Invensense::update()
{
update_accel(_accel_instance);
update_gyro(_gyro_instance);
_publish_temperature(_accel_instance, _temp_filtered);
return true;
}
由此可以看到AP_InertialSensor::update()这个函数确实完成了后端和前端的通讯功能，如前所述，AP_InertialSensor.h/.cpp这两份文件主要实现的就是前后端的连接。
目前先写到这，后面有时间再更新
最后
以上就是落后衬衫为你收集整理的APM_ArduCopter源码解析学习（四）——IMU前言一、system.cpp二、库内传感器配置及前后端三、Copter.cpp的全部内容，希望文章能够帮你解决APM_ArduCopter源码解析学习（四）——IMU前言一、system.cpp二、库内传感器配置及前后端三、Copter.cpp所遇到的程序开发问题。
如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。
本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。
发表评论 取消回复