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

• 前言
• 一、system.cpp
• • 1.1 车辆内部初始化
  • 1.2 Sub::init_ardupilot()
  • 1.3 Sub::startup_INS_ground()
• 二、库内传感器配置及前后端
• • backend
• 三、Ardusub.cpp

前言

一、system.cpp

上一篇博文已经说过了，在init_ardupilot()内部车辆将完成大部分设备的初始化工作。Ardusub的init_ardupilot()这个函数还是很好找的，就实现在ardupilot/Ardusub/system.cpp文件内部。但是在开始讲解这个函数之前，想先把车辆内部的初始化流程先说一下。

1.1 车辆内部初始化

关于车辆类型及其继承方式，详见我的上一篇博文：Ardusub源码解析学习（三）——车辆类型

继承关系如下：

AP_HAL::HAL::Callbacks
  |---- AP_Vechile
    |---- Sub

之前已经说过，AP_Vehicle类在公有部分内部声明有两个函数setup()和loop()，并且在对应的.cpp文件中得到实现；在protected部分提供了一个纯虚的接口函数init_ardupilot()，并且未在AP_Vehicle.cpp中对其进行实现（具体查看我的上一篇博文）。而实际上，在AP_Vehicle.cpp中对setup()进行具体实现的时候调用了init_ardupilot()。

Sub类继承自AP_Vehicle类，并且并未对setup()和loop()进行修改或重写。相对应的，其在内部以private方式继承了fast_loop()和init_ardupilot()，fast_loop()的具体实现在第一篇博文里面已经介绍过了，这边主要来说一下init_ardupilot()。

private:
...
void fast_loop() override;
...
void init_ardupilot() override;

已知Sub类继承了AP_Vehicle类所实现的函数，那么Sub::setup()函数在运行过程中便会调用到Sub::init_ardupilot()函数，而Sub::init_ardupilot则正好实现在system.cpp内部，由此引出我们本次的主要内容。

1.2 Sub::init_ardupilot()

如下内容是针对于水下无人机Sub的初始化函数。部分内容已备注。

void Sub::init_ardupilot()
{ 
    BoardConfig.init();
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
    can_mgr.init();
#endif

#if AP_FEATURE_BOARD_DETECT
    // 在BoardConfig.init()之后才能进行检测
    switch (AP_BoardConfig::get_board_type()) { 
    case AP_BoardConfig::PX4_BOARD_PIXHAWK2:
        AP_Param::set_by_name("GND_EXT_BUS", 0);
        celsius.init(0);
        break;
    default:
        AP_Param::set_by_name("GND_EXT_BUS", 1);
        celsius.init(1);
        break;
    }
#else
    AP_Param::set_default_by_name("GND_EXT_BUS", 1);
    celsius.init(1);
#endif

    // 初始化加持器设备
#if GRIPPER_ENABLED == ENABLED
    g2.gripper.init();
#endif

#if AC_FENCE == ENABLED
    fence.init();
#endif

    // 初始化通知系统
    notify.init();

    // 初始化电池监控器
    battery.init();

    barometer.init();

    // 设置带有串行端口的电报插槽
    gcs().setup_uarts();

#if LOGGING_ENABLED == ENABLED
    log_init();
#endif

    // 初始化rc通道，包括设置模式
    rc().init();

    init_rc_in();               // 从radio设置rc通道
    init_rc_out();              // 设置电机并输出到电调
    init_joystick();            // 手柄摇杆初始化

    relay.init();

    
    hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);

    // GPS初始化
    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
    // 使光流可用于AHRs
    ahrs.set_optflow(&optflow);
#endif

    // 初始化位置类
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
    Location::set_terrain(&terrain);
    wp_nav.set_terrain(&terrain);
#endif

    pos_control.set_dt(MAIN_LOOP_SECONDS);

    // 初始化光流传感器
#if OPTFLOW == ENABLED
    init_optflow();
#endif

#if HAL_MOUNT_ENABLED
    // 初始化相机云台
    camera_mount.init();
    // 必须执行此步骤，以便正确初始化伺服
    camera_mount.set_angle_targets(0, 0, 0);
    // 由于某些原因，对set_angle_targets的调用将模式更改为mavlink定位！
    camera_mount.set_mode(MAV_MOUNT_MODE_RC_TARGETING);
#endif

#ifdef USERHOOK_INIT
    USERHOOK_INIT
#endif

    // 初始化baro并确定我们是否具有外部（深度）压力传感器
    barometer.set_log_baro_bit(MASK_LOG_IMU);
    barometer.calibrate(false);
    barometer.update();

    for (uint8_t i = 0; i < barometer.num_instances(); i++) { 
        if (barometer.get_type(i) == AP_Baro::BARO_TYPE_WATER) { 
            barometer.set_primary_baro(i);
            depth_sensor_idx = i;
            ap.depth_sensor_present = true;
            sensor_health.depth = barometer.healthy(depth_sensor_idx); // 初始化健康标志
            break; // Go with the first one we find
        }
    }

    if (!ap.depth_sensor_present) { 
        // 只有板载气压计
        // 未检测到外部水下深度传感器
        barometer.set_primary_baro(0);
        ahrs.set_alt_measurement_noise(10.0f);  // 读数与INS的其余部分不符
    } else { 
        ahrs.set_alt_measurement_noise(0.1f);
    }

    leak_detector.init();

    last_pilot_heading = ahrs.yaw_sensor;

    // 初始化测距仪
#if RANGEFINDER_ENABLED == ENABLED
    init_rangefinder();
#endif

    // 初始化AP_RPM库
#if RPM_ENABLED == ENABLED
    rpm_sensor.init();
#endif

    // 初始化mission库
    mission.init();

    // 初始化AP_Logger库
#if LOGGING_ENABLED == ENABLED
    logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&sub, &Sub::Log_Write_Vehicle_Startup_Messages, void));
#endif

    startup_INS_ground();

#ifdef ENABLE_SCRIPTING
    g2.scripting.init();
#endif // ENABLE_SCRIPTING

    //我们不想写入串行端口以使我们在飞行中暂停，因此一旦准备好飞行，请将串行端口设置为非阻塞
    serial_manager.set_blocking_writes_all(false);

    // 启用CPU故障安全
    mainloop_failsafe_enable();

    ins.set_log_raw_bit(MASK_LOG_IMU_RAW);

    // 根据要求禁用安全
    BoardConfig.init_safety();    
    
    hal.console->print("\nInit complete");

    // 标记初始化已完成
    ap.initialised = true;
}

无论如何请在上述程序段中找到下面这段，该函数可进行地面启动过程中需要的所有惯性传感器校准等工作。

startup_INS_ground();

1.3 Sub::startup_INS_ground()

由此我们跳转到位于同一文件下的该函数内部

void Sub::startup_INS_ground()
{ 
    // 初始化ahrs（如果使用该设备，则可将imu校准推入mpu6000）
    ahrs.init();
    ahrs.set_vehicle_class(AHRS_VEHICLE_SUBMARINE);	//设置车辆类型为SUBMARINE

    // 热身和校准陀螺仪偏移
    ins.init(scheduler.get_loop_rate_hz());

    // 重置包括陀螺仪偏置的ahrs
    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)
{ 
    // 记住采样率
    _loop_rate = loop_rate;
    _loop_delta_t = 1.0f / loop_rate;

    // 我们不允许超出INST正常循环时间10倍的增量值。 较大的增量值可能会导致状态估计量出现偏差
    _loop_delta_t_max = 10 * _loop_delta_t;

    if (_gyro_count == 0 && _accel_count == 0) { 
        _start_backends();
    }

    // 如有必要，初始化加速度级别。 这是必需的，因为我们无法在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));
        }
    }

    // 校准陀螺仪，除非已禁用陀螺仪校准
    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;

    // 初始化IMU批量记录
    batchsampler.init();

    // 谐波陷波的中心频率始终取自计算值，以便可以动态更新，计算值始终是配置的中心频率的倍数，因此请从配置值开始
    _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));
        // 初始化默认设置，这些设置随后将在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()建议阅读源代码学习。

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加入到后端，方便前端调用。以我目前在用的Pixhawk2.4.8来说，其就归属于FMUv3类型的。

前面宏定义部分：

#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

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;
    ...

在代码段内，会对检测到的板子类型加入对应的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对象。

针对于原来的语句

ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180));

我们观察ADD_BACKEND()这个宏

    
#define ADD_BACKEND(x) do { \ if (((1U<<probe_count)&enable_mask) && _add_backend(x)) { \ found_mask |= (1U<<probe_count); \ } \ probe_count++; \ } while (0)

可知最主要的是其中的_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();
    }

对于_backends[]数组中的成员，我们刚刚获取到的是AP_InertialSensor_Invensense中的mpu，因此于对应的.cpp文件中查找到对应的函数。这边由于函数内容过长就不全部放上来了，内部具体的实现内容简单说了就是配置一下芯片的参数及量程什么的（不太准确，建议自己去阅读一下）。然后这个函数最关键的地方就是在最后面：

// 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));

这段程序开启了一个新的读取线程，线程运行时间为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;

由此可知该读线程运行时间为1000us，即1ms运行一次，通过_poll_data()函数内部调用的_read_fifo()读取mpu内部的数据，因此我们在Ardusub.cpp中找不到对于imu读取的相关函数，因为在start()函数内部以及实现。

为了方便理清关系，这边把对应的逻辑表示一下

Sub::init_ardupilot()
  |---- startup_INS_ground();
    |---- ins.init();
      |---- _start_backends();
        |----detect_backends();    |----_backends[i]->start();
          |----ADD_BACKEND();    |----_dev->register_periodic_callback();

三、Ardusub.cpp

回到最初的Ardusub.cpp下面的fast_loop()，里面的ins.update()完成了立即更新INS以获取当前的陀螺仪数据的功能。内部具体代码如下

void AP_InertialSensor::update(void)
{ 
    // 等待采样
    wait_for_sample();

	// 如果不是处于在线仿真则运行以下代码
    if (!_hil_mode) { 
        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { 		//最多兼容INS_MAX_INSTANCES=3个传感器
        	// 读取前重新置位传感器健康标志位
            // 将传感器标记为不健康，并通过_publish_gyro（）和_publish_accel（）将每个后端中的update（）标记为健康
            _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();
        }

        // 清除累加器
        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];
            }
        }

        // 如果一个传感器的错误计数非零，但另一个传感器没有，则调整健康状态。
        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) { 
                // 宁愿不要使用出现错误的陀螺仪
                _gyro_healthy[i] = false;
            }
            if (_accel_healthy[i] && _accel_error_count[i] > _accel_startup_error_count[i] && have_zero_accel_error_count) { 
                // 不希望使用出现错误的加速度
                _accel_healthy[i] = false;
            }
        }

        // 将健康加速度和陀螺设置为首要
        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();
        }

类似于_backends[i]->start()的调用方式，该函数内部如下。

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这两份文件主要实现的就是前后端的连接。

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