Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: correct consumption of ODOMETRY velocity

Browse Source
This commit is contained in:
Randy Mackay 2022-11-24 19:34:36 +09:00
parent e9b8948ade
commit 6dbc4c657a
1 changed files with 3 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -3381,7 +3381,9 @@ 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));
visual_odom->handle_vision_position_estimate(m.time_usec, timestamp_ms, m.x, m.y, m.z, q, posErr, angErr, m.reset_counter);
const Vector3f vel{m.vx, m.vy, m.vz};
// convert velocity vector from FRD to NED frame
Vector3f vel{m.vx, m.vy, m.vz};
vel = q * vel;
visual_odom->handle_vision_speed_estimate(m.time_usec, timestamp_ms, vel, m.reset_counter);
#endif
}