aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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)) {
const Vector3f vel_corrected_frd = attitude.inverse() * vel_corrected;
const mavlink_odometry_t odometry{
now_us + time_offset_us,
float(pos_corrected.x),
float(pos_corrected.y),
float(pos_corrected.z),
{attitude[0], attitude[1], attitude[2], attitude[3]},
vel_corrected_frd.x,
vel_corrected_frd.y,
vel_corrected_frd.z,
gyro.x,
gyro.y,
gyro.z,
{},
{},
MAV_FRAME_LOCAL_FRD,
MAV_FRAME_BODY_FRD,
0,
MAV_ESTIMATOR_TYPE_VIO,
50 // quality hardcoded to 50%
time_usec: now_us + time_offset_us,
x: float(pos_corrected.x),
y: float(pos_corrected.y),
z: float(pos_corrected.z),
q: {attitude[0], attitude[1], attitude[2], attitude[3]},
vx: vel_corrected_frd.x,
vy: vel_corrected_frd.y,
vz: vel_corrected_frd.z,
rollspeed: gyro.x,
pitchspeed: gyro.y,
yawspeed: gyro.z,
pose_covariance: {},
velocity_covariance: {},
frame_id: MAV_FRAME_LOCAL_FRD,
child_frame_id: MAV_FRAME_BODY_FRD,
reset_counter: 0,
estimator_type: MAV_ESTIMATOR_TYPE_VIO,
quality: 50, // quality hardcoded to 50%
};
mavlink_msg_odometry_encode_status(
system_id,