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

mavlink_receiver: add evh/evv to vision msg

Browse Source 
this will be used by the ekf2 at some point
This commit is contained in:
ChristophTobler
2018-02-15 10:07:03 +01:00
committed by Daniel Agar
parent 12c343317c
commit f9cb575ced
1 changed files with 4 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+4 -2
View File
@@ -1134,6 +1134,8 @@ MavlinkReceiver::handle_message_local_position_ned_cov(mavlink_message_t *msg)
// Low risk change for now. TODO : full covariance matrix
vision_position.eph = sqrtf(fmaxf(pos.covariance[0], pos.covariance[9]));
vision_position.epv = sqrtf(pos.covariance[17]);
vision_position.evh = sqrtf(fmaxf(pos.covariance[24], pos.covariance[30]));
vision_position.evv = sqrtf(pos.covariance[35]);
// set position/velocity invalid if standard deviation is bigger than ev_max_std_dev
const float ev_max_std_dev = 100.0f;
@@ -1142,7 +1144,7 @@ MavlinkReceiver::handle_message_local_position_ned_cov(mavlink_message_t *msg)
vision_position.xy_valid = false;
}
if (sqrtf(fmaxf(pos.covariance[24], pos.covariance[30])) > ev_max_std_dev) {
if (vision_position.evh > ev_max_std_dev) {
vision_position.v_xy_valid = false;
}
@@ -1150,7 +1152,7 @@ MavlinkReceiver::handle_message_local_position_ned_cov(mavlink_message_t *msg)
vision_position.z_valid = false;
}
if (sqrtf(pos.covariance[35]) > ev_max_std_dev) {
if (vision_position.evv > ev_max_std_dev) {
vision_position.v_z_valid = false;
}