Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: support covariance field in vis pose msg

This commit is contained in:
chobits 2020-06-04 10:55:08 +08:00 committed by Randy Mackay
parent e64c92b322
commit 991387dbfb
2 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/GCS_MAVLink/GCS.h
View File

@ -783,6 +783,7 @@ private:
const float roll,
const float pitch,
const float yaw,
const float covariance[21],
const uint8_t reset_counter,
const uint16_t payload_size);
void handle_vision_speed_estimate(const mavlink_message_t &msg);

17
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -2941,7 +2941,7 @@ void GCS_MAVLINK::handle_vision_position_estimate(const mavlink_message_t &msg)
mavlink_vision_position_estimate_t m;
mavlink_msg_vision_position_estimate_decode(&msg, &m);
handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.reset_counter,
handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.covariance, m.reset_counter,
PAYLOAD_SIZE(chan, VISION_POSITION_ESTIMATE));
}
@ -2950,7 +2950,7 @@ void GCS_MAVLINK::handle_global_vision_position_estimate(const mavlink_message_t
mavlink_global_vision_position_estimate_t m;
mavlink_msg_global_vision_position_estimate_decode(&msg, &m);
handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.reset_counter,
handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.covariance, m.reset_counter,
PAYLOAD_SIZE(chan, GLOBAL_VISION_POSITION_ESTIMATE));
}
@ -2960,7 +2960,7 @@ void GCS_MAVLINK::handle_vicon_position_estimate(const mavlink_message_t &msg)
mavlink_msg_vicon_position_estimate_decode(&msg, &m);
// vicon position estimate does not include reset counter
handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, 0,
handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw, m.covariance, 0,
PAYLOAD_SIZE(chan, VICON_POSITION_ESTIMATE));
}
@ -2974,10 +2974,13 @@ void GCS_MAVLINK::handle_common_vision_position_estimate_data(const uint64_t use
const float roll,
const float pitch,
const float yaw,
const float covariance[21],
const uint8_t reset_counter,
const uint16_t payload_size)
{
#if HAL_VISUALODOM_ENABLED
float posErr = 0;
float angErr = 0;
// correct offboard timestamp to be in local ms since boot
uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(usec, payload_size);
@ -2985,7 +2988,13 @@ void GCS_MAVLINK::handle_common_vision_position_estimate_data(const uint64_t use
if (visual_odom == nullptr) {
return;
}
visual_odom->handle_vision_position_estimate(usec, timestamp_ms, x, y, z, roll, pitch, yaw, reset_counter);
if (!isnan(covariance[0])) {
posErr = cbrtf(sq(covariance[0])+sq(covariance[6])+sq(covariance[11]));
angErr = cbrtf(sq(covariance[15])+sq(covariance[18])+sq(covariance[20]));
}
visual_odom->handle_vision_position_estimate(usec, timestamp_ms, x, y, z, roll, pitch, yaw, posErr, angErr, reset_counter);
#endif
}