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

SITL: vicon odometry corrected

This commit is contained in:
Randy Mackay 2022-11-25 08:22:21 +09:00
parent 6dbc4c657a
commit 9fae8b4376
1 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/SITL/SIM_Vicon.cpp
View File

@ -210,6 +210,7 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
// send ODOMETRY message // send ODOMETRY message
if (should_send(ViconTypeMask::ODOMETRY) && get_free_msg_buf_index(msg_buf_index)) { if (should_send(ViconTypeMask::ODOMETRY) && get_free_msg_buf_index(msg_buf_index)) {
const Vector3f vel_corrected_frd = attitude.inverse() * vel_corrected;
mavlink_msg_odometry_pack_chan( mavlink_msg_odometry_pack_chan(
system_id, system_id,
component_id, component_id,
@ -222,9 +223,9 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
pos_corrected.y, pos_corrected.y,
pos_corrected.z, pos_corrected.z,
&attitude[0], &attitude[0],
vel_corrected.x, vel_corrected_frd.x,
vel_corrected.y, vel_corrected_frd.y,
vel_corrected.z, vel_corrected_frd.z,
gyro.x, gyro.x,
gyro.y, gyro.y,
gyro.z, gyro.z,