PX4_ECL_EKF代码分析1_px4 ekf2 代码详解

写在前面

源码版本：1.6.0rc1
源码位置1：Firmware-1.6.0rc1\src\modules\ekf2_main.cpp
源码位置2：Firmware-1.6.0rc1\src\lib\ecl\EKF\

整体框架：

上图PX4的EKF代码框架，PX4的代码由两部分组成，一部分是在modules下的ekf2，另一部分是ecl代码库中EKF部分。

第一部分实现数据的订阅（subscribe）、整理、储存、经过处理的数据发布（publish）。
第二部分实现数据的处理。

第一部分：

int ekf2_main(int argc, char *argv[])
{
	if (argc < 2) {
		PX4_WARN("usage: ekf2 {start|stop|status}");
		return 1;
	}

	if (!strcmp(argv[1], "start")) {

		if (ekf2::instance != nullptr) {
			PX4_WARN("already running");
			return 1;
		}

		ekf2::instance = new Ekf2();

		if (ekf2::instance == nullptr) {
			PX4_WARN("alloc failed");
			return 1;
		}

		if (argc >= 3) {
			if (!strcmp(argv[2], "--replay")) {
				ekf2::instance->set_replay_mode(true);
			}
		}

		if (OK != ekf2::instance->start()) {
			delete ekf2::instance;
			ekf2::instance = nullptr;
			PX4_WARN("start failed");
			return 1;
		}
		return 0;
	}
	if (!strcmp(argv[1], "stop")) {
		if (ekf2::instance == nullptr) {
			PX4_WARN("not running");
			return 1;
		}
		ekf2::instance->exit();
		// wait for the destruction of the instance
		while (ekf2::instance != nullptr) {
			usleep(50000);
		}
		return 0;
	}

	if (!strcmp(argv[1], "print")) {
		if (ekf2::instance != nullptr) {

			return 0;
		}
		return 1;
	}

	if (!strcmp(argv[1], "status")) {
		if (ekf2::instance) {
			PX4_WARN("running");
			ekf2::instance->print_status();
			return 0;

		} else {
			PX4_WARN("not running");
			return 1;
		}
	}
	PX4_WARN("unrecognized command");
	return 1;
}
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

EKF2的功能代码查询，上位机在输入{start|stop|status}三个字符串使得EKF2代码运行、停止、查询状态，如：输入start，若EKF2已经在运行则打印“already running”，若没有运行就实例化一个对象 （其中包含了一些私有变量的初始化）

创建一个新的进程 new Ekf2()，并使之ekf2::instance->start()。跟进这个start()。

int Ekf2::start()
{
	ASSERT(_control_task == -1);
	/* start the task */
	_control_task = px4_task_spawn_cmd("ekf2",
					   SCHED_DEFAULT,
					   SCHED_PRIORITY_MAX - 5,
					   5800,
					   (px4_main_t)&Ekf2::task_main_trampoline,
					   nullptr);

	if (_control_task < 0) {
		PX4_WARN("task start failed");
		return -errno;
	}
	return OK;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

函数中px4_task_spawn_cmd函数的作用是创建一个新的进程，在nuttx系统中作为一个独立的进程运行，和其他模块之间通过uORB进程间相互通讯，参数变量表示分别为：1、进程的入口函数 2、进程默认调度 3、进程优先级 4、进程栈大小 5、进程入口函数（下一步会跟进这个函数） 6、nullptr。

跟进第五个变量，进程入口函数：

void Ekf2::task_main_trampoline(int argc, char *argv[])
{
	ekf2::instance->task_main();
}
1
2
3
4

跟进task_main() 因函数体积较大故将其拆开分析

void Ekf2::task_main()
{
	// subscribe to relevant topics
	int sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	int airspeed_sub = orb_subscribe(ORB_ID(airspeed));
	int params_sub = orb_subscribe(ORB_ID(parameter_update));
	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
	int range_finder_sub = orb_subscribe(ORB_ID(distance_sensor));
	int ev_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position));
	int ev_att_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude));
	int vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
	int status_sub = orb_subscribe(ORB_ID(vehicle_status));
	int sensor_selection_sub = orb_subscribe(ORB_ID(sensor_selection));

	// initialise parameter cache
	updateParams();
	// initialize data structures outside of loop
	// because they will else not always be
	// properly populated
	sensor_combined_s sensors = {};
	vehicle_gps_position_s gps = {};
	airspeed_s airspeed = {};
	optical_flow_s optical_flow = {};
	distance_sensor_s range_finder = {};
	vehicle_land_detected_s vehicle_land_detected = {};
	vehicle_local_position_s ev_pos = {};
	vehicle_attitude_s ev_att = {};
	vehicle_status_s vehicle_status = {};
	sensor_selection_s sensor_selection = {};
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

函数开头，订阅了一堆数据用于后面计算使用，更新参数，定义结构体。订阅的数据根据消息ID信息（如：sensor_combined），可以到：Firmware-1.6.0rc1\msg\ sensor_combined.msg 查看msg文件夹下是PX4用到的所有结构体。

	px4_pollfd_struct_t fds[2] = {};
	fds[0].fd = sensors_sub;
	fds[0].events = POLLIN;
	fds[1].fd = params_sub;
	fds[1].events = POLLIN;
	
	while (!_task_should_exit) {
		int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);

		if (ret < 0) {
			// Poll error, sleep and try again
			usleep(10000);
			continue;

		} else if (ret == 0) {
			// Poll timeout or no new data, do nothing
			continue;
		}
		if (fds[1].revents & POLLIN) {
			// read from param to clear updated flag
			struct parameter_update_s update;
			orb_copy(ORB_ID(parameter_update), params_sub, &update);
			updateParams();
			// fetch sensor data in next loop
			continue;
			
		} else if (!(fds[0].revents & POLLIN)) {
			// no new data
			continue;
		}
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

这里定义的两组topic作为阻塞等待获取（fds[0].fd=阻塞等待的句柄，fds[0].events=阻塞等待方式），而其他订阅的数据作为检查更新，言外之意阻塞等待的数据是比较重要的数据，是进行数据处理的关键步骤，而检查更新的数据是次要的有更新才会获取。

以sensor_combined阻塞等待为例：以1000毫秒阻塞等待句柄为_sensors_sub的数据，前面可以看到其消息ID为sensor_combined，如果返回值<0，说明出现错误，休息一会(usleep(10000);)继续(continue)阻塞等待这个这个数据；如果返回值=0，说明在等待1000毫秒之后依然没有拿到数据，继续(continue)阻塞等待，直到拿到数据为止。

这里两个topic分别是sensors_sub、params_sub，在往上看是由sensor_combined、parameter_update为句柄定义的topic，分别看一下两个结构体里面都有啥：

sensor_combined.msg里面有陀螺、加计、磁力计、气压计等一系列数据融合所必须的数据，所以对sensor_combined为句柄的topic需要阻塞等待

parameter_update.msg 里面只有一个变量，但这个此消息用于通知系统一个或多个参数更改，固其作用也很重要。

	bool gps_updated = false;
	bool airspeed_updated = false;
	bool optical_flow_updated = false;
	bool range_finder_updated = false;
	bool vehicle_land_detected_updated = false;
	bool vision_position_updated = false;
	bool vision_attitude_updated = false;
	bool vehicle_status_updated = false;
1
2
3
4
5
6
7
8

定义一些标志位。

		orb_copy(ORB_ID(sensor_combined), sensors_sub, &sensors);
		// update all other topics if they have new data

		orb_check(status_sub, &vehicle_status_updated);

		if (vehicle_status_updated) {
			orb_copy(ORB_ID(vehicle_status), status_sub, &vehicle_status);
		}

		orb_check(gps_sub, &gps_updated);

		if (gps_updated) {
			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
		}

		orb_check(airspeed_sub, &airspeed_updated);

		if (airspeed_updated) {
			orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
		}

		orb_check(optical_flow_sub, &optical_flow_updated);

		if (optical_flow_updated) {
			orb_copy(ORB_ID(optical_flow), optical_flow_sub, &optical_flow);
		}

		orb_check(range_finder_sub, &range_finder_updated);

		if (range_finder_updated) {
			orb_copy(ORB_ID(distance_sensor), range_finder_sub, &range_finder);

			if (range_finder.min_distance > range_finder.current_distance
			    || range_finder.max_distance < range_finder.current_distance) {
				range_finder_updated = false;
			}
		}

		orb_check(ev_pos_sub, &vision_position_updated);

		if (vision_position_updated) {
			orb_copy(ORB_ID(vehicle_vision_position), ev_pos_sub, &ev_pos);
		}

		orb_check(ev_att_sub, &vision_attitude_updated);

		if (vision_attitude_updated) {
			orb_copy(ORB_ID(vehicle_vision_attitude), ev_att_sub, &ev_att);
		}
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

上面就是检查更新的部分了，例如：检查（check）以vehicle_status为消息ID的数据是否有更新，如果有更新则把数据复制出来（copy），后面的同理。

// in replay mode we are getting the actual timestamp from the sensor topic
		hrt_abstime now = 0;
		if (_replay_mode) {
			now = sensors.timestamp;
		} else {
			now = hrt_absolute_time();
		}
1
2
3
4
5
6
7

这里获取绝对时间，用于回放时，各种传感器的实际时间戳

// push imu data into estimator
		float gyro_integral[3];
		gyro_integral[0] = sensors.gyro_rad[0] * sensors.gyro_integral_dt;
		gyro_integral[1] = sensors.gyro_rad[1] * sensors.gyro_integral_dt;
		gyro_integral[2] = sensors.gyro_rad[2] * sensors.gyro_integral_dt;
		float accel_integral[3];
		accel_integral[0] = sensors.accelerometer_m_s2[0] * sensors.accelerometer_integral_dt;
		accel_integral[1] = sensors.accelerometer_m_s2[1] * sensors.accelerometer_integral_dt;
		accel_integral[2] = sensors.accelerometer_m_s2[2] * sensors.accelerometer_integral_dt;
1
2
3
4
5
6
7
8
9

将陀螺、加计的数据进行积分，并压栈储存。

 _ekf.setIMUData(now, sensors.gyro_integral_dt * 1.e6f, sensors.accelerometer_integral_dt * 1.e6f,
				gyro_integral, accel_integral);
1
2

凡是带有 _ekf. 前缀的函数都在ECL库中。进入这个setIMUData()看一下都干啥了：

void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt,
				    float (&delta_ang)[3], float (&delta_vel)[3])
{
	if (!_initialised) {
		init(time_usec);
		_initialised = true;
	}
	float dt = (float)(time_usec - _time_last_imu) / 1000 / 1000;
	dt = math::max(dt, 1.0e-4f);
	dt = math::min(dt, 0.02f);

	_time_last_imu = time_usec;

	if (_time_last_imu > 0) {
		_dt_imu_avg = 0.8f * _dt_imu_avg + 0.2f * dt;
	}
	// copy data
	imuSample imu_sample_new = {};
	imu_sample_new.delta_ang = Vector3f(delta_ang);
	imu_sample_new.delta_vel = Vector3f(delta_vel);

	// convert time from us to secs   把时间转换成秒
	imu_sample_new.delta_ang_dt = delta_ang_dt / 1e6f;
	imu_sample_new.delta_vel_dt = delta_vel_dt / 1e6f;
	imu_sample_new.time_us = time_usec;
	_imu_ticks++;

	// calculate a metric which indicates the amount of coning vibration
	//计算一个表示圆锥振动量的度量
	Vector3f temp = cross_product(imu_sample_new.delta_ang, _delta_ang_prev);//做一个向量叉乘
	_vibe_metrics[0] = 0.99f * _vibe_metrics[0] + 0.01f * temp.norm();

	// calculate a metric which indiates the amount of high frequency gyro vibration
	//计算一个表示高频陀螺振动量的度量
	temp = imu_sample_new.delta_ang - _delta_ang_prev;
	_delta_ang_prev = imu_sample_new.delta_ang;
	_vibe_metrics[1] = 0.99f * _vibe_metrics[1] + 0.01f * temp.norm();

	// calculate a metric which indicates the amount of high fequency accelerometer vibration
	//计算一个表示高频加速度计振动量的度量值
	temp = imu_sample_new.delta_vel - _delta_vel_prev;
	_delta_vel_prev = imu_sample_new.delta_vel;
	_vibe_metrics[2] = 0.99f * _vibe_metrics[2] + 0.01f * temp.norm();

	// accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available
	//累积和下采样imu数据，并在新的下采样数据可用时推入缓冲区
	if (collect_imu(imu_sample_new)) {//判断采样是否允许
		_imu_buffer.push(imu_sample_new);//imu采样数据压栈储存
		_imu_ticks = 0;
		_imu_updated = true;

		// down-sample the drag specific force data by accumulating and calculating the mean when
		// sufficient samples have been collected
		//在收集到足够的样本后，通过累积和计算平均值对阻力比力数据进行取样
		if (_params.fusion_mode & MASK_USE_DRAG) {
			_drag_sample_count ++;
			// note acceleration is accumulated as a delta velocity
			_drag_down_sampled.accelXY(0) += imu_sample_new.delta_vel(0);
			_drag_down_sampled.accelXY(1) += imu_sample_new.delta_vel(1);
			_drag_down_sampled.time_us += imu_sample_new.time_us;
			_drag_sample_time_dt += imu_sample_new.delta_vel_dt;

			// calculate the downsample ratio for drag specific force data
			//为阻力比力数据计算下样本比
			uint8_t min_sample_ratio = (uint8_t) ceilf((float)_imu_buffer_length / _obs_buffer_length);

			if (min_sample_ratio < 5) {
				min_sample_ratio = 5;
			}
			// calculate and store means from accumulated values
			if (_drag_sample_count >= min_sample_ratio) {
				// note conversion from accumulated delta velocity to acceleration
				_drag_down_sampled.accelXY(0) /= _drag_sample_time_dt;
				_drag_down_sampled.accelXY(1) /= _drag_sample_time_dt;
				_drag_down_sampled.time_us /= _drag_sample_count;

				// write to buffer
				_drag_buffer.push(_drag_down_sampled);//最终的目的就是push  将下采样数据压栈保存

				// reset accumulators
				_drag_sample_count = 0;
				_drag_down_sampled.accelXY.zero();
				_drag_down_sampled.time_us = 0;
				_drag_sample_time_dt = 0.0f;
			}
		}
		// get the oldest data from the buffer
		_imu_sample_delayed = _imu_buffer.get_oldest();

		// calculate the minimum interval between observations required to guarantee no loss of data
		// this will occur if data is overwritten before its time stamp falls behind the fusion time horizon
		//计算观测所需的最小间隔，以确保数据不会丢失。
		//如果数据在时间戳落后于融合时间范围之前被覆盖，就会发生这种情况
		_min_obs_interval_us = (_imu_sample_new.time_us - _imu_sample_delayed.time_us) / (_obs_buffer_length - 1);

	} else {
		_imu_updated = false;
	}
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

		// read mag data读取磁力计
        if (sensors.magnetometer_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {// 如果无效
			// set a zero timestamp to let the ekf replay program know that this data is not valid
			_timestamp_mag_us = 0;

        } else {//有效
			if ((sensors.timestamp + sensors.magnetometer_timestamp_relative) != _timestamp_mag_us) {
				_timestamp_mag_us = sensors.timestamp + sensors.magnetometer_timestamp_relative;

				// Reset learned bias parameters if there has been a persistant change in magnetometer ID
				// Do not reset parmameters when armed to prevent potential time slips casued by parameter set
				// and notification events
				// Check if there has been a persistant change in magnetometer ID

                //如果磁力计ID有持续变化，则重置学习偏置参数
                //不重置参数时，以防止潜在的时间滑脱造成的参数设置和通知事件
                //检查磁力计ID是否持续变化
				orb_copy(ORB_ID(sensor_selection), sensor_selection_sub, &sensor_selection);
				if (sensor_selection.mag_device_id != 0 && sensor_selection.mag_device_id != _mag_bias_id.get()) {
					if (_invalid_mag_id_count < 200) {
						_invalid_mag_id_count++;
					}
				} else {
					if (_invalid_mag_id_count > 0) {
						_invalid_mag_id_count--;
					}
				}
				if ((vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) && (_invalid_mag_id_count > 100)) {
					// the sensor ID used for the last saved mag bias is not confirmed to be the same as the current sensor ID
					// this means we need to reset the learned bias values to zero
                    //上次保存的mag偏置使用的传感器ID没有被确认为与当前传感器ID相同，这意味着需要将学习偏置值重置为零
					_mag_bias_x.set(0.f);
					_mag_bias_x.commit_no_notification();
					_mag_bias_y.set(0.f);
					_mag_bias_y.commit_no_notification();
					_mag_bias_z.set(0.f);
					_mag_bias_z.commit_no_notification();
					_mag_bias_id.set(sensor_selection.mag_device_id);
                    _mag_bias_id.commit();
					_invalid_mag_id_count = 0;

					PX4_INFO("Mag sensor ID changed to %i", _mag_bias_id.get());
				}
				// If the time last used by the EKF is less than specified, then accumulate the
                // data and push the average when the specified interval is reached.
                //如果EKF最后使用的时间小于指定的时间，则累积数据并在达到指定的时间间隔时推入平均值。
				_mag_time_sum_ms += _timestamp_mag_us / 1000;
				_mag_sample_count++;
				_mag_data_sum[0] += sensors.magnetometer_ga[0];
				_mag_data_sum[1] += sensors.magnetometer_ga[1];
				_mag_data_sum[2] += sensors.magnetometer_ga[2];
				uint32_t mag_time_ms = _mag_time_sum_ms / _mag_sample_count;
				
				if (mag_time_ms - _mag_time_ms_last_used > _params->sensor_interval_min_ms) {
					float mag_sample_count_inv = 1.0f / (float)_mag_sample_count;
                    // calculate mean of measurements and correct for learned bias offsets
                    //计算测量值的平均值，并纠正学习偏置
					float mag_data_avg_ga[3] = {_mag_data_sum[0] *mag_sample_count_inv - _mag_bias_x.get(),
								    _mag_data_sum[1] *mag_sample_count_inv - _mag_bias_y.get(),
								    _mag_data_sum[2] *mag_sample_count_inv - _mag_bias_z.get()
								   };
					_ekf.setMagData(1000 * (uint64_t)mag_time_ms, mag_data_avg_ga);
					_mag_time_ms_last_used = mag_time_ms;
					_mag_time_sum_ms = 0;
					_mag_sample_count = 0;
					_mag_data_sum[0] = 0.0f;
					_mag_data_sum[1] = 0.0f;
					_mag_data_sum[2] = 0.0f;
				}
			}
		}
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

同样最后将磁力计数据压栈储存_ekf.setMagData(1000 * (uint64_t)mag_time_ms, mag_data_avg_ga);这里就不将setMagData()函数列出来了，同上面一样都在Firmware-1.6.0rc1\src\lib\ecl\EKF\estimator_interface.cpp文件中。

		// read baro data
		if (sensors.baro_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
			// set a zero timestamp to let the ekf replay program know that this data is not valid
			_timestamp_balt_us = 0;

		} else {
			if ((sensors.timestamp + sensors.baro_timestamp_relative) != _timestamp_balt_us) {
				_timestamp_balt_us = sensors.timestamp + sensors.baro_timestamp_relative;

				// If the time last used by the EKF is less than specified, then accumulate the
				// data and push the average when the specified interval is reached.
				_balt_time_sum_ms += _timestamp_balt_us / 1000;
				_balt_sample_count++;
				_balt_data_sum += sensors.baro_alt_meter;
				uint32_t balt_time_ms = _balt_time_sum_ms / _balt_sample_count;

				if (balt_time_ms - _balt_time_ms_last_used > (uint32_t)_params->sensor_interval_min_ms) {
					float balt_data_avg = _balt_data_sum / (float)_balt_sample_count;
					_ekf.setBaroData(1000 * (uint64_t)balt_time_ms, balt_data_avg);//气压计数据压栈储存
					_balt_time_ms_last_used = balt_time_ms;
					_balt_time_sum_ms = 0;
					_balt_sample_count = 0;
					_balt_data_sum = 0.0f;

				}
			}
		}
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

读取气压计并储存。

		// read gps data if available
		if (gps_updated) {
			struct gps_message gps_msg = {};
			gps_msg.time_usec = gps.timestamp;
			gps_msg.lat = gps.lat;
			gps_msg.lon = gps.lon;
			gps_msg.alt = gps.alt;
			gps_msg.fix_type = gps.fix_type;
			gps_msg.eph = gps.eph;
			gps_msg.epv = gps.epv;
			gps_msg.sacc = gps.s_variance_m_s;
			gps_msg.vel_m_s = gps.vel_m_s;
			gps_msg.vel_ned[0] = gps.vel_n_m_s;
			gps_msg.vel_ned[1] = gps.vel_e_m_s;
			gps_msg.vel_ned[2] = gps.vel_d_m_s;
			gps_msg.vel_ned_valid = gps.vel_ned_valid;
			gps_msg.nsats = gps.satellites_used;
			//TODO add gdop to gps topic
			gps_msg.gdop = 0.0f;
			_ekf.setGpsData(gps.timestamp, &gps_msg);
		}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

更新GPS数据，在后面是空速计、光流、视觉这里就不再一一粘贴。

// run the EKF update and output
	if (_ekf.update())
1
2

别看着一句话很短 _ekf.update() ，但计算量很大，这个是EKF代码的核心，这个代码在ECL库中，这里先不展开，先把整体框架写完。

	{	//这个括号是对应上面的if (_ekf.update())的
		// integrate time to monitor time slippage  集成时间来监控时间滑动
		if (start_time_us == 0) {
			start_time_us = now;
		
		} else if (start_time_us > 0) {
			integrated_time_us += (uint64_t)((double)sensors.gyro_integral_dt * 1.0e6);//换算成微秒
		}
		matrix::Quaternion<float> q;
		_ekf.copy_quaternion(q.data());
		
		float velocity[3];
		_ekf.get_velocity(velocity);
1
2
3
4
5
6
7
8
9
10
11
12
13

这里定义个时间，用于回放日志时间戳对齐的作用，定义一个四元数、速度用来ekf中计算出来的q和v，这里的作用是为了发布不同的topic填充数据的作用。

			float gyro_rad[3];
			// generate control state data
			control_state_s ctrl_state = {};
			float gyro_bias[3] = {};
			_ekf.get_gyro_bias(gyro_bias);//复制陀螺仪的偏差
			ctrl_state.timestamp = now;//时间戳
			gyro_rad[0] = sensors.gyro_rad[0] - gyro_bias[0];
			gyro_rad[1] = sensors.gyro_rad[1] - gyro_bias[1];
			gyro_rad[2] = sensors.gyro_rad[2] - gyro_bias[2];
			ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad[0]);//数据填充
			ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad[1]);
			ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad[2]);
			ctrl_state.roll_rate_bias = gyro_bias[0];
			ctrl_state.pitch_rate_bias = gyro_bias[1];
			ctrl_state.yaw_rate_bias = gyro_bias[2];

			// Velocity in body frame
			Vector3f v_n(velocity);
			matrix::Dcm<float> R_to_body(q.inversed());
			Vector3f v_b = R_to_body * v_n;
			ctrl_state.x_vel = v_b(0);
			ctrl_state.y_vel = v_b(1);
			ctrl_state.z_vel = v_b(2);


			// Local Position NED
			float position[3];
			_ekf.get_position(position);
			ctrl_state.x_pos = position[0];//数据填充
			ctrl_state.y_pos = position[1];
			ctrl_state.z_pos = position[2];

			// Attitude quaternion
			q.copyTo(ctrl_state.q);

			_ekf.get_quat_reset(&ctrl_state.delta_q_reset[0], &ctrl_state.quat_reset_counter);

			// Acceleration data
			matrix::Vector<float, 3> acceleration(sensors.accelerometer_m_s2);

			float accel_bias[3];
			_ekf.get_accel_bias(accel_bias);//复制加速度误差
			ctrl_state.x_acc = acceleration(0) - accel_bias[0];//数据填充
			ctrl_state.y_acc = acceleration(1) - accel_bias[1];
			ctrl_state.z_acc = acceleration(2) - accel_bias[2];

			// compute lowpass filtered horizontal acceleration
			acceleration = R_to_body.transpose() * acceleration;
			_acc_hor_filt = 0.95f * _acc_hor_filt + 0.05f * sqrtf(acceleration(0) * acceleration(0) +
					acceleration(1) * acceleration(1));
			ctrl_state.horz_acc_mag = _acc_hor_filt;

			ctrl_state.airspeed_valid = false;

			// use estimated velocity for airspeed estimate//用估计的速度估计空速
			if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_MEAS) {
				// use measured airspeed
				if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && now - airspeed.timestamp < 1e6
				    && airspeed.timestamp > 0) {
					ctrl_state.airspeed = airspeed.indicated_airspeed_m_s;
					ctrl_state.airspeed_valid = true;
				}

			} else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_EST) {
				if (_ekf.local_position_is_valid()) {
					ctrl_state.airspeed = sqrtf(velocity[0] * velocity[0] + velocity[1] * velocity[1] + velocity[2] * velocity[2]);
					ctrl_state.airspeed_valid = true;
				}

			} else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_DISABLED) {
				// do nothing, airspeed has been declared as non-valid above, controllers
				// will handle this assuming always trim airspeed
			}

			// publish control state data
			if (_control_state_pub == nullptr) {
				_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);

			} else {
				orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
			}
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

经过一系列的数据填充，最后以消息ID为control_state的topic发布数据，这里我们进去看一下control_state这个结构体都有啥，
Firmware-1.6.0rc1\msg\ control_state.msg

正式上面所填充的有关数据，这里没有截图完这个内容比较多。

			// generate vehicle attitude quaternion data
			struct vehicle_attitude_s att = {};
			att.timestamp = now;

			q.copyTo(att.q);
			att.rollspeed = gyro_rad[0];
			att.pitchspeed = gyro_rad[1];
			att.yawspeed = gyro_rad[2];

			// publish vehicle attitude data
			if (_att_pub == nullptr) {
				_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);

			} else {
                   int j = 0;
                   mhrt_abstime[timei] = hrt_absolute_time();
                    PX4_INFO("hrt_absolute_time = %d",hrt_absolute_time());
                   if(timei == 9)
                   {
                       for(j=0;j<9;j++)
                       {
                          //  PX4_WARN("mhrt_abstime = %d", mhrt_abstime[j]);
                            temp_time = mhrt_abstime[j+1] - mhrt_abstime[j];
                           //PX4_WARN("mhrt_abstime = %d",temp_time/1000);
                       }
                        timei = 0;
                   }
                    timei++;
				orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
			}
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

这里可以发现，每个topic发布前面都会有一个时间戳，这个是为了回放日志的时候时间对其的一个作用，这段代码也很明确，将消息ID为vehicle_attitude的topic发布。
vehicle_attitude，从字面意思来看是载具的姿态，同样看一下这个topic里面都包含什么：

这里正是上面所填充的数据，进行发布。

后面就不再粘贴了，都是数据基本处理，填充数据，以不同的ID发布topic。vehicle_global_position、estimator_status、wind_estimate等等。

这里就把ekf2的框架写完了，但是PX4的EKF最核心的东西还没开始，其最核心的在ECL代码库中的EKF融合算法。

总结：

1. 数据订阅
2. 数据处理（这里核心的还没写）
3. 数据打包填充，按照不同的topic发布出去。

以上就是我对PX4 ekf2_main.cpp的理解，还有很多不理解的地方还在学习中，如有理解错误的地方请批评指正，欢迎一起学习填坑PX4，QQ：1103706199