fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mocap timestamp cleanup (#5021)

Browse Source
This commit is contained in:
tommises 2016-07-11 04:46:29 -06:00 committed by Lorenz Meier
parent c9652fd42a
commit 4fa2c54485
4 changed files with 6 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/att_pos_mocap.msg
View File

@ -1,7 +1,6 @@
uint32 id # ID of the estimator, commonly the component ID of the incoming message
uint64 timestamp_boot # time of this estimate, in microseconds since system start
uint64 timestamp_computer # timestamp provided by the companion computer, in us
uint64 timestamp_received # timestamp when the estimate was received
float32[4] q # Estimated attitude as quaternion

2
src/modules/local_position_estimator/sensors/mocap.cpp
View File

@ -47,7 +47,7 @@ int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
y(Y_mocap_y) = _sub_mocap.get().y;
y(Y_mocap_z) = _sub_mocap.get().z;
_mocapStats.update(y);
_time_last_mocap = _sub_mocap.get().timestamp_boot;
_time_last_mocap = _sub_mocap.get().timestamp;
return OK;
}

2
src/modules/mavlink/mavlink_messages.cpp
View File

@ -1741,7 +1741,7 @@ protected:
if (_mocap_sub->update(&_mocap_time, &mocap)) {
mavlink_att_pos_mocap_t msg;
msg.time_usec = mocap.timestamp_boot;
msg.time_usec = mocap.timestamp;
msg.q[0] = mocap.q[0];
msg.q[1] = mocap.q[1];
msg.q[2] = mocap.q[2];

7
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -677,14 +677,13 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
mavlink_att_pos_mocap_t mocap;
mavlink_msg_att_pos_mocap_decode(msg, &mocap);
struct att_pos_mocap_s att_pos_mocap;
memset(&att_pos_mocap, 0, sizeof(att_pos_mocap));
struct att_pos_mocap_s att_pos_mocap = {};
// Use the component ID to identify the mocap system
att_pos_mocap.id = msg->compid;
att_pos_mocap.timestamp_boot = sync_stamp(mocap.time_usec);
att_pos_mocap.timestamp_computer = sync_stamp(mocap.time_usec); // Synced time
att_pos_mocap.timestamp = sync_stamp(mocap.time_usec);
att_pos_mocap.timestamp_received = hrt_absolute_time();
att_pos_mocap.q[0] = mocap.q[0];
att_pos_mocap.q[1] = mocap.q[1];