我是靠谱客的博主 落后衬衫，这篇文章主要介绍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。
复制代码
1
2
3
4
5
6
7
8
9
10private: ... 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的初始化函数。部分内容已备注。
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214void 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; }
无论如何请在上述程序段中找到下面这段，该函数可进行地面启动过程中需要的所有惯性传感器校准等工作。
复制代码
1
2
3
4
5
6startup_INS_ground();
- 1
1.3 Copter::startup_INS_ground()
由此我们跳转到位于同一文件下的该函数内部
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17//****************************************************************************** //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()函数：
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55void 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()建议阅读源代码学习。
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26/* * 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宏定义部分：
复制代码
1
2
3
4
5
6
7
8
9
10
11
12#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()：
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101#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°。
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27AP_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()的功能介绍复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85bool 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语句如下面所示：
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50// 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; } }
针对于原来的语句
复制代码
1
2
3
4
5
6ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_ROLL_180));
- 1
我们观察ADD_BACKEND()这个宏，具体路径在AP_InertialSensor.cpp中。
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14/* 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()
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13bool 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()方法，用于配置并启动所有传感器。
复制代码
1
2
3
4
5
6
7
8for (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()函数，然后这个函数最关键的地方就是在最后面：
复制代码
1
2
3
4
5
6
7// 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
复制代码
1
2
3
4
5
6_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以获取当前的陀螺仪数据的功能。内部具体代码如下
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97/* 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; }
注意到内部的后端更新函数
复制代码
1
2
3
4
5
6
7
8for (uint8_t i=0; i<_backend_count; i++) { _backends[i]->update(); }
- 1
- 2
- 3
类似于_backends[i]->start()的调用方式，该函数内部如下。
复制代码
1
2
3
4
5
6
7
8
9
10
11
12
13/* 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内容请搜索靠谱客的其他文章。
本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。
发表评论 取消回复