yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: switch back from _mavlink_timesync.sync_stamp to hrt_absolute_time

Browse Source 
- the timestamp is only used for logging and log analysis. For that it's
  important to have the timestamp when a setpoint becomes active.
- there was a consistent problem with the position_setpoint_triplet
  timestamp, where the timestamp was just bogus. Timesync seems to work
  correctly though. Might be a problem on the sender side?
  For example here:
  https://logs.px4.io/plot_app?log=41918a7d-4c1d-464d-9abe-aef2c0818d92
This commit is contained in:
Beat Küng
2018-08-22 08:03:03 +02:00
committed by Lorenz Meier
parent 0935354c0a
commit b6b935026a
2 changed files with 17 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -3560,7 +3560,7 @@ protected:
if (_debug_sub->update(&_debug_time, &debug)) { if (_debug_sub->update(&_debug_time, &debug)) {
mavlink_named_value_float_t msg = {}; mavlink_named_value_float_t msg = {};
msg.time_boot_ms = debug.timestamp * 1e-3f; msg.time_boot_ms = debug.timestamp / 1000ULL;
memcpy(msg.name, debug.key, sizeof(msg.name)); memcpy(msg.name, debug.key, sizeof(msg.name));
/* enforce null termination */ /* enforce null termination */
msg.name[sizeof(msg.name) - 1] = '\0'; msg.name[sizeof(msg.name) - 1] = '\0';
@@ -3629,7 +3629,7 @@ protected:
if (_debug_sub->update(&_debug_time, &debug)) { if (_debug_sub->update(&_debug_time, &debug)) {
mavlink_debug_t msg = {}; mavlink_debug_t msg = {};
msg.time_boot_ms = debug.timestamp * 1e-3f; msg.time_boot_ms = debug.timestamp / 1000ULL;
msg.ind = debug.ind; msg.ind = debug.ind;
msg.value = debug.value; msg.value = debug.value;

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

@@ -894,7 +894,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_loiter_sp = (bool)(set_position_target_local_ned.type_mask & 0x3000);
bool is_idle_sp = (bool)(set_position_target_local_ned.type_mask & 0x4000); bool is_idle_sp = (bool)(set_position_target_local_ned.type_mask & 0x4000);
offboard_control_mode.timestamp = _mavlink_timesync.sync_stamp(set_position_target_local_ned.time_boot_ms * 1e3f); offboard_control_mode.timestamp = hrt_absolute_time();
if (_offboard_control_mode_pub == nullptr) { if (_offboard_control_mode_pub == nullptr) {
_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode); _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode);
@@ -922,7 +922,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
} else { } else {
/* It's not a pure force setpoint: publish to setpoint triplet topic */ /* It's not a pure force setpoint: publish to setpoint triplet topic */
struct position_setpoint_triplet_s pos_sp_triplet = {}; struct position_setpoint_triplet_s pos_sp_triplet = {};
pos_sp_triplet.timestamp = _mavlink_timesync.sync_stamp(set_position_target_local_ned.time_boot_ms * 1e3f); pos_sp_triplet.timestamp = hrt_absolute_time();
pos_sp_triplet.previous.valid = false; pos_sp_triplet.previous.valid = false;
pos_sp_triplet.next.valid = false; pos_sp_triplet.next.valid = false;
pos_sp_triplet.current.valid = true; pos_sp_triplet.current.valid = true;
@@ -1063,7 +1063,7 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
offboard_control_mode.ignore_velocity = true; offboard_control_mode.ignore_velocity = true;
offboard_control_mode.ignore_acceleration_force = true; offboard_control_mode.ignore_acceleration_force = true;
offboard_control_mode.timestamp = _mavlink_timesync.sync_stamp(set_actuator_control_target.time_usec * 1e3f); offboard_control_mode.timestamp = hrt_absolute_time();
if (_offboard_control_mode_pub == nullptr) { if (_offboard_control_mode_pub == nullptr) {
_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode); _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)
_offboard_control_mode.ignore_velocity = true; _offboard_control_mode.ignore_velocity = true;
_offboard_control_mode.ignore_acceleration_force = true; _offboard_control_mode.ignore_acceleration_force = true;
_offboard_control_mode.timestamp = _mavlink_timesync.sync_stamp(set_attitude_target.time_boot_ms * 1e3f); _offboard_control_mode.timestamp = hrt_absolute_time();
if (_offboard_control_mode_pub == nullptr) { if (_offboard_control_mode_pub == nullptr) {
_offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &_offboard_control_mode); _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &_offboard_control_mode);
@@ -1349,7 +1349,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
/* Publish attitude setpoint if attitude and thrust ignore bits are not set */ /* Publish attitude setpoint if attitude and thrust ignore bits are not set */
if (!(_offboard_control_mode.ignore_attitude)) { if (!(_offboard_control_mode.ignore_attitude)) {
vehicle_attitude_setpoint_s att_sp = {}; vehicle_attitude_setpoint_s att_sp = {};
att_sp.timestamp = _mavlink_timesync.sync_stamp(set_attitude_target.time_boot_ms * 1e3f); att_sp.timestamp = hrt_absolute_time();
if (!ignore_attitude_msg) { // only copy att sp if message contained new data if (!ignore_attitude_msg) { // only copy att sp if message contained new data
matrix::Quatf q(set_attitude_target.q); matrix::Quatf q(set_attitude_target.q);
@@ -1379,7 +1379,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
///XXX add support for ignoring individual axes ///XXX add support for ignoring individual axes
if (!(_offboard_control_mode.ignore_bodyrate)) { if (!(_offboard_control_mode.ignore_bodyrate)) {
vehicle_rates_setpoint_s rates_sp = {}; vehicle_rates_setpoint_s rates_sp = {};
rates_sp.timestamp = _mavlink_timesync.sync_stamp(set_attitude_target.time_boot_ms * 1e3f); rates_sp.timestamp = hrt_absolute_time();
if (!ignore_bodyrate_msg) { // only copy att rates sp if message contained new data if (!ignore_bodyrate_msg) { // only copy att rates sp if message contained new data
rates_sp.roll = set_attitude_target.body_roll_rate; rates_sp.roll = set_attitude_target.body_roll_rate;
@@ -1627,7 +1627,7 @@ MavlinkReceiver::handle_message_obstacle_distance(mavlink_message_t *msg)
mavlink_msg_obstacle_distance_decode(msg, &mavlink_obstacle_distance); mavlink_msg_obstacle_distance_decode(msg, &mavlink_obstacle_distance);
obstacle_distance_s obstacle_distance = {}; obstacle_distance_s obstacle_distance = {};
obstacle_distance.timestamp = _mavlink_timesync.sync_stamp(mavlink_obstacle_distance.time_usec); obstacle_distance.timestamp = hrt_absolute_time();
obstacle_distance.sensor_type = mavlink_obstacle_distance.sensor_type; obstacle_distance.sensor_type = mavlink_obstacle_distance.sensor_type;
memcpy(obstacle_distance.distances, mavlink_obstacle_distance.distances, sizeof(obstacle_distance.distances)); memcpy(obstacle_distance.distances, mavlink_obstacle_distance.distances, sizeof(obstacle_distance.distances));
obstacle_distance.increment = mavlink_obstacle_distance.increment; obstacle_distance.increment = mavlink_obstacle_distance.increment;
@@ -1651,7 +1651,7 @@ MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_mess
struct vehicle_trajectory_waypoint_s trajectory_waypoint = {}; struct vehicle_trajectory_waypoint_s trajectory_waypoint = {};
trajectory_waypoint.timestamp = _mavlink_timesync.sync_stamp(trajectory.time_usec); trajectory_waypoint.timestamp = hrt_absolute_time();
const int number_valid_points = trajectory.valid_points; const int number_valid_points = trajectory.valid_points;
for (int i = 0; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) { for (int i = 0; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) {
@@ -2130,7 +2130,7 @@ void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg)
mavlink_msg_follow_target_decode(msg, &follow_target_msg); mavlink_msg_follow_target_decode(msg, &follow_target_msg);
follow_target_topic.timestamp = _mavlink_timesync.sync_stamp(follow_target_msg.timestamp * 1e3f); follow_target_topic.timestamp = hrt_absolute_time();
follow_target_topic.lat = follow_target_msg.lat * 1e-7; follow_target_topic.lat = follow_target_msg.lat * 1e-7;
follow_target_topic.lon = follow_target_msg.lon * 1e-7; follow_target_topic.lon = follow_target_msg.lon * 1e-7;
@@ -2434,11 +2434,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
void MavlinkReceiver::handle_message_named_value_float(mavlink_message_t *msg) void MavlinkReceiver::handle_message_named_value_float(mavlink_message_t *msg)
{ {
mavlink_named_value_float_t debug_msg; mavlink_named_value_float_t debug_msg;
debug_key_value_s debug_topic = {}; debug_key_value_s debug_topic;
mavlink_msg_named_value_float_decode(msg, &debug_msg); mavlink_msg_named_value_float_decode(msg, &debug_msg);
debug_topic.timestamp = _mavlink_timesync.sync_stamp(debug_msg.time_boot_ms * 1e3f); debug_topic.timestamp = hrt_absolute_time();
memcpy(debug_topic.key, debug_msg.name, sizeof(debug_topic.key)); 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.key[sizeof(debug_topic.key) - 1] = '\0'; // enforce null termination
debug_topic.value = debug_msg.value; debug_topic.value = debug_msg.value;
@@ -2454,11 +2454,11 @@ void MavlinkReceiver::handle_message_named_value_float(mavlink_message_t *msg)
void MavlinkReceiver::handle_message_debug(mavlink_message_t *msg) void MavlinkReceiver::handle_message_debug(mavlink_message_t *msg)
{ {
mavlink_debug_t debug_msg; mavlink_debug_t debug_msg;
debug_value_s debug_topic = {}; debug_value_s debug_topic;
mavlink_msg_debug_decode(msg, &debug_msg); mavlink_msg_debug_decode(msg, &debug_msg);
debug_topic.timestamp = _mavlink_timesync.sync_stamp(debug_msg.time_boot_ms * 1e3f); debug_topic.timestamp = hrt_absolute_time();
debug_topic.ind = debug_msg.ind; debug_topic.ind = debug_msg.ind;
debug_topic.value = debug_msg.value; debug_topic.value = debug_msg.value;
@@ -2473,11 +2473,11 @@ void MavlinkReceiver::handle_message_debug(mavlink_message_t *msg)
void MavlinkReceiver::handle_message_debug_vect(mavlink_message_t *msg) void MavlinkReceiver::handle_message_debug_vect(mavlink_message_t *msg)
{ {
mavlink_debug_vect_t debug_msg; mavlink_debug_vect_t debug_msg;
debug_vect_s debug_topic = {}; debug_vect_s debug_topic;
mavlink_msg_debug_vect_decode(msg, &debug_msg); mavlink_msg_debug_vect_decode(msg, &debug_msg);
debug_topic.timestamp = _mavlink_timesync.sync_stamp(debug_msg.time_usec); debug_topic.timestamp = hrt_absolute_time();
memcpy(debug_topic.name, debug_msg.name, sizeof(debug_topic.name)); 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.name[sizeof(debug_topic.name) - 1] = '\0'; // enforce null termination
debug_topic.x = debug_msg.x; debug_topic.x = debug_msg.x;