forked from Archive/PX4-Autopilot
position_setpoint_triplet topic: set the timestamp when publishing
This commit is contained in:
parent
89a7e0cf87
commit
3a084fbdb8
|
@ -788,6 +788,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
|
|||
} else {
|
||||
/* It's not a pure force setpoint: publish to setpoint triplet topic */
|
||||
struct position_setpoint_triplet_s pos_sp_triplet = {};
|
||||
pos_sp_triplet.timestamp = hrt_absolute_time();
|
||||
pos_sp_triplet.previous.valid = false;
|
||||
pos_sp_triplet.next.valid = false;
|
||||
pos_sp_triplet.current.valid = true;
|
||||
|
|
|
@ -622,6 +622,7 @@ Navigator::task_main()
|
|||
}
|
||||
|
||||
if (_pos_sp_triplet_updated) {
|
||||
_pos_sp_triplet.timestamp = hrt_absolute_time();
|
||||
publish_position_setpoint_triplet();
|
||||
_pos_sp_triplet_updated = false;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue