px4+vins+ego单机鲁棒飞行二-1（更改px4外部视觉估计固件）

前提：固件1.11.2

一、EKF2源码-获取视觉里程计信息

位置：PX4-Autopolot/src/modules/ekf2/ekf2_main.cpp中void Ekf2::Run()函数，大概在第1100行

		// get external vision data
		// if error estimates are unavailable, use parameter defined defaults
		new_ev_data_received = false;

		if (_ev_odom_sub.updated()) {
			new_ev_data_received = true;

			// copy both attitude & position, we need both to fill a single extVisionSample
			_ev_odom_sub.copy(&_ev_odom);

			extVisionSample ev_data {};

			// check for valid velocity data
			if (PX4_ISFINITE(_ev_odom.vx) && PX4_ISFINITE(_ev_odom.vy) && PX4_ISFINITE(_ev_odom.vz)) {
				ev_data.vel(0) = _ev_odom.vx;
				ev_data.vel(1) = _ev_odom.vy;
				ev_data.vel(2) = _ev_odom.vz;

				if (_ev_odom.velocity_frame == vehicle_odometry_s::BODY_FRAME_FRD) {
					ev_data.vel_frame = estimator::BODY_FRAME_FRD;

				} else {
					ev_data.vel_frame = estimator::LOCAL_FRAME_FRD;
				}

				// velocity measurement error from ev_data or parameters
				float param_evv_noise_var = sq(_param_ekf2_evv_noise.get());

				if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(_ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VX_VARIANCE])
				    && PX4_ISFINITE(_ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VY_VARIANCE])
				    && PX4_ISFINITE(_ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE])) {
					ev_data.velCov(0, 0) = _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VX_VARIANCE];
					ev_data.velCov(0, 1) = ev_data.velCov(1, 0) = _ev_odom.velocity_covariance[1];
					ev_data.velCov(0, 2) = ev_data.velCov(2, 0) = _ev_odom.velocity_covariance[2];
					ev_data.velCov(1, 1) = _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VY_VARIANCE];
					ev_data.velCov(1, 2) = ev_data.velCov(2, 1) = _ev_odom.velocity_covariance[7];
					ev_data.velCov(2, 2) = _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE];

				} else {
					ev_data.velCov = matrix::eye<float, 3>() * param_evv_noise_var;
				}
			}

			// check for valid position data
			if (PX4_ISFINITE(_ev_odom.x) && PX4_ISFINITE(_ev_odom.y) && PX4_ISFINITE(_ev_odom.z)) {
				ev_data.pos(0) = _ev_odom.x;
				ev_data.pos(1) = _ev_odom.y;
				ev_data.pos(2) = _ev_odom.z;

				float param_evp_noise_var = sq(_param_ekf2_evp_noise.get());

				// position measurement error from ev_data or parameters
				if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_X_VARIANCE])
				    && PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_Y_VARIANCE])
				    && PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_Z_VARIANCE])) {
					ev_data.posVar(0) = fmaxf(param_evp_noise_var, _ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_X_VARIANCE]);
					ev_data.posVar(1) = fmaxf(param_evp_noise_var, _ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_Y_VARIANCE]);
					ev_data.posVar(2) = fmaxf(param_evp_noise_var, _ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_Z_VARIANCE]);

				} else {
					ev_data.posVar.setAll(param_evp_noise_var);
				}
			}

			// check for valid orientation data
			if (PX4_ISFINITE(_ev_odom.q[0])) {
				ev_data.quat = matrix::Quatf(_ev_odom.q);

				// orientation measurement error from ev_data or parameters
				float param_eva_noise_var = sq(_param_ekf2_eva_noise.get());

				if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE])) {
					ev_data.angVar = fmaxf(param_eva_noise_var, _ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE]);

				} else {
					ev_data.angVar = param_eva_noise_var;
				}
			}

			// use timestamp from external computer, clocks are synchronized when using MAVROS
			ev_data.time_us = _ev_odom.timestamp_sample;
			_ekf.setExtVisionData(ev_data);

			ekf2_timestamps.visual_odometry_timestamp_rel = (int16_t)((int64_t)_ev_odom.timestamp / 100 -
					(int64_t)ekf2_timestamps.timestamp / 100);
		}

		bool vehicle_land_detected_updated = _vehicle_land_detected_sub.updated();

		if (vehicle_land_detected_updated) {
			if (_vehicle_land_detected_sub.copy(&_vehicle_land_detected)) {
				_ekf.set_in_air_status(!_vehicle_land_detected.landed);
			}
		}

最终使用函数_ekf.setExtVisionData(ev_data)将外部数据数据传入。

二、EKF2源码-设置外部视觉数据

位置：PX4-Autopolot/src/modules/ekf2/estimator_interface.cpp中大概在400行的位置

// set attitude and position data derived from an external vision system
void EstimatorInterface::setExtVisionData(const extVisionSample& evdata)
{
	if (!_initialised || _ev_buffer_fail) {
		return;
	}

	// Allocate the required buffer size if not previously done
	// Do not retry if allocation has failed previously
	if (_ext_vision_buffer.get_length() < _obs_buffer_length) {
		_ev_buffer_fail = !_ext_vision_buffer.allocate(_obs_buffer_length);

		if (_ev_buffer_fail) {
			printBufferAllocationFailed("vision");
			return;
		}
	}

	// limit data rate to prevent data being lost
	if ((evdata.time_us - _time_last_ext_vision) > _min_obs_interval_us) {
		_time_last_ext_vision = evdata.time_us;

		extVisionSample ev_sample_new = evdata;
		// calculate the system time-stamp for the mid point of the integration period
		ev_sample_new.time_us -= _params.ev_delay_ms * 1000;
		ev_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;

		_ext_vision_buffer.push(ev_sample_new);
	}
}

最后一行将数据压入_ext_vision_buffer中。

三、源码中对位置的发送

位置：PX4-Autopolot/src/modules/ekf2/ekf2_main.cpp中void Ekf2::Run()函数，大概在第1210行

		// run the EKF update and output
		perf_begin(_ekf_update_perf);
		const bool ekf_updated = _ekf.update();
		perf_end(_ekf_update_perf);

		// integrate time to monitor time slippage
		if (_start_time_us == 0) {
			_start_time_us = now;
			_last_time_slip_us = 0;

		} else if (_start_time_us > 0) {
			_integrated_time_us += imu_dt;
			_last_time_slip_us = (now - _start_time_us) - _integrated_time_us;
		}

		if (ekf_updated) {

			filter_control_status_u control_status;
			_ekf.get_control_mode(&control_status.value);

			// only publish position after successful alignment
			if (control_status.flags.tilt_align) {
				// generate vehicle local position data
				vehicle_local_position_s &lpos = _vehicle_local_position_pub.get();

				// generate vehicle odometry data
				vehicle_odometry_s odom{};

				lpos.timestamp = now;

				odom.timestamp = hrt_absolute_time();
				odom.timestamp_sample = now;

				odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;

				// Position of body origin in local NED frame
				Vector3f position = _ekf.getPosition();
				const float lpos_x_prev = lpos.x;
				const float lpos_y_prev = lpos.y;
				lpos.x = (_ekf.local_position_is_valid()) ? position(0) : 0.0f;
				lpos.y = (_ekf.local_position_is_valid()) ? position(1) : 0.0f;
				lpos.z = position(2);

				lpos.x = ev_data.pos(0);
				lpos.y = ev_data.pos(1);
				lpos.z = ev_data.pos(2);

				// Vehicle odometry position
				odom.x = lpos.x;
				odom.y = lpos.y;
				odom.z = lpos.z;

				// Velocity of body origin in local NED frame (m/s)
				const Vector3f velocity = _ekf.getVelocity();
				lpos.vx = velocity(0);
				lpos.vy = velocity(1);
				lpos.vz = velocity(2);

				// Vehicle odometry linear velocity
				odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
				odom.vx = lpos.vx;
				odom.vy = lpos.vy;
				odom.vz = lpos.vz;

				// vertical position time derivative (m/s)
				lpos.z_deriv = _ekf.getVerticalPositionDerivative();

				// Acceleration of body origin in local frame
				Vector3f vel_deriv = _ekf.getVelocityDerivative();
				lpos.ax = vel_deriv(0);
				lpos.ay = vel_deriv(1);
				lpos.az = vel_deriv(2);

下面三行代码是我自己添加的，目的就是直接使用外部视觉估计的位置

lpos.x = ev_data.pos(0);
lpos.y = ev_data.pos(1);
lpos.z = ev_data.pos(2);

四、测试

1. 这样改不太行，会出现critical navigation failure的错误。