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

Pass through horizontal and vertial covariances

Browse Source
This commit is contained in:
Dion Gonano
2018-08-31 21:01:33 +00:00
committed by Daniel Agar
parent 8158cdfcf4
commit 7691990a50
2 changed files with 6 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+2 -1
View File
@@ -1173,8 +1173,9 @@ void Ekf2::run()
ev_data.quat = q; ev_data.quat = q;
// position measurement error from parameters. TODO : use covariances from topic // position measurement error from parameters. TODO : use covariances from topic
ev_data.posErr = fmaxf(_ev_pos_noise.get(), fmaxf(ev_pos.eph, ev_pos.epv)); ev_data.posErr = fmaxf(_ev_pos_noise.get(), ev_pos.eph);
ev_data.angErr = _ev_ang_noise.get(); ev_data.angErr = _ev_ang_noise.get();
ev_data.hgtErr = fmaxf(_ev_pos_noise.get(), ev_pos.epv);
// only set data if all positions and velocities are valid // only set data if all positions and velocities are valid
if (ev_pos.xy_valid && ev_pos.z_valid && ev_pos.v_xy_valid && ev_pos.v_z_valid) { if (ev_pos.xy_valid && ev_pos.z_valid && ev_pos.v_xy_valid && ev_pos.v_z_valid) {
src/modules/mavlink/mavlink_receiver.cpp
+4
View File
@@ -1253,6 +1253,10 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
vision_position.y = pos.y; vision_position.y = pos.y;
vision_position.z = pos.z; vision_position.z = pos.z;
//copy horizontal and vertical covariances
vision_position.eph = fmaxf(pos.covariance[0], pos.covariance[6]);
vision_position.epv = pos.covariance[11];
vision_position.xy_valid = true; vision_position.xy_valid = true;
vision_position.z_valid = true; vision_position.z_valid = true;
vision_position.v_xy_valid = true; vision_position.v_xy_valid = true;