fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-13 23:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Feature: VIO: add ODOMETRY stream (#11084)

Browse Source 
* mavlink_messages: remove LOCAL_POSITION_NED_COV stream

* mavlink_messages.cpp: add ODOMETRY stream

* add MAV_ODOM_LP parameter to activate odometry loopback

* EKF2: add vehicle_odometry publisher

* Replace VISION_POSITION_ESTIMATE loopback with ODOMETRY

* LPE: add vehicle_odometry publisher

* set vehicle_odometry local_frame field

* mavlink_messages.cpp: ODOMETRY frame_id depends on MAV_ODOM_LP
This commit is contained in:
Nuno Marques
2018-12-21 17:54:04 +00:00
committed by James Goppert
parent b3018646d5
commit cfd1be584e
10 changed files with 250 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+62
View File
@@ -283,6 +283,7 @@ private:
uORB::Publication<vehicle_local_position_s> _vehicle_local_position_pub;
uORB::Publication<vehicle_global_position_s> _vehicle_global_position_pub;
uORB::Publication<vehicle_odometry_s> _vehicle_odometry_pub;
Ekf _ekf;
@@ -509,6 +510,7 @@ Ekf2::Ekf2():
_perf_ekf_update(perf_alloc_once(PC_ELAPSED, "EKF2 update")),
_vehicle_local_position_pub(ORB_ID(vehicle_local_position)),
_vehicle_global_position_pub(ORB_ID(vehicle_global_position)),
_vehicle_odometry_pub(ORB_ID(vehicle_odometry)),
_params(_ekf.getParamHandle()),
_obs_dt_min_ms(_params->sensor_interval_min_ms),
_mag_delay_ms(_params->mag_delay_ms),
@@ -1297,7 +1299,13 @@ void Ekf2::run()
// generate vehicle local position data
vehicle_local_position_s &lpos = _vehicle_local_position_pub.get();
// generate vehicle odometry data
vehicle_odometry_s &odom = _vehicle_odometry_pub.get();
lpos.timestamp = now;
odom.timestamp = lpos.timestamp;
odom.local_frame = odom.LOCAL_FRAME_NED;
// Position of body origin in local NED frame
float position[3];
@@ -1308,6 +1316,11 @@ void Ekf2::run()
lpos.y = (_ekf.local_position_is_valid()) ? position[1] : 0.0f;
lpos.z = position[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)
float velocity[3];
_ekf.get_velocity(velocity);
@@ -1315,6 +1328,11 @@ void Ekf2::run()
lpos.vy = velocity[1];
lpos.vz = velocity[2];
// Vehicle odometry linear velocity
odom.vx = lpos.vx;
odom.vy = lpos.vy;
odom.vz = lpos.vz;
// vertical position time derivative (m/s)
_ekf.get_pos_d_deriv(&lpos.z_deriv);
@@ -1352,6 +1370,16 @@ void Ekf2::run()
lpos.yaw = matrix::Eulerf(q).psi();
// Vehicle odometry quaternion
q.copyTo(odom.q);
// Vehicle odometry angular rates
float gyro_bias[3];
_ekf.get_gyro_bias(gyro_bias);
odom.rollspeed = sensors.gyro_rad[0] - gyro_bias[0];
odom.pitchspeed = sensors.gyro_rad[1] - gyro_bias[1];
odom.yawspeed = sensors.gyro_rad[2] - gyro_bias[2];
lpos.dist_bottom_valid = _ekf.get_terrain_valid();
float terrain_vpos;
@@ -1394,9 +1422,43 @@ void Ekf2::run()
lpos.hagl_max = INFINITY;
}
// Get covariances to vehicle odometry
float covariances[24];
_ekf.get_covariances(covariances);
// get the covariance matrix size
const size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
const size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
// initially set pose covariances to 0
for (size_t i = 0; i < POS_URT_SIZE; i++) {
odom.pose_covariance[i] = 0.0;
}
// set the position variances
odom.pose_covariance[odom.COVARIANCE_MATRIX_X_VARIANCE] = covariances[7];
odom.pose_covariance[odom.COVARIANCE_MATRIX_Y_VARIANCE] = covariances[8];
odom.pose_covariance[odom.COVARIANCE_MATRIX_Z_VARIANCE] = covariances[9];
// TODO: implement propagation from quaternion covariance to Euler angle covariance
// by employing the covariance law
// initially set velocity covariances to 0
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
odom.velocity_covariance[i] = 0.0;
}
// set the linear velocity variances
odom.velocity_covariance[odom.COVARIANCE_MATRIX_VX_VARIANCE] = covariances[4];
odom.velocity_covariance[odom.COVARIANCE_MATRIX_VY_VARIANCE] = covariances[5];
odom.velocity_covariance[odom.COVARIANCE_MATRIX_VZ_VARIANCE] = covariances[6];
// publish vehicle local position data
_vehicle_local_position_pub.update();
// publish vehicle odometry data
_vehicle_odometry_pub.update();
if (_ekf.global_position_is_valid() && !_preflt_fail) {
// generate and publish global position data
vehicle_global_position_s &global_pos = _vehicle_global_position_pub.get();