forked from Archive/PX4-Autopilot
MAVLink: clean up vision timestamps
This commit is contained in:
parent
c0a406b81f
commit
8b3045baa2
|
@ -1459,7 +1459,7 @@ protected:
|
|||
|
||||
if (_pos_sub->update(&_pos_time, &vpos)) {
|
||||
mavlink_vision_position_estimate_t vmsg;
|
||||
vmsg.usec = vpos.timestamp_boot;
|
||||
vmsg.usec = vpos.timestamp;
|
||||
vmsg.x = vpos.x;
|
||||
vmsg.y = vpos.y;
|
||||
vmsg.z = vpos.z;
|
||||
|
|
|
@ -957,14 +957,13 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
|
|||
mavlink_vision_position_estimate_t pos;
|
||||
mavlink_msg_vision_position_estimate_decode(msg, &pos);
|
||||
|
||||
struct vision_position_estimate_s vision_position;
|
||||
memset(&vision_position, 0, sizeof(vision_position));
|
||||
struct vision_position_estimate_s vision_position = {};
|
||||
|
||||
// Use the component ID to identify the vision sensor
|
||||
vision_position.id = msg->compid;
|
||||
|
||||
vision_position.timestamp_boot = sync_stamp(pos.usec);
|
||||
vision_position.timestamp_computer = sync_stamp(pos.usec); // Synced time
|
||||
vision_position.timestamp = sync_stamp(pos.usec);
|
||||
vision_position.timestamp_received = hrt_absolute_time();
|
||||
vision_position.x = pos.x;
|
||||
vision_position.y = pos.y;
|
||||
vision_position.z = pos.z;
|
||||
|
|
Loading…
Reference in New Issue