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

add timestamp field to uORB msgs; sync timestamp whenever possible

Browse Source
This commit is contained in:
TSC21
2018-08-09 13:40:48 +02:00
committed by Beat Küng
parent 4a08003952
commit e932030d88
111 changed files with 203 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+3 -3
View File
@@ -3484,7 +3484,7 @@ protected:
if (_debug_sub->update(&_debug_time, &debug)) {
mavlink_named_value_float_t msg = {};
msg.time_boot_ms = debug.timestamp_ms;
msg.time_boot_ms = debug.timestamp * 1e-3f;
memcpy(msg.name, debug.key, sizeof(msg.name));
/* enforce null termination */
msg.name[sizeof(msg.name) - 1] = '\0';
@@ -3553,7 +3553,7 @@ protected:
if (_debug_sub->update(&_debug_time, &debug)) {
mavlink_debug_t msg = {};
msg.time_boot_ms = debug.timestamp_ms;
msg.time_boot_ms = debug.timestamp * 1e-3f;
msg.ind = debug.ind;
msg.value = debug.value;
@@ -3620,7 +3620,7 @@ protected:
if (_debug_sub->update(&_debug_time, &debug)) {
mavlink_debug_vect_t msg = {};
msg.time_usec = debug.timestamp_us;
msg.time_usec = debug.timestamp;
memcpy(msg.name, debug.name, sizeof(msg.name));
/* enforce null termination */
msg.name[sizeof(msg.name) - 1] = '\0';
src/modules/mavlink/mavlink_receiver.cpp
+15 -18
View File
@@ -670,7 +670,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
struct optical_flow_s f = {};
f.timestamp = flow.time_usec;
f.timestamp = _mavlink_timesync.sync_stamp(flow.time_usec);
f.integration_timespan = flow.integration_time_us;
f.pixel_flow_x_integral = flow.integrated_x;
f.pixel_flow_y_integral = flow.integrated_y;
@@ -699,7 +699,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
struct distance_sensor_s d = {};
if (flow.distance > 0.0f) { // negative values signal invalid data
d.timestamp = flow.integration_time_us * 1000; /* ms to us */
d.timestamp = _mavlink_timesync.sync_stamp(flow.integration_time_us * 1000); /* ms to us */
d.min_distance = 0.3f;
d.max_distance = 5.0f;
d.current_distance = flow.distance; /* both are in m */
@@ -908,7 +908,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
bool is_loiter_sp = (bool)(set_position_target_local_ned.type_mask & 0x3000);
bool is_idle_sp = (bool)(set_position_target_local_ned.type_mask & 0x4000);
offboard_control_mode.timestamp = hrt_absolute_time();
offboard_control_mode.timestamp = _mavlink_timesync.sync_stamp(set_position_target_local_ned.time_boot_ms * 1e3f);
if (_offboard_control_mode_pub == nullptr) {
_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode);
@@ -936,7 +936,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.timestamp = _mavlink_timesync.sync_stamp(set_position_target_local_ned.time_boot_ms * 1e3f);
pos_sp_triplet.previous.valid = false;
pos_sp_triplet.next.valid = false;
pos_sp_triplet.current.valid = true;
@@ -1077,7 +1077,7 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
offboard_control_mode.ignore_velocity = true;
offboard_control_mode.ignore_acceleration_force = true;
offboard_control_mode.timestamp = hrt_absolute_time();
offboard_control_mode.timestamp = _mavlink_timesync.sync_stamp(set_actuator_control_target.time_usec * 1e3f);
if (_offboard_control_mode_pub == nullptr) {
_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode);
@@ -1301,7 +1301,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
_offboard_control_mode.ignore_velocity = true;
_offboard_control_mode.ignore_acceleration_force = true;
_offboard_control_mode.timestamp = hrt_absolute_time();
_offboard_control_mode.timestamp = _mavlink_timesync.sync_stamp(set_attitude_target.time_boot_ms * 1e3f);
if (_offboard_control_mode_pub == nullptr) {
_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &_offboard_control_mode);
@@ -1325,7 +1325,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
if (!(_offboard_control_mode.ignore_attitude)) {
vehicle_attitude_setpoint_s att_sp = {};
att_sp.timestamp = hrt_absolute_time();
att_sp.timestamp = _mavlink_timesync.sync_stamp(set_attitude_target.time_boot_ms * 1e3f);
if (!ignore_attitude_msg) { // only copy att sp if message contained new data
matrix::Quatf q(set_attitude_target.q);
@@ -1355,7 +1355,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
///XXX add support for ignoring individual axes
if (!(_offboard_control_mode.ignore_bodyrate)) {
vehicle_rates_setpoint_s rates_sp = {};
rates_sp.timestamp = hrt_absolute_time();
rates_sp.timestamp = _mavlink_timesync.sync_stamp(set_attitude_target.time_boot_ms * 1e3f);
if (!ignore_bodyrate_msg) { // only copy att rates sp if message contained new data
rates_sp.roll = set_attitude_target.body_roll_rate;
@@ -1390,7 +1390,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
tstatus.timestamp = hrt_absolute_time();
tstatus.timestamp = _mavlink_timesync.sync_stamp(tstatus.timestamp);
tstatus.telem_time = tstatus.timestamp;
/* tstatus.heartbeat_time is set by system heartbeats */
tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
@@ -1611,7 +1611,7 @@ MavlinkReceiver::handle_message_obstacle_distance(mavlink_message_t *msg)
mavlink_msg_obstacle_distance_decode(msg, &mavlink_obstacle_distance);
obstacle_distance_s obstacle_distance = {};
obstacle_distance.timestamp = hrt_absolute_time();
obstacle_distance.timestamp = _mavlink_timesync.sync_stamp(mavlink_obstacle_distance.time_usec);
obstacle_distance.sensor_type = mavlink_obstacle_distance.sensor_type;
memcpy(obstacle_distance.distances, mavlink_obstacle_distance.distances, sizeof(obstacle_distance.distances));
obstacle_distance.increment = mavlink_obstacle_distance.increment;
@@ -1635,7 +1635,7 @@ MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_mess
struct vehicle_trajectory_waypoint_s trajectory_waypoint = {};
trajectory_waypoint.timestamp = hrt_absolute_time();
trajectory_waypoint.timestamp = _mavlink_timesync.sync_stamp(trajectory.time_usec);
const int number_valid_points = trajectory.valid_points;
for (int i = 0; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) {
@@ -2121,7 +2121,7 @@ void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg)
mavlink_msg_follow_target_decode(msg, &follow_target_msg);
follow_target_topic.timestamp = hrt_absolute_time();
follow_target_topic.timestamp = _mavlink_timesync.sync_stamp(follow_target_msg.timestamp * 1e3f);
follow_target_topic.lat = follow_target_msg.lat * 1e-7;
follow_target_topic.lon = follow_target_msg.lon * 1e-7;
@@ -2429,8 +2429,7 @@ void MavlinkReceiver::handle_message_named_value_float(mavlink_message_t *msg)
mavlink_msg_named_value_float_decode(msg, &debug_msg);
debug_topic.timestamp = hrt_absolute_time();
debug_topic.timestamp_ms = debug_msg.time_boot_ms;
debug_topic.timestamp = _mavlink_timesync.sync_stamp(debug_msg.time_boot_ms * 1e3f);
memcpy(debug_topic.key, debug_msg.name, sizeof(debug_topic.key));
debug_topic.key[sizeof(debug_topic.key) - 1] = '\0'; // enforce null termination
debug_topic.value = debug_msg.value;
@@ -2450,8 +2449,7 @@ void MavlinkReceiver::handle_message_debug(mavlink_message_t *msg)
mavlink_msg_debug_decode(msg, &debug_msg);
debug_topic.timestamp = hrt_absolute_time();
debug_topic.timestamp_ms = debug_msg.time_boot_ms;
debug_topic.timestamp = _mavlink_timesync.sync_stamp(debug_msg.time_boot_ms * 1e3f);
debug_topic.ind = debug_msg.ind;
debug_topic.value = debug_msg.value;
@@ -2470,8 +2468,7 @@ void MavlinkReceiver::handle_message_debug_vect(mavlink_message_t *msg)
mavlink_msg_debug_vect_decode(msg, &debug_msg);
debug_topic.timestamp = hrt_absolute_time();
debug_topic.timestamp_us = debug_msg.time_usec;
debug_topic.timestamp = _mavlink_timesync.sync_stamp(debug_msg.time_usec);
memcpy(debug_topic.name, debug_msg.name, sizeof(debug_topic.name));
debug_topic.name[sizeof(debug_topic.name) - 1] = '\0'; // enforce null termination
debug_topic.x = debug_msg.x;
src/modules/uavcan/libuavcan
+1 -1
Submodule src/modules/uavcan/libuavcan updated: c044630390...dfcdf22eda