程序员经验分享-hwhale

APM_ArduCopter源码解析学习（四）——IMU

2023-05-16

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() 这个函数里面，其路径是在ardupilot\libraries\AP_InertialSensor\AP_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 times\n");
        _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这两份文件主要实现的就是前后端的连接。

目前先写到这，后面有时间再更新

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

APM

ArduCopter

IMU

源码解析学习

APM_ArduCopter源码解析学习（四）——IMU 的相关文章

  • VINS（七）estimator_node 数据对齐 imu预积分 vision

    首先通过vins estimator mode监听几个Topic xff08 频率2000Hz xff09 xff0c 将imu数据 xff0c feature数据 xff0c raw image数据 xff08 用于回环检测 xff09

  • 开源 apm_使用开源APM软件：InspectIT

    开源 apm 在当今时代 xff0c 软件系统不断变得越来越复杂 同时 xff0c 客户对响应时间和可用性的期望比以往更高 如您所知 xff0c 性能不佳的服务可能会将客户吸引到竞争对手的产品中 因此 xff0c 系统故障和性能不佳通常会对

  • 使用IMU进行状态估计及进阶

    文章目录 前言基本思想一 姿态估计1 1 通过6轴IMU来进行姿态估计的入门级方法1 1 1 通过加速度计计算姿态1 1 2 引入陀螺仪来得到更好的姿态估计 1 2 四元数解算姿态角解析 二 姿态估算与滤波的关系2 1 状态方程和观测方程2

  • 利用Kalibr标定双目相机与IMU

    本文介绍如何利用Kalibr标定工具进行双目相机与IMU的联合标定 主要过程包括以下四步 xff1a 生成标定板标定双目相机标定IMU联合标定 1 生成标定板 使用AprilTag rosrun kalibr kalibr create t

  • Arducopter Yaw角分析

    Arducopter Yaw 现梳理一遍Poshold模式下的yaw的情况 xff1a 首先从 Copter fast loop gt update flight mode gt Copter ModePosHold run span cl

  • 在ROS下Intel RealSense D435i 驱动的安装，避免踩坑，避免缺少imu话题等各种问题（适用于D400系列、SR300和T265跟踪模块等）

    版权声明 本文为博主原创文章 未经博主允许不得转载 https blog csdn net AnChenliang 1002 article details 109454465 目录 背景 方法1 使用apt安装 不建议使用此方法 了解一下

  • 【3】IMU模块：PA-IMU-460 ROS驱动 + 与GNSS时间同步

    一 模块介绍 惯性测量单元 IMU 产品展示 西安精准测控有限责任公司 说明 这是一款国产的IMU模块 之所以选择这个是因为同等精度的产品价格8500元 这个只要2500元 缺点是 担心国产的模块性能不好 参数需要自己标定 二 程序运行 c

  • 再谈IMU数据处理（滤波器）

    本文开始前 xff0c 先回答一个问题 上一篇文章最后提到了卡尔曼滤波器用来做一维数据的数字滤波处理 xff0c 最终的实验结果说 xff1a 该模型下的卡尔曼滤波处理与二阶IIR低通滤波处理效果几乎一致 有网友指出是错误的 xff0c 卡

  • loam中imu消除重力加速度的数学推导

    最近在看loam的源码发现里面有一段关于imu消除重力加速度的源码 xff0c 刚开始看不明白后来终于搞清楚了 xff0c 欢迎大家批评指正 要理解这个问题首先得明白欧拉角到旋转矩阵的变换 先上图 此图描述的是先绕X xff0c 再绕Y x

  • BetaFlight深入传感设计之三：IMU传感模块

    BetaFlight深入传感设计之三 xff1a IMU传感模块 1 HwPreInit HwInit阶段1 1 业务HwPreInit gyroPreInit1 2 业务HwInit gyroInit amp accInit1 2 1 g

  • Omnibus F4V3 Pro飞控，APM飞控显示电池电压电流

    默认时 xff0c Omnibus F4 Pro烧写APM飞控后 xff0c 电池检测器没有设置的 想要屏幕上显示电池电压和电流信息 xff0c 就需要设置一下 设置信息如下 xff1a BATT MONITOR 61 4 然后重启 xff

  • 解：高性能MEMS IMU解决方案-ADXRS290

    origin https ezchina analog com message 34890 对于复杂且高动态惯性配置的MEMS IMU应用 xff0c 评估功能时需要考虑许多属性 在设计周期早期评估这些属性优于追逐开放性成果 xff0c 从

  • realsense d435i标定imu与camera

    realsense d435i标定imu与camera 1 标定目的 realsense d435i包含两个红外相机 红外发射器 RGB相机和IMU四个模块 xff0c 显然四个传感器的空间位置是不同的 xff0c 我们在处理图像和IMU数

  • 利用IMU数据来计算位移

    目标 xff1a 利用IMU测得的加速度信息来计算位移 xff0c 很简单假设是匀加速运动或是匀速运动都可以 xff0c 主要是看问题的背景来具体去确定运动模型 xff0c 这里我就取个简单的匀加速运动模型 学习过程 xff1a 1 了解I

  • APM EKF2 alt source

    主要看NavEKF2 core下面的selectHeightForFusion 函数 首先从应用层读取高度 xff0c 再进行角度补偿 xff0c 分别有三种高度源 xff1a baro rangefinder GPS xff08 可以在地

  • IMU 测量模型和运动学模型

    一 概念 高斯白噪声 测量噪声是AD转换器件引起的外部噪声 xff0c 波动激烈的测量白噪声 随机游走 这里指零偏Bias 随机游走噪声 xff0c 是传感器内部机械 温度等各种物理因素产生的传感器内部误差的综合参数 xff0c 是变化缓慢

  • 一文了解IMU原理、误差模型、标定、惯性传感器选型以及IMU产品调研(含IMU、AHRS、VRU和INS区别)

    在此记录一下测试IMU过程中的其它文章 xff0c 便于以后查看 xff1a IMU的误差标定以及姿态解算ROS下通过USB端口读取摄像头数据 包括笔记本自带摄像头 激光 摄像头 IMU等传感器数据同步方法 message filters

  • Java探针-Java Agent技术-阿里面试题 javaagent 动态字节码修改 skywalking -无侵入探针深入理解

    关注UAV MOF工作原理 同创和dynatrace探针如何重启preload注入的 Docker 动态修改容器中的环境变量 动态修改 java JAVA OPTS linux进程启动拦截判断 0 好像说明白了一些 Java Agent 一

  • IMU+摄像头实现无标记运动捕捉

    惯性传感和计算机视觉的进步为在临床和自然环境中获得精准数据带来了新可能 然而在临床应用时需要仔细地将传感器与身体对齐 这减慢了数据收集过程 随着无标记运动捕捉的发展 研究者们提出了一个新的深度学习模型 利用来自视觉 惯性传感器及其噪声数据来

  • An Introduction for IMU 2 - IMU数据融合与姿态解算

    在上一篇博客中 我们已经介绍了IMU的内部工作原理 以及如何通过Arduino读取MPU6050的数据 虽然可以从DMP直接读取姿态角 但其数据返回的频率相对较低 同时由于DMP库不是开源的 其内部的工作原理 输出姿态角的准确性都不清楚 而

随机推荐

  • iOS8 Core Image In Swift：更复杂的滤镜

    iOS8 Core Image In Swift xff1a 自动改善图像以及内置滤镜的使用 iOS8 Core Image In Swift xff1a 更复杂的滤镜 iOS8 Core Image In Swift xff1a 人脸检测

  • 使用CocoaPods过程中的几个问题

    当把CocoaPods生成的workspace移动到上层目录时 xff0c 需要改下Pods xcconfig和工程里的一些设置 xff0c 就通常没什么难度 当遇到这个问题时 xff1a Could not automatically s

  • 仿网易云音乐的播放进度条

    仿网易云音乐的播放进度条 xff0c 有三种状态 xff1a 播放 暂停和拖动 xff0c 只是实现了动画和主要的交互逻辑 xff0c 其他细节 xff08 如暂停音乐的播放等 xff09 还需要自己完善 xff1a DKPlayerBar

  • 用 Houston 在本地调试远程通知

    Houston 的背景 Houston 在 GitHub 上的地址 xff1a https github com nomad Houston xff0c 作者又是Mattt Thompson xff0c 简直是惨无人道啊 xff0c 又高产

  • 如何设计一个 iOS 控件?(iOS 控件完全解析)

    代码的等级 xff1a 可编译 可运行 可测试 可读 可维护 可复用 前言 一个控件从外在特征来说 xff0c 主要是封装这几点 xff1a 交互方式显示样式数据使用 对外在特征的封装 xff0c 能让我们在多种环境下达到 PM 对产品的要

  • 蛋花花分享人工智能概念的诞生与发展

    蛋花花分享人工智能概念的诞生与发展 xff01 如今人工智能非常的火 xff0c 在各行各业都有突出的变现 xff0c 让人期待它未来的发展 蛋花花认为了解人工智能向何处去 xff0c 首先要知道人工智能从何处来 1956年夏 xff0c

  • 用JFreeChart 生成报表

    JFreeChart是JAVA平台上的一个开放的图表绘制类库 它完全使用JAVA语言编写 xff0c 是为applications applets servlets 以及JSP等使用所设计 JFreeChart可生成饼图 xff08 pie

  • Mac原生字典支持的词典

    一共十八部词典 xff0c 见下 xff1a 打了一个包 xff0c 有700多M xff0c 在CSDN没有权限上传这么大的文件 xff0c 故上传到115上 放到 Library Dictionaries 目录中即可 下载地址

  • NSAttributedString 详解

    NSAttributedString可以让我们使一个字符串显示的多样化 xff0c 但是目前到iOS 5为止 xff0c 好像对它支持的不是很好 xff0c 因为显示起来不太方便 xff08 至少没有在OS X上方便 xff09 首先导入C

  • ftp三种用户权限设置

    修改配置文件vsftpd conf Vi etc vsftpd vsftpd conf 修改匿名用户为禁止 2在禁止登录名单里删除root vi etc vsftpd user list vi etc vsftpd ftpuser 然后按d

  • 常见的socket出错总结

    常见错误 ECONNREFUSED 111 没有这个端口 EAGAIN 11 buff已满 EPIPE 32 客户端断掉了 ECONNRESET xff08 104 xff09 客户端先可以正常连接服务端 xff0c 并可以进行数据收发 x

  • CentOS-8中安装JDK 1.8

    备忘录 xff1a 喜欢 xff0c 即可 xff0c 无它 本例环境 xff1a 操作系统 xff1a CentOS 8 1 1911 x86 64 dvd1 安装包 xff1a jdk 8u251 linux x64 rpm 远程连接工

  • Android 系统（125）---Android通过Dialer实现暗码启动

    Android通过Dialer实现暗码启动 目前接触比较多的就是通过dialer应用来启动 触发暗码 本文以Dialer为例 xff0c 1 经过调试定位 xff0c 发现拨号盘接对应的Activity为DialtactsActivity

  • 阿里程序员常用的 15 个高效工具，大部分已开源！

    阿里程序员常用的 15 个高效工具 xff0c 大部分已开源 xff01 阿里将自身在各类业务场景下的技术积淀 xff0c 通过开源 云上实现或工具等形式对外开放 xff0c 本文将精选了一些阿里巴巴的开发者工具 xff0c 希望能帮助开发

  • Linux域名解析（DNS）

    DNS简介 域名系统 xff08 英文 xff1a Domain Name System xff0c 缩写 xff1a DNS xff09 xff0c 使用应用层协议 xff0c 是互联网的一项服务 它作为将域名和IP地址相互映射的一个分布

  • redhat6.7系统突然异常死机问题处理

    redhat6 7正常使用过一段时间之后异常死机 xff0c cat var log messages查看日志没有明显的error报错 xff0c 看带外管理日志发现是系统的问题 xff0c 后来通过修改grub conf配置解决了 xff

  • 下载并构建PX4

    根据官方的文档 xff0c PX4下载和构建的方式有两种 xff1a Linux系列的Console模式 xff08 当然也支持Windows下的MINGW32 xff09 和Windows模式 在Windows平台下 xff0c 我们习惯

  • 又是一年年终时...

    今年的第一天 xff0c 也就是 2009 年的第一天 xff0c 我用一个懒觉迎接了 2009 xff0c 整整睡到了中午 11 30 才醒 新一年初 xff0c 也就是明天 xff0c 我决定用早起来迎接 习惯了晚上学习 xff0c 早

  • 前端传值（枚举类接收问题）

    最近做的这个项目中 xff0c 用到了大量的枚举类 xff0c 今天来记录一下我遇到的问题 xff0c 如果能帮到大家就更好了 xff01 1 枚举类如何转为json xff08 在一个类的属性中 xff0c 这个枚举类属性如何直接使用在接

  • APM_ArduCopter源码解析学习（四）——IMU

    APM ArduCopter源码解析学习 xff08 四 xff09 IMU 前言一 system cpp 1 1 无人机内部初始化1 2 Copter init ardupilot 1 3 Copter startup INS groun

热门标签