上一篇博文已經說過了，在init_ardupilot()內部車輛將完成大部分裝置的初始化工作。Ardusub的init_ardupilot()這個函數還是很好找的，就實現在ardupilot/Ardusub/system.cpp檔案內部。但是在開始講解這個函數之前，想先把車輛內部的初始化流程先說一下。
關於車輛型別及其繼承方式，詳見我的上一篇博文：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內部，由此引出我們本次的主要內容。
如下內容是針對於水下無人機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();
由此我們跳轉到位於同一檔案下的該函數內部
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()完成陀螺儀的校準工作。
這邊重點來講一下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下面的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這兩份檔案主要實現的就是前後端的連線。
目前先寫到這，後面有時間再更新