写在前面
源码版本：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;
}
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;
}
函数中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();
}
跟进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 = {};
函数开头，订阅了一堆数据用于后面计算使用，更新参数，定义结构体。订阅的数据根据消息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;
}
这里定义的两组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;
定义一些标志位。
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);
}
上面就是检查更新的部分了，例如：检查（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();
}
这里获取绝对时间，用于回放时，各种传感器的实际时间戳
// 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;
将陀螺、加计的数据进行积分，并压栈储存。
_ekf.setIMUData(now, sensors.gyro_integral_dt * 1.e6f, sensors.accelerometer_integral_dt * 1.e6f,
gyro_integral, accel_integral);
凡是带有 _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;
}
// 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;
}
}
}
同样最后将磁力计数据压栈储存_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;
}
}
}
读取气压计并储存。
// 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);
}
更新GPS数据，在后面是空速计、光流、视觉这里就不再一一粘贴。
// run the EKF update and output
if (_ekf.update())
别看着一句话很短 _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);
这里定义个时间，用于回放日志时间戳对齐的作用，定义一个四元数、速度用来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);
}
经过一系列的数据填充，最后以消息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);
}
这里可以发现，每个topic发布前面都会有一个时间戳，这个是为了回放日志的时候时间对其的一个作用，这段代码也很明确，将消息ID为vehicle_attitude的topic发布。
vehicle_attitude，从字面意思来看是载具的姿态，同样看一下这个topic里面都包含什么：
这里正是上面所填充的数据，进行发布。
后面就不再粘贴了，都是数据基本处理，填充数据，以不同的ID发布topic。vehicle_global_position、estimator_status、wind_estimate等等。
这里就把ekf2的框架写完了，但是PX4的EKF最核心的东西还没开始，其最核心的在ECL代码库中的EKF融合算法。
总结：
- 数据订阅
- 数据处理（这里核心的还没写）
- 数据打包填充，按照不同的topic发布出去。
以上就是我对PX4 ekf2_main.cpp的理解，还有很多不理解的地方还在学习中，如有理解错误的地方请批评指正，欢迎一起学习填坑PX4，QQ：1103706199
扫一扫
1726
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
