前言

一、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();

    /*
     *  設定“主回圈已死”檢查， 請注意，這依賴于正在初始化的RC庫
     */
    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()建議閱讀源代碼學習，

/*
 * 啟動所有用于陀螺儀和加速度測量的后端， 如果尚未呼叫detect_backends()，它將自動呼叫它，
 */
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()這個宏

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

可知最主要的是其中的_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()的呼叫方式，該函式內部如下，

/*
  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這兩份檔案主要實作的就是前后端的連接，

目前先寫到這，后面有時間再更新