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

GCS_MAVLink: send Odometry quality to VisualOdom

This commit is contained in:
Randy Mackay 2024-02-20 16:09:08 +09:00 committed by Andrew Tridgell
parent d1360c14f8
commit 442c90ff1b
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -3694,12 +3694,12 @@ void GCS_MAVLINK::handle_odometry(const mavlink_message_t &msg)
} }
const uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(m.time_usec, PAYLOAD_SIZE(chan, ODOMETRY)); const uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(m.time_usec, PAYLOAD_SIZE(chan, ODOMETRY));
visual_odom->handle_pose_estimate(m.time_usec, timestamp_ms, m.x, m.y, m.z, q, posErr, angErr, m.reset_counter); visual_odom->handle_pose_estimate(m.time_usec, timestamp_ms, m.x, m.y, m.z, q, posErr, angErr, m.reset_counter, m.quality);
// convert velocity vector from FRD to NED frame // convert velocity vector from FRD to NED frame
Vector3f vel{m.vx, m.vy, m.vz}; Vector3f vel{m.vx, m.vy, m.vz};
vel = q * vel; vel = q * vel;
visual_odom->handle_vision_speed_estimate(m.time_usec, timestamp_ms, vel, m.reset_counter); visual_odom->handle_vision_speed_estimate(m.time_usec, timestamp_ms, vel, m.reset_counter, m.quality);
} }
// there are several messages which all have identical fields in them. // there are several messages which all have identical fields in them.
@ -3731,7 +3731,7 @@ void GCS_MAVLINK::handle_common_vision_position_estimate_data(const uint64_t use
angErr = cbrtf(sq(covariance[15])+sq(covariance[18])+sq(covariance[20])); angErr = cbrtf(sq(covariance[15])+sq(covariance[18])+sq(covariance[20]));
} }
visual_odom->handle_pose_estimate(usec, timestamp_ms, x, y, z, roll, pitch, yaw, posErr, angErr, reset_counter); visual_odom->handle_pose_estimate(usec, timestamp_ms, x, y, z, roll, pitch, yaw, posErr, angErr, reset_counter, 0);
} }
void GCS_MAVLINK::handle_att_pos_mocap(const mavlink_message_t &msg) void GCS_MAVLINK::handle_att_pos_mocap(const mavlink_message_t &msg)
@ -3747,7 +3747,7 @@ void GCS_MAVLINK::handle_att_pos_mocap(const mavlink_message_t &msg)
return; return;
} }
// note: att_pos_mocap does not include reset counter // note: att_pos_mocap does not include reset counter
visual_odom->handle_pose_estimate(m.time_usec, timestamp_ms, m.x, m.y, m.z, m.q, 0, 0, 0); visual_odom->handle_pose_estimate(m.time_usec, timestamp_ms, m.x, m.y, m.z, m.q, 0, 0, 0, 0);
} }
void GCS_MAVLINK::handle_vision_speed_estimate(const mavlink_message_t &msg) void GCS_MAVLINK::handle_vision_speed_estimate(const mavlink_message_t &msg)
@ -3760,7 +3760,7 @@ void GCS_MAVLINK::handle_vision_speed_estimate(const mavlink_message_t &msg)
mavlink_msg_vision_speed_estimate_decode(&msg, &m); mavlink_msg_vision_speed_estimate_decode(&msg, &m);
const Vector3f vel = {m.x, m.y, m.z}; const Vector3f vel = {m.x, m.y, m.z};
uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(m.usec, PAYLOAD_SIZE(chan, VISION_SPEED_ESTIMATE)); uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(m.usec, PAYLOAD_SIZE(chan, VISION_SPEED_ESTIMATE));
visual_odom->handle_vision_speed_estimate(m.usec, timestamp_ms, vel, m.reset_counter); visual_odom->handle_vision_speed_estimate(m.usec, timestamp_ms, vel, m.reset_counter, 0);
} }
#endif // HAL_VISUALODOM_ENABLED #endif // HAL_VISUALODOM_ENABLED