Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

SIM_Vicon: label fields going into ODOMETRY packets

Browse Source
This commit is contained in:
Peter Barker 2024-03-07 13:14:35 +11:00 committed by Andrew Tridgell
parent d657f40a41
commit 298ef64836
1 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/SITL/SIM_Vicon.cpp
View File

@ -217,24 +217,24 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
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; const Vector3f vel_corrected_frd = attitude.inverse() * vel_corrected;
const mavlink_odometry_t odometry{ const mavlink_odometry_t odometry{
now_us + time_offset_us, time_usec: now_us + time_offset_us,
float(pos_corrected.x), x: float(pos_corrected.x),
float(pos_corrected.y), y: float(pos_corrected.y),
float(pos_corrected.z), z: float(pos_corrected.z),
{attitude[0], attitude[1], attitude[2], attitude[3]}, q: {attitude[0], attitude[1], attitude[2], attitude[3]},
vel_corrected_frd.x, vx: vel_corrected_frd.x,
vel_corrected_frd.y, vy: vel_corrected_frd.y,
vel_corrected_frd.z, vz: vel_corrected_frd.z,
gyro.x, rollspeed: gyro.x,
gyro.y, pitchspeed: gyro.y,
gyro.z, yawspeed: gyro.z,
{}, pose_covariance: {},
{}, velocity_covariance: {},
MAV_FRAME_LOCAL_FRD, frame_id: MAV_FRAME_LOCAL_FRD,
MAV_FRAME_BODY_FRD, child_frame_id: MAV_FRAME_BODY_FRD,
0, reset_counter: 0,
MAV_ESTIMATOR_TYPE_VIO, estimator_type: MAV_ESTIMATOR_TYPE_VIO,
50 // quality hardcoded to 50% quality: 50, // quality hardcoded to 50%
}; };
mavlink_msg_odometry_encode_status( mavlink_msg_odometry_encode_status(
system_id, system_id,