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

mavlink : add advanced timesync algorithm

Browse Source
This commit is contained in:
Mohammed Kabir
2018-03-12 01:52:03 -04:00
committed by Beat Küng
parent 6b2daef5ec
commit 39bb65ffd7
8 changed files with 424 additions and 134 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+10 -113
View File
@@ -96,6 +96,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_parameters_manager(parent),
_mavlink_ftp(parent),
_mavlink_log_handler(parent),
_mavlink_timesync(parent),
_status{},
_hil_local_pos{},
_hil_land_detector{},
@@ -128,7 +129,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_manual_pub(nullptr),
_obstacle_distance_pub(nullptr),
_land_detector_pub(nullptr),
_time_offset_pub(nullptr),
_follow_target_pub(nullptr),
_landing_target_pose_pub(nullptr),
_transponder_report_pub(nullptr),
@@ -147,8 +147,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_hil_local_alt0(0.0f),
_hil_local_proj_ref{},
_offboard_control_mode{},
_time_offset_avg_alpha(0.8),
_time_offset(0),
_orb_class_instance(-1),
_mom_switch_pos{},
_mom_switch_state(0),
@@ -285,14 +283,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_heartbeat(msg);
break;
case MAVLINK_MSG_ID_SYSTEM_TIME:
handle_message_system_time(msg);
break;
case MAVLINK_MSG_ID_TIMESYNC:
handle_message_timesync(msg);
break;
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
handle_message_distance_sensor(msg);
break;
@@ -812,7 +802,7 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
// Use the component ID to identify the mocap system
att_pos_mocap.id = msg->compid;
att_pos_mocap.timestamp = sync_stamp(mocap.time_usec);
att_pos_mocap.timestamp = _mavlink_timesync.sync_stamp(mocap.time_usec);
att_pos_mocap.timestamp_received = hrt_absolute_time();
att_pos_mocap.q[0] = mocap.q[0];
@@ -1106,7 +1096,7 @@ MavlinkReceiver::handle_message_attitude_quaternion_cov(mavlink_message_t *msg)
struct vehicle_attitude_s vision_attitude = {};
vision_attitude.timestamp = sync_stamp(att.time_usec);
vision_attitude.timestamp = _mavlink_timesync.sync_stamp(att.time_usec);
vision_attitude.q[0] = att.q[0];
vision_attitude.q[1] = att.q[1];
@@ -1132,7 +1122,7 @@ MavlinkReceiver::handle_message_local_position_ned_cov(mavlink_message_t *msg)
struct vehicle_local_position_s vision_position = {};
vision_position.timestamp = sync_stamp(pos.time_usec);
vision_position.timestamp = _mavlink_timesync.sync_stamp(pos.time_usec);
vision_position.xy_valid = true;
vision_position.z_valid = true;
@@ -1192,7 +1182,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
struct vehicle_local_position_s vision_position = {};
vision_position.timestamp = sync_stamp(pos.usec);
vision_position.timestamp = _mavlink_timesync.sync_stamp(pos.usec);
vision_position.x = pos.x;
vision_position.y = pos.y;
vision_position.z = pos.z;
@@ -1204,7 +1194,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
struct vehicle_attitude_s vision_attitude = {};
vision_attitude.timestamp = sync_stamp(pos.usec);
vision_attitude.timestamp = _mavlink_timesync.sync_stamp(pos.usec);
matrix::Quatf q(matrix::Eulerf(pos.roll, pos.pitch, pos.yaw));
q.copyTo(vision_attitude.q);
@@ -1793,80 +1783,6 @@ MavlinkReceiver::get_message_interval(int msgId)
mavlink_msg_message_interval_send(_mavlink->get_channel(), msgId, interval);
}
void
MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg)
{
mavlink_system_time_t time;
mavlink_msg_system_time_decode(msg, &time);
timespec tv = {};
px4_clock_gettime(CLOCK_REALTIME, &tv);
// date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
bool onb_unix_valid = (unsigned long long)tv.tv_sec > PX4_EPOCH_SECS;
bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL;
if (!onb_unix_valid && ofb_unix_valid) {
tv.tv_sec = time.time_unix_usec / 1000000ULL;
tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL;
if (px4_clock_settime(CLOCK_REALTIME, &tv)) {
PX4_ERR("failed setting clock");
}
}
}
void
MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
{
mavlink_timesync_t tsync = {};
mavlink_msg_timesync_decode(msg, &tsync);
struct time_offset_s tsync_offset = {};
uint64_t now_ns = hrt_absolute_time() * 1000LL ;
if (tsync.tc1 == 0) {
mavlink_timesync_t rsync; // return timestamped sync message
rsync.tc1 = now_ns;
rsync.ts1 = tsync.ts1;
mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &rsync);
return;
} else if (tsync.tc1 > 0) {
int64_t offset_ns = (int64_t)(tsync.ts1 + now_ns - tsync.tc1 * 2) / 2 ;
int64_t dt = _time_offset - offset_ns;
if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew
_time_offset = offset_ns;
// Provide a warning only if not syncing initially
if (_time_offset != 0) {
PX4_ERR("[timesync] Hard setting offset.");
}
} else {
smooth_time_offset(offset_ns);
}
}
tsync_offset.offset_ns = _time_offset ;
if (_time_offset_pub == nullptr) {
_time_offset_pub = orb_advertise(ORB_ID(time_offset), &tsync_offset);
} else {
orb_publish(ORB_ID(time_offset), _time_offset_pub, &tsync_offset);
}
}
void
MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
{
@@ -2074,7 +1990,7 @@ void MavlinkReceiver::handle_message_landing_target(mavlink_message_t *msg)
if (landing_target.position_valid && landing_target.frame == MAV_FRAME_LOCAL_NED) {
landing_target_pose_s landing_target_pose = {};
landing_target_pose.timestamp = sync_stamp(landing_target.time_usec);
landing_target_pose.timestamp = _mavlink_timesync.sync_stamp(landing_target.time_usec);
landing_target_pose.abs_pos_valid = true;
landing_target_pose.x_abs = landing_target.x;
landing_target_pose.y_abs = landing_target.y;
@@ -2547,6 +2463,9 @@ MavlinkReceiver::receive_thread(void *arg)
/* handle packet with log component */
_mavlink_log_handler.handle_message(&msg);
/* handle packet with timesync component */
_mavlink_timesync.handle_message(&msg);
/* handle packet with parent object */
_mavlink->handle_message(&msg);
}
@@ -2587,28 +2506,6 @@ void MavlinkReceiver::print_status()
}
uint64_t MavlinkReceiver::sync_stamp(uint64_t usec)
{
if (_time_offset != 0) {
return usec + (_time_offset / 1000) ;
} else {
return hrt_absolute_time();
}
}
void MavlinkReceiver::smooth_time_offset(int64_t offset_ns)
{
/* The closer alpha is to 1.0, the faster the moving
* average updates in response to new offset samples.
*/
_time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset;
}
void *MavlinkReceiver::start_helper(void *context)
{