diff --git a/boards/cubepilot/io-v2/nuttx-config/nsh/defconfig b/boards/cubepilot/io-v2/nuttx-config/nsh/defconfig index 86125cabee..7250d4d0f5 100644 --- a/boards/cubepilot/io-v2/nuttx-config/nsh/defconfig +++ b/boards/cubepilot/io-v2/nuttx-config/nsh/defconfig @@ -18,6 +18,7 @@ CONFIG_ARMV7M_USEBASEPRI=y CONFIG_ARM_MPU_EARLY_RESET=y CONFIG_BOARD_LOOPSPERMSEC=2000 CONFIG_C99_BOOL8=y +CONFIG_CLOCK_MONOTONIC=y CONFIG_DEBUG_FULLOPT=y CONFIG_DEBUG_SYMBOLS=y CONFIG_DEFAULT_SMALL=y diff --git a/boards/px4/io-v2/nuttx-config/nsh/defconfig b/boards/px4/io-v2/nuttx-config/nsh/defconfig index 1444f21613..d1b8522db0 100644 --- a/boards/px4/io-v2/nuttx-config/nsh/defconfig +++ b/boards/px4/io-v2/nuttx-config/nsh/defconfig @@ -18,6 +18,7 @@ CONFIG_ARMV7M_USEBASEPRI=y CONFIG_ARM_MPU_EARLY_RESET=y CONFIG_BOARD_LOOPSPERMSEC=2000 CONFIG_C99_BOOL8=y +CONFIG_CLOCK_MONOTONIC=y CONFIG_DEBUG_FULLOPT=y CONFIG_DEBUG_SYMBOLS=y CONFIG_DEFAULT_SMALL=y diff --git a/msg/sensor_gps.msg b/msg/sensor_gps.msg index a02300df03..b0038626b1 100644 --- a/msg/sensor_gps.msg +++ b/msg/sensor_gps.msg @@ -1,6 +1,7 @@ # GPS position in WGS84 coordinates. -# the field 'timestamp' is for the position & velocity (microseconds) +# the field 'timestamp_sample' is for the position & velocity (microseconds) uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample uint32 device_id # unique device ID for the sensor that does not change between power cycles @@ -31,7 +32,6 @@ float32 vel_d_m_s # GPS Down velocity, (metres/sec) float32 cog_rad # Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) bool vel_ned_valid # True if NED velocity is valid -int32 timestamp_time_relative # timestamp + timestamp_time_relative = Time of the UTC timestamp since system start, (microseconds) uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 uint8 satellites_used # Number of satellites used diff --git a/msg/templates/urtps/microRTPS_timesync.h.em b/msg/templates/urtps/microRTPS_timesync.h.em index ffe4541f18..6e7e5d5b7a 100644 --- a/msg/templates/urtps/microRTPS_timesync.h.em +++ b/msg/templates/urtps/microRTPS_timesync.h.em @@ -288,16 +288,16 @@ private: /** Timesync Status msg Setters **/ @[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ - inline void setMsgSourceProtocol(timesync_status_msg_t *msg, const uint8_t &source_protocol) { msg->source_protocol_() = source_protocol; } + inline void setMsgSourceProtocol(timesync_status_msg_t *msg, const uint8_t &source_protocol) { msg->source_() = source_protocol; } inline void setMsgRemoteTimeStamp(timesync_status_msg_t *msg, const uint64_t &remote_timestamp) { msg->remote_timestamp_() = remote_timestamp; } inline void setMsgObservedOffset(timesync_status_msg_t *msg, const int64_t &observed_offset) { msg->observed_offset_() = observed_offset; } inline void setMsgEstimatedOffset(timesync_status_msg_t *msg, const int64_t &estimated_offset) { msg->estimated_offset_() = estimated_offset; } - inline void setMsgRoundTripTime(timesync_status_msg_t *msg, const uint32_t &round_trip_time) { msg->round_trip_time_() = round_trip_time; } + //inline void setMsgRoundTripTime(timesync_status_msg_t *msg, const uint32_t &round_trip_time) { msg->round_trip_time_() = round_trip_time; } @[elif ros2_distro]@ - inline void setMsgSourceProtocol(timesync_status_msg_t *msg, const uint8_t &source_protocol) { msg->source_protocol() = source_protocol; } + inline void setMsgSourceProtocol(timesync_status_msg_t *msg, const uint8_t &source_protocol) { msg->source() = source_protocol; } inline void setMsgRemoteTimeStamp(timesync_status_msg_t *msg, const uint64_t &remote_timestamp) { msg->remote_timestamp() = remote_timestamp; } inline void setMsgObservedOffset(timesync_status_msg_t *msg, const int64_t &observed_offset) { msg->observed_offset() = observed_offset; } inline void setMsgEstimatedOffset(timesync_status_msg_t *msg, const int64_t &estimated_offset) { msg->estimated_offset() = estimated_offset; } - inline void setMsgRoundTripTime(timesync_status_msg_t *msg, const uint32_t &round_trip_time) { msg->round_trip_time() = round_trip_time; } + //inline void setMsgRoundTripTime(timesync_status_msg_t *msg, const uint32_t &round_trip_time) { msg->round_trip_time() = round_trip_time; } @[end if]@ }; diff --git a/msg/timesync_status.msg b/msg/timesync_status.msg index 9d557352dc..44333cdb3f 100644 --- a/msg/timesync_status.msg +++ b/msg/timesync_status.msg @@ -1,10 +1,17 @@ uint64 timestamp # time since system start (microseconds) -uint8 SOURCE_PROTOCOL_MAVLINK = 0 -uint8 SOURCE_PROTOCOL_RTPS = 1 -uint8 source_protocol # timesync source. Source can be MAVLink or the microRTPS bridge +uint8 SOURCE_UNKNOWN = 0 +uint8 SOURCE_PROTOCOL_MAVLINK = 1 +uint8 SOURCE_PROTOCOL_RTPS = 2 +uint8 SOURCE_GPS1 = 3 +uint8 SOURCE_GPS2 = 4 +uint8 source # timesync source -uint64 remote_timestamp # remote system timestamp (microseconds) int64 observed_offset # raw time offset directly observed from this timesync packet (microseconds) int64 estimated_offset # smoothed time offset between companion system and PX4 (microseconds) -uint32 round_trip_time # round trip time of this timesync packet (microseconds) + +uint64 deviation + +int32 sequence + +bool converged diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg index e5c87169b5..e06a6b8c80 100644 --- a/msg/vehicle_gps_position.msg +++ b/msg/vehicle_gps_position.msg @@ -1,6 +1,7 @@ # GPS position in WGS84 coordinates. # the field 'timestamp' is for the position & velocity (microseconds) uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample int32 lat # Latitude in 1E-7 degrees int32 lon # Longitude in 1E-7 degrees @@ -17,6 +18,8 @@ float32 epv # GPS vertical position accuracy (metres) float32 hdop # Horizontal dilution of precision float32 vdop # Vertical dilution of precision +float32 pdop + int32 noise_per_ms # GPS noise per millisecond int32 jamming_indicator # indicates jamming is occurring uint8 jamming_state # indicates whether jamming has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Warning, 3: Critical @@ -28,7 +31,6 @@ float32 vel_d_m_s # GPS Down velocity, (metres/sec) float32 cog_rad # Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) bool vel_ned_valid # True if NED velocity is valid -int32 timestamp_time_relative # timestamp + timestamp_time_relative = Time of the UTC timestamp since system start, (microseconds) uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 uint8 satellites_used # Number of satellites used diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 94614bf5d8..9ed70d950e 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -72,9 +72,9 @@ static constexpr wq_config_t INS1{"wq:INS1", 6000, -15}; static constexpr wq_config_t INS2{"wq:INS2", 6000, -16}; static constexpr wq_config_t INS3{"wq:INS3", 6000, -17}; -static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18}; +static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -19}; -static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19}; +static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -20}; static constexpr wq_config_t ttyS0{"wq:ttyS0", 1632, -21}; static constexpr wq_config_t ttyS1{"wq:ttyS1", 1632, -22}; diff --git a/platforms/common/include/px4_platform_common/tasks.h b/platforms/common/include/px4_platform_common/tasks.h index a7e1e370a4..28f7e74205 100644 --- a/platforms/common/include/px4_platform_common/tasks.h +++ b/platforms/common/include/px4_platform_common/tasks.h @@ -101,11 +101,11 @@ typedef struct { #endif // PX4 work queue starting high priority -#define PX4_WQ_HP_BASE (SCHED_PRIORITY_MAX - 15) +#define PX4_WQ_HP_BASE (SCHED_PRIORITY_MAX - 19) // Fast drivers - they need to run as quickly as possible to minimize control // latency. -#define SCHED_PRIORITY_FAST_DRIVER (SCHED_PRIORITY_MAX - 0) +#define SCHED_PRIORITY_FAST_DRIVER (PX4_WQ_HP_BASE + 1) // Actuator outputs should run as soon as the rate controller publishes // the actuator controls topic @@ -136,7 +136,7 @@ typedef struct { // Slow drivers should run at a rate where they do not impact the overall // system execution -#define SCHED_PRIORITY_SLOW_DRIVER (PX4_WQ_HP_BASE - 35) +#define SCHED_PRIORITY_SLOW_DRIVER (PX4_WQ_HP_BASE - 18) // The navigation system needs to execute regularly but has no realtime needs #define SCHED_PRIORITY_NAVIGATION (SCHED_PRIORITY_DEFAULT + 5) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c b/platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c index 8e4785362c..11da52037c 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c @@ -578,12 +578,27 @@ ts_to_abstime(const struct timespec *ts) /** * Convert absolute time to a timespec. */ -void -abstime_to_ts(struct timespec *ts, hrt_abstime abstime) +struct timespec abstime_to_ts(hrt_abstime abstime) { - ts->tv_sec = abstime / 1000000; - abstime -= ts->tv_sec * 1000000; - ts->tv_nsec = abstime * 1000; + // get offset between hrt and system CLOCK_MONOTONIC + struct timespec system_time; + irqstate_t flags = enter_critical_section(); + const hrt_abstime now_us = hrt_absolute_time(); + clock_gettime(CLOCK_REALTIME, &system_time); + leave_critical_section(flags); + + // adjust input abstime with offset to CLOCK_MONOTONIC + int64_t system_time_us = (system_time.tv_sec * 1000000) + (system_time.tv_nsec / 1000); + int64_t offset_us = system_time_us - now_us; + + uint64_t ts_abstime = abstime + offset_us; + + struct timespec ts; + ts.tv_sec = ts_abstime / 1000000; + ts_abstime -= ts.tv_sec * 1000000; + ts.tv_nsec = ts_abstime * 1000; // microseconds -> nanoseconds + + return ts; } /** diff --git a/platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c b/platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c index dbb8b062a4..ed33a45d53 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -490,14 +490,12 @@ static int hrt_tim_isr(int irq, void *context, void *arg) { /* grab the timer for latency tracking purposes */ - latency_actual = rCNT; /* copy interrupt status */ uint32_t status = rSTATUS; /* ack the interrupts we just read */ - rSTATUS = status; #ifdef HRT_PPM_CHANNEL @@ -579,23 +577,46 @@ hrt_absolute_time(void) hrt_abstime ts_to_abstime(const struct timespec *ts) { - hrt_abstime result; + // get offset between hrt and system CLOCK_MONOTONIC + struct timespec system_time; + irqstate_t flags = enter_critical_section(); + const hrt_abstime now_us = hrt_absolute_time(); + clock_gettime(CLOCK_REALTIME, &system_time); + leave_critical_section(flags); - result = (hrt_abstime)(ts->tv_sec) * 1000000; - result += ts->tv_nsec / 1000; + // convert input ts to hrt with offset to system CLOCK_MONOTONIC + int64_t ts_us = (ts->tv_sec * 1000000) + (ts->tv_nsec / 1000); + int64_t system_time_us = (system_time.tv_sec * 1000000) + (system_time.tv_nsec / 1000); - return result; + int64_t offset_us = system_time_us - now_us; + + return ts_us - offset_us; } /** * Convert absolute time to a timespec. */ -void -abstime_to_ts(struct timespec *ts, hrt_abstime abstime) +struct timespec abstime_to_ts(hrt_abstime abstime) { - ts->tv_sec = abstime / 1000000; - abstime -= ts->tv_sec * 1000000; - ts->tv_nsec = abstime * 1000; + // get offset between hrt and system CLOCK_MONOTONIC + struct timespec system_time; + irqstate_t flags = enter_critical_section(); + const hrt_abstime now_us = hrt_absolute_time(); + clock_gettime(CLOCK_REALTIME, &system_time); + leave_critical_section(flags); + + // adjust input abstime with offset to CLOCK_MONOTONIC + int64_t system_time_us = (system_time.tv_sec * 1000000) + (system_time.tv_nsec / 1000); + int64_t offset_us = system_time_us - now_us; + + uint64_t ts_abstime = abstime + offset_us; + + struct timespec ts; + ts.tv_sec = ts_abstime / 1000000; + ts_abstime -= ts.tv_sec * 1000000; + ts.tv_nsec = ts_abstime * 1000; // microseconds -> nanoseconds + + return ts; } /** diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/hrt/hrt.c b/platforms/nuttx/src/px4/nxp/s32k1xx/hrt/hrt.c index 9251398917..30717a26cd 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/hrt/hrt.c +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/hrt/hrt.c @@ -626,12 +626,27 @@ ts_to_abstime(const struct timespec *ts) /** * Convert absolute time to a timespec. */ -void -abstime_to_ts(struct timespec *ts, hrt_abstime abstime) +struct timespec abstime_to_ts(hrt_abstime abstime) { - ts->tv_sec = abstime / 1000000; - abstime -= ts->tv_sec * 1000000; - ts->tv_nsec = abstime * 1000; + // get offset between hrt and system CLOCK_MONOTONIC + struct timespec system_time; + irqstate_t flags = enter_critical_section(); + const hrt_abstime now_us = hrt_absolute_time(); + clock_gettime(CLOCK_REALTIME, &system_time); + leave_critical_section(flags); + + // adjust input abstime with offset to CLOCK_MONOTONIC + int64_t system_time_us = (system_time.tv_sec * 1000000) + (system_time.tv_nsec / 1000); + int64_t offset_us = system_time_us - now_us; + + uint64_t ts_abstime = abstime + offset_us; + + struct timespec ts; + ts.tv_sec = ts_abstime / 1000000; + ts_abstime -= ts.tv_sec * 1000000; + ts.tv_nsec = ts_abstime * 1000; // microseconds -> nanoseconds + + return ts; } /** diff --git a/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c b/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c index dc35ca82c0..9eee964acd 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -605,13 +605,11 @@ error: static int hrt_tim_isr(int irq, void *context, void *arg) { - uint32_t status; - /* grab the timer for latency tracking purposes */ latency_actual = rCNT; /* copy interrupt status */ - status = rSR; + uint32_t status = rSR; /* ack the interrupts we just read */ rSR = ~status; @@ -700,23 +698,47 @@ hrt_absolute_time(void) hrt_abstime ts_to_abstime(const struct timespec *ts) { - hrt_abstime result; + // get offset between hrt and system CLOCK_MONOTONIC + struct timespec system_time; - result = (hrt_abstime)(ts->tv_sec) * 1000000; - result += ts->tv_nsec / 1000; + irqstate_t flags = enter_critical_section(); + const hrt_abstime now_us = hrt_absolute_time(); + clock_gettime(CLOCK_REALTIME, &system_time); + leave_critical_section(flags); - return result; + int64_t system_time_us = (system_time.tv_sec * 1000000) + (system_time.tv_nsec / 1000); + int64_t offset_us = system_time_us - now_us; + + // convert input ts to hrt with offset to system CLOCK_REALTIME + uint64_t ts_us = (ts->tv_sec * 1000000) + (ts->tv_nsec / 1000); + return ts_us - offset_us; } /** * Convert absolute time to a timespec. */ -void -abstime_to_ts(struct timespec *ts, hrt_abstime abstime) +struct timespec abstime_to_ts(hrt_abstime abstime) { - ts->tv_sec = abstime / 1000000; - abstime -= ts->tv_sec * 1000000; - ts->tv_nsec = abstime * 1000; + // get offset between hrt and system CLOCK_MONOTONIC + struct timespec system_time; + + irqstate_t flags = enter_critical_section(); + const hrt_abstime now_us = hrt_absolute_time(); + clock_gettime(CLOCK_REALTIME, &system_time); + leave_critical_section(flags); + + // adjust input abstime with offset to CLOCK_MONOTONIC + int64_t system_time_us = (system_time.tv_sec * 1000000) + (system_time.tv_nsec / 1000); + int64_t offset_us = system_time_us - now_us; + + uint64_t ts_abstime = abstime + offset_us; + + struct timespec ts; + ts.tv_sec = ts_abstime / 1000000; + ts_abstime -= ts.tv_sec * 1000000; + ts.tv_nsec = ts_abstime * 1000; // microseconds -> nanoseconds + + return ts; } /** @@ -731,7 +753,7 @@ hrt_store_absolute_time(volatile hrt_abstime *t) } /** - * Initialise the high-resolution timing module. + * Initialize the high-resolution timing module. */ void hrt_init(void) @@ -786,11 +808,11 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte irqstate_t flags = px4_enter_critical_section(); /* if the entry is currently queued, remove it */ - /* note that we are using a potentially uninitialised + /* note that we are using a potentially uninitialized entry->link here, but it is safe as sq_rem() doesn't dereference the passed node unless it is found in the list. So we potentially waste a bit of time searching the - queue for the uninitialised entry->link but we don't do + queue for the uninitialized entry->link but we don't do anything actually unsafe. */ if (entry->deadline != 0) { diff --git a/platforms/posix/src/px4/common/drv_hrt.cpp b/platforms/posix/src/px4/common/drv_hrt.cpp index d9638d03ee..2735a83b5d 100644 --- a/platforms/posix/src/px4/common/drv_hrt.cpp +++ b/platforms/posix/src/px4/common/drv_hrt.cpp @@ -484,11 +484,13 @@ hrt_call_invoke() hrt_unlock(); } -void abstime_to_ts(struct timespec *ts, hrt_abstime abstime) +struct timespec abstime_to_ts(hrt_abstime abstime) { - ts->tv_sec = abstime / 1000000; - abstime -= ts->tv_sec * 1000000; - ts->tv_nsec = abstime * 1000; + struct timespec ts {}; + ts.tv_sec = abstime / 1000000; + abstime -= ts.tv_sec * 1000000; + ts.tv_nsec = abstime * 1000; + return ts; } int px4_clock_gettime(clockid_t clk_id, struct timespec *tp) @@ -496,7 +498,7 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp) if (clk_id == CLOCK_MONOTONIC) { #if defined(ENABLE_LOCKSTEP_SCHEDULER) const uint64_t abstime = lockstep_scheduler->get_absolute_time(); - abstime_to_ts(tp, abstime - px4_timestart_monotonic); + *tp = abstime_to_ts(abstime - px4_timestart_monotonic); return 0; #else // defined(ENABLE_LOCKSTEP_SCHEDULER) #if defined(__PX4_DARWIN) diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 1b7d61f733..4e4d51d47e 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -102,7 +102,7 @@ __EXPORT extern hrt_abstime ts_to_abstime(const struct timespec *ts); /** * Convert absolute time to a timespec. */ -__EXPORT extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); +__EXPORT extern struct timespec abstime_to_ts(hrt_abstime abstime); /** * Compute the delta between a timestamp taken in the past diff --git a/src/drivers/gps/CMakeLists.txt b/src/drivers/gps/CMakeLists.txt index 96e517f355..c0664fda18 100644 --- a/src/drivers/gps/CMakeLists.txt +++ b/src/drivers/gps/CMakeLists.txt @@ -38,6 +38,7 @@ px4_add_module( MAIN gps COMPILE_FLAGS -Wno-stringop-overflow # due to https://gcc.gnu.org/bugzilla/show_bug.cgi?id=91707 + -Wno-error; SRCS gps.cpp devices/src/gps_helper.cpp diff --git a/src/drivers/gps/definitions.h b/src/drivers/gps/definitions.h index fee9bfb6f2..0479bfe188 100644 --- a/src/drivers/gps/definitions.h +++ b/src/drivers/gps/definitions.h @@ -56,9 +56,3 @@ */ #define gps_absolute_time hrt_absolute_time typedef hrt_abstime gps_abstime; - - -// TODO: this functionality is not available on the Snapdragon yet -#ifdef __PX4_QURT -#define NO_MKTIME -#endif diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index d6940d9c8c..2f6ce7db21 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit d6940d9c8ccb8ab3273c677097a29d46903021ff +Subproject commit 2f6ce7db217275aaa614f439325bf7136a0423d7 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index c2e210a7b0..f603ce4330 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -365,8 +365,6 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) { GPS *gps = (GPS *)user; - timespec rtc_system_time; - switch (type) { case GPSCallbackType::readDeviceData: { int timeout; @@ -395,22 +393,6 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) case GPSCallbackType::surveyInStatus: /* not used */ - break; - - case GPSCallbackType::setClock: - - px4_clock_gettime(CLOCK_REALTIME, &rtc_system_time); - timespec rtc_gps_time = *(timespec *)data1; - int drift_time = abs(rtc_system_time.tv_sec - rtc_gps_time.tv_sec); - - if (drift_time >= SET_CLOCK_DRIFT_TIME_S) { - // as of 2021 setting the time on Nuttx temporarily pauses interrupts - // so only set the time if it is very wrong. - // TODO: clock slewing of the RTC for small time differences - px4_clock_settime(CLOCK_REALTIME, &rtc_gps_time); - } - - break; } @@ -1124,7 +1106,7 @@ GPS::publish() { if (_instance == Instance::Main || _is_gps_main_advertised.load()) { _report_gps_pos.device_id = get_device_id(); - + _report_gps_pos.timestamp = hrt_absolute_time(); _report_gps_pos_pub.publish(_report_gps_pos); // Heading/yaw data can be updated at a lower rate than the other navigation data. // The uORB message definition requires this data to be set to a NAN if no new valid data is available. @@ -1138,6 +1120,7 @@ GPS::publishSatelliteInfo() { if (_instance == Instance::Main) { if (_p_report_sat_info != nullptr) { + _p_report_sat_info->timestamp = hrt_absolute_time(); _report_sat_info_pub.publish(*_p_report_sat_info); } @@ -1282,7 +1265,7 @@ int GPS::task_spawn(int argc, char *argv[], Instance instance) } int task_id = px4_task_spawn_cmd("gps", SCHED_DEFAULT, - SCHED_PRIORITY_SLOW_DRIVER, TASK_STACK_SIZE, + SCHED_PRIORITY_FAST_DRIVER, TASK_STACK_SIZE, entry_point, (char *const *)argv); if (task_id < 0) { diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index de9c62005d..1bd0c8de39 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -314,7 +314,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure * to use an independent time source (based on hardware TIM5) instead of HRT. * The proper solution is to be developed. */ - report.timestamp = hrt_absolute_time(); + report.timestamp_sample = hrt_absolute_time(); report.lat = msg.latitude_deg_1e8 / 10; report.lon = msg.longitude_deg_1e8 / 10; @@ -372,8 +372,6 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s); report.vel_ned_valid = true; - report.timestamp_time_relative = 0; - const uint64_t gnss_ts_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); switch (msg.gnss_time_standard) { @@ -430,7 +428,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure report.heading = heading; report.heading_offset = heading_offset; report.heading_accuracy = heading_accuracy; - + report.timestamp = hrt_absolute_time(); publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/examples/fake_gps/FakeGps.cpp b/src/examples/fake_gps/FakeGps.cpp index 1605a2f9b9..b0cb73f917 100644 --- a/src/examples/fake_gps/FakeGps.cpp +++ b/src/examples/fake_gps/FakeGps.cpp @@ -59,6 +59,7 @@ void FakeGps::Run() } sensor_gps_s sensor_gps{}; + sensor_gps.timestamp_sample = hrt_absolute_time(); sensor_gps.time_utc_usec = hrt_absolute_time() + 1613692609599954; sensor_gps.lat = _latitude; sensor_gps.lon = _longitude; @@ -77,7 +78,6 @@ void FakeGps::Run() sensor_gps.vel_e_m_s = 0.0200f; sensor_gps.vel_d_m_s = -0.0570f; sensor_gps.cog_rad = 0.3988f; - sensor_gps.timestamp_time_relative = 0; sensor_gps.heading = NAN; sensor_gps.heading_offset = 0.0000; sensor_gps.fix_type = 4; diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index dd87dc2dd7..29f6254dff 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -66,6 +66,7 @@ add_subdirectory(systemlib) add_subdirectory(system_identification) add_subdirectory(tecs) add_subdirectory(terrain_estimation) +add_subdirectory(timesync) add_subdirectory(tunes) add_subdirectory(version) add_subdirectory(weather_vane) diff --git a/src/lib/timesync/CMakeLists.txt b/src/lib/timesync/CMakeLists.txt new file mode 100644 index 0000000000..0046d55dc5 --- /dev/null +++ b/src/lib/timesync/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(timesync + Timesync.cpp + Timesync.hpp +) diff --git a/src/lib/timesync/Timesync.cpp b/src/lib/timesync/Timesync.cpp new file mode 100644 index 0000000000..4d62cd4a65 --- /dev/null +++ b/src/lib/timesync/Timesync.cpp @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "Timesync.hpp" + +#include + +void Timesync::update(int64_t offset_us) +{ + const bool converged = sync_converged(); + + // Calculate the difference of this sample from the current estimate + uint64_t deviation = llabs((int64_t)_time_offset - offset_us); + + if (sync_converged() && (deviation > MAX_DEVIATION_SAMPLE)) { + // Increment the counter if we have a good estimate and are getting samples far from the estimate + _high_deviation_count++; + + // We reset the filter if we received 5 consecutive samples which violate our present estimate. + // This is most likely due to a time jump on the offboard system. + if (_high_deviation_count > MAX_CONSECUTIVE_HIGH_DEVIATION) { + PX4_ERR("Time jump detected. Resetting time synchroniser."); + // Reset the filter + reset_filter(); + } + + } else { + // Filter gain scheduling + if (!sync_converged()) { + // Interpolate with a sigmoid function + double progress = (double)_sequence / (double)CONVERGENCE_WINDOW; + double p = 1. - exp(0.5 * (1. - 1. / (1. - progress))); + _filter_alpha = p * ALPHA_GAIN_FINAL + (1. - p) * ALPHA_GAIN_INITIAL; + _filter_beta = p * BETA_GAIN_FINAL + (1. - p) * BETA_GAIN_INITIAL; + + } else { + _filter_alpha = ALPHA_GAIN_FINAL; + _filter_beta = BETA_GAIN_FINAL; + } + + // Perform filter update + add_sample(offset_us); + + // Increment sequence counter after filter update + _sequence++; + + // Reset high deviation count after filter update + _high_deviation_count = 0; + } + + // Publish status message + timesync_status_s timesync_status{}; + timesync_status.observed_offset = offset_us; + timesync_status.estimated_offset = (int64_t)round(_time_offset); + timesync_status.deviation = deviation; + timesync_status.sequence = _sequence; + timesync_status.source = _source; + timesync_status.converged = sync_converged(); + timesync_status.timestamp = hrt_absolute_time(); + _timesync_status_pub.publish(timesync_status); + + if (!converged && sync_converged()) { + PX4_DEBUG("new time offset: %.1f", _time_offset); + } +} + +void Timesync::add_sample(int64_t offset_us) +{ + /* Online exponential smoothing filter. The derivative of the estimate is also + * estimated in order to produce an estimate without steady state lag: + * https://en.wikipedia.org/wiki/Exponential_smoothing#Double_exponential_smoothing + */ + double time_offset_prev = _time_offset; + + if (_sequence == 0) { // First offset sample + _time_offset = offset_us; + + } else { + // Update the clock offset estimate + _time_offset = _filter_alpha * offset_us + (1. - _filter_alpha) * (_time_offset + _time_skew); + + // Update the clock skew estimate + _time_skew = _filter_beta * (_time_offset - time_offset_prev) + (1. - _filter_beta) * _time_skew; + } +} + +void Timesync::reset_filter() +{ + PX4_DEBUG("reset filter"); + + // Do a full reset of all statistics and parameters + _sequence = 0; + _time_offset = 0.0; + _time_skew = 0.0; + _filter_alpha = ALPHA_GAIN_INITIAL; + _filter_beta = BETA_GAIN_INITIAL; + _high_deviation_count = 0; +} diff --git a/src/modules/mavlink/mavlink_timesync.h b/src/lib/timesync/Timesync.hpp similarity index 80% rename from src/modules/mavlink/mavlink_timesync.h rename to src/lib/timesync/Timesync.hpp index 3204696f42..bf66db4eee 100644 --- a/src/modules/mavlink/mavlink_timesync.h +++ b/src/lib/timesync/Timesync.hpp @@ -40,8 +40,6 @@ #pragma once -#include "mavlink_bridge_header.h" - #include #include @@ -75,64 +73,63 @@ static constexpr double BETA_GAIN_FINAL = 0.003; // exhanged timesync packets is less than CONVERGENCE_WINDOW. A lower value will // allow the timesync to converge faster, but with potentially less accurate initial // offset and skew estimates. -static constexpr uint32_t CONVERGENCE_WINDOW = 500; +static constexpr uint32_t CONVERGENCE_WINDOW = 200; // Outlier rejection and filter reset // -// Samples with round-trip time higher than MAX_RTT_SAMPLE are not used to update the filter. -// More than MAX_CONSECUTIVE_HIGH_RTT number of such events in a row will throw a warning -// but not reset the filter. // Samples whose calculated clock offset is more than MAX_DEVIATION_SAMPLE off from the current // estimate are not used to update the filter. More than MAX_CONSECUTIVE_HIGH_DEVIATION number // of such events in a row will reset the filter. This usually happens only due to a time jump // on the remote system. // TODO : automatically determine these using ping statistics? -static constexpr uint64_t MAX_RTT_SAMPLE = 10_ms; -static constexpr uint64_t MAX_DEVIATION_SAMPLE = 100_ms; -static constexpr uint32_t MAX_CONSECUTIVE_HIGH_RTT = 5; -static constexpr uint32_t MAX_CONSECUTIVE_HIGH_DEVIATION = 5; +static constexpr uint64_t MAX_DEVIATION_SAMPLE = 50_ms; +static constexpr uint32_t MAX_CONSECUTIVE_HIGH_DEVIATION = 10; -class Mavlink; - -class MavlinkTimesync +class Timesync { public: - explicit MavlinkTimesync(Mavlink *mavlink); - ~MavlinkTimesync() = default; + Timesync() = default; + ~Timesync() = default; - void handle_message(const mavlink_message_t *msg); + void update(int64_t offset_us); + + int64_t time_offset() const { return (int64_t)round(_time_offset); } /** * Convert remote timestamp to local hrt time (usec) * Use synchronised time if available, monotonic boot time otherwise */ - uint64_t sync_stamp(uint64_t usec); + uint64_t sync_stamp(uint64_t usec) const + { + // Only return synchronised stamp if we have converged to a good value + if (sync_converged()) { + return usec + time_offset(); -private: - - /* do not allow top copying this class */ - MavlinkTimesync(MavlinkTimesync &); - MavlinkTimesync &operator = (const MavlinkTimesync &); - -protected: - - /** - * Online exponential filter to smooth time offset - */ - void add_sample(int64_t offset_us); + } else { + return hrt_absolute_time(); + } + } /** * Return true if the timesync algorithm converged to a good estimate, * return false otherwise */ - bool sync_converged(); + bool sync_converged() const { return _sequence >= CONVERGENCE_WINDOW; } + + void set_source(uint8_t source) { _source = source; } /** * Reset the exponential filter and its states */ void reset_filter(); - uORB::PublicationMulti _timesync_status_pub{ORB_ID(timesync_status)}; +private: + /** + * Online exponential filter to smooth time offset + */ + void add_sample(int64_t offset_us); + + uORB::PublicationMulti _timesync_status_pub{ORB_ID(timesync_status)}; uint32_t _sequence{0}; @@ -146,7 +143,6 @@ protected: // Outlier rejection and filter reset uint32_t _high_deviation_count{0}; - uint32_t _high_rtt_count{0}; - Mavlink *const _mavlink; + uint8_t _source{timesync_status_s::SOURCE_UNKNOWN}; }; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 98f8115d4d..ee34b9e6a7 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1682,7 +1682,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) } gps_message gps_msg{ - .time_usec = vehicle_gps_position.timestamp, + .time_usec = vehicle_gps_position.timestamp_sample, .lat = vehicle_gps_position.lat, .lon = vehicle_gps_position.lon, .alt = vehicle_gps_position.alt, @@ -1700,8 +1700,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) }, .vel_ned_valid = vehicle_gps_position.vel_ned_valid, .nsats = vehicle_gps_position.satellites_used, - .pdop = sqrtf(vehicle_gps_position.hdop *vehicle_gps_position.hdop - + vehicle_gps_position.vdop * vehicle_gps_position.vdop), + .pdop = vehicle_gps_position.pdop, }; _ekf.setGpsData(gps_msg); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 89633391a7..585e50b7a2 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -123,6 +123,7 @@ void LoggedTopics::add_default_topics() add_topic_multi("control_allocator_status", 200, 2); add_optional_topic_multi("rate_ctrl_status", 200, 2); add_optional_topic_multi("telemetry_status", 1000, 4); + add_topic_multi("timesync_status", 1000, 3); // EKF multi topics (currently max 9 estimators) #if CONSTRAINED_MEMORY diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 60a0196099..94c3bbdc63 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015-2021 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2022 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -91,7 +91,6 @@ px4_add_module( mavlink_shell.cpp mavlink_simple_analyzer.cpp mavlink_stream.cpp - mavlink_timesync.cpp mavlink_ulog.cpp MavlinkStatustextHandler.cpp tune_publisher.cpp diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 24351ea38e..67870428bb 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -87,8 +87,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink_ftp(parent), _mavlink_log_handler(parent), _mission_manager(parent), - _parameters_manager(parent), - _mavlink_timesync(parent) + _parameters_manager(parent) { _handle_sens_flow_maxhgt = param_find("SENS_FLOW_MAXHGT"); _handle_sens_flow_maxr = param_find("SENS_FLOW_MAXR"); @@ -304,6 +303,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_request_event(msg); break; + case MAVLINK_MSG_ID_TIMESYNC: + handle_message_timesync(msg); + break; + + case MAVLINK_MSG_ID_SYSTEM_TIME: + handle_message_system_time(msg); + break; + default: break; } @@ -967,7 +974,7 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg) vehicle_odometry_s mocap_odom{}; mocap_odom.timestamp = hrt_absolute_time(); - mocap_odom.timestamp_sample = _mavlink_timesync.sync_stamp(mocap.time_usec); + mocap_odom.timestamp_sample = _timesync.sync_stamp(mocap.time_usec); mocap_odom.x = mocap.x; mocap_odom.y = mocap.y; @@ -1350,7 +1357,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) vehicle_odometry_s visual_odom{}; visual_odom.timestamp = hrt_absolute_time(); - visual_odom.timestamp_sample = _mavlink_timesync.sync_stamp(ev.usec); + visual_odom.timestamp_sample = _timesync.sync_stamp(ev.usec); visual_odom.x = ev.x; visual_odom.y = ev.y; @@ -1389,7 +1396,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) vehicle_odometry_s odometry{}; odometry.timestamp = hrt_absolute_time(); - odometry.timestamp_sample = _mavlink_timesync.sync_stamp(odom.time_usec); + odometry.timestamp_sample = _timesync.sync_stamp(odom.time_usec); /* The position is in a local FRD frame */ odometry.x = odom.x; @@ -1927,7 +1934,7 @@ MavlinkReceiver::handle_message_trajectory_representation_bezier(mavlink_message vehicle_trajectory_bezier_s trajectory_bezier{}; - trajectory_bezier.timestamp = _mavlink_timesync.sync_stamp(trajectory.time_usec); + trajectory_bezier.timestamp = _timesync.sync_stamp(trajectory.time_usec); for (int i = 0; i < vehicle_trajectory_bezier_s::NUMBER_POINTS; ++i) { trajectory_bezier.control_points[i].position[0] = trajectory.pos_x[i]; @@ -2383,7 +2390,6 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.cog * 1e-2f))); // cdeg -> rad gps.vel_ned_valid = true; - gps.timestamp_time_relative = 0; gps.time_utc_usec = hil_gps.time_usec; gps.satellites_used = hil_gps.satellites_visible; @@ -2424,7 +2430,7 @@ 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 = _mavlink_timesync.sync_stamp(landing_target.time_usec); + landing_target_pose.timestamp = _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; @@ -2914,6 +2920,82 @@ void MavlinkReceiver::handle_message_statustext(mavlink_message_t *msg) } } +void MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) +{ + mavlink_system_time_t system_time; + mavlink_msg_system_time_decode(msg, &system_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 = system_time.time_unix_usec > PX4_EPOCH_SECS * 1000000ULL; + + if (!onb_unix_valid && ofb_unix_valid) { + + const time_t tv_sec = system_time.time_unix_usec / 1000000ULL; + + if (llabs(tv.tv_sec - tv_sec) > 5) { + tv.tv_sec = system_time.time_unix_usec / 1000000ULL; + tv.tv_nsec = (system_time.time_unix_usec % 1000000ULL) * 1000ULL; + + if (px4_clock_settime(CLOCK_REALTIME, &tv) == OK) { + // convert to date time + char datetime_buf[40] {}; + struct tm date_time; + time_t utc_time_sec_current = tv.tv_sec + (tv.tv_nsec / 1e9); + localtime_r(&utc_time_sec_current, &date_time); + strftime(datetime_buf, sizeof(datetime_buf), "%a %Y-%m-%d %H:%M:%S %Z", &date_time); + PX4_INFO("[SYSTEM_TIME] setting time: %s", datetime_buf); + + } else { + PX4_ERR("[SYSTEM_TIME] Failed setting realtime clock"); + } + } + } +} + +void MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) +{ + const hrt_abstime now = hrt_absolute_time(); + + mavlink_timesync_t tsync{}; + mavlink_msg_timesync_decode(msg, &tsync); + + if (tsync.tc1 == 0) { + // Message originating from remote system, timestamp and return it + mavlink_timesync_t rsync{}; + rsync.ts1 = tsync.ts1; + rsync.tc1 = now * 1000ULL; + mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &rsync); + + } else if (tsync.tc1 > 0) { + // Message originating from this system, compute time offset from it + + // Calculate the round trip time (RTT) it took the timesync packet to bounce back to us from remote system + uint64_t rtt_us = now - (tsync.ts1 / 1000ULL); + + // Samples with round-trip time higher than 10 milliseconds are not used to update the filter. + if (rtt_us > 10_ms) { + // More than 5 number of such events in a row will throw a warning + if (++_high_rtt_count > 5) { + PX4_WARN("[timesync] RTT too high for timesync: %llu ms (sender: %i)", rtt_us / 1000ULL, msg->compid); + // Reset counter to rate-limit warnings + _high_rtt_count = 0; + } + + } else { + _timesync.set_source(timesync_status_s::SOURCE_PROTOCOL_MAVLINK); + // Calculate time offset between this system and the remote system, assuming RTT for + // the timesync packet is roughly equal both ways. + int64_t offset_us = (int64_t)((tsync.ts1 / 1000ULL) + now - (tsync.tc1 / 1000ULL) * 2) / 2; + + _timesync.update(offset_us); + } + } +} + void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force) { // check HEARTBEATs for timeout @@ -3182,9 +3264,6 @@ MavlinkReceiver::run() /* 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); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index a2d45e0087..624f6d61fb 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -46,7 +46,6 @@ #include "mavlink_mission.h" #include "mavlink_parameters.h" #include "MavlinkStatustextHandler.hpp" -#include "mavlink_timesync.h" #include "tune_publisher.h" #include @@ -55,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -164,7 +164,9 @@ private: void handle_message_distance_sensor(mavlink_message_t *msg); void handle_message_follow_target(mavlink_message_t *msg); void handle_message_generator_status(mavlink_message_t *msg); - void handle_message_set_gps_global_origin(mavlink_message_t *msg); + void handle_message_gimbal_device_information(mavlink_message_t *msg); + void handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg); + void handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg); void handle_message_gps_rtcm_data(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); @@ -184,21 +186,22 @@ private: void handle_message_radio_status(mavlink_message_t *msg); void handle_message_rc_channels(mavlink_message_t *msg); void handle_message_rc_channels_override(mavlink_message_t *msg); + void handle_message_request_event(mavlink_message_t *msg); void handle_message_serial_control(mavlink_message_t *msg); void handle_message_set_actuator_control_target(mavlink_message_t *msg); void handle_message_set_attitude_target(mavlink_message_t *msg); + void handle_message_set_gps_global_origin(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); void handle_message_set_position_target_global_int(mavlink_message_t *msg); void handle_message_set_position_target_local_ned(mavlink_message_t *msg); void handle_message_statustext(mavlink_message_t *msg); + void handle_message_system_time(mavlink_message_t *msg); + void handle_message_timesync(mavlink_message_t *msg); void handle_message_tunnel(mavlink_message_t *msg); void handle_message_trajectory_representation_bezier(mavlink_message_t *msg); void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg); void handle_message_utm_global_position(mavlink_message_t *msg); void handle_message_vision_position_estimate(mavlink_message_t *msg); - void handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg); - void handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg); - void handle_message_gimbal_device_information(mavlink_message_t *msg); #if !defined(CONSTRAINED_FLASH) void handle_message_debug(mavlink_message_t *msg); @@ -206,7 +209,6 @@ private: void handle_message_debug_vect(mavlink_message_t *msg); void handle_message_named_value_float(mavlink_message_t *msg); #endif // !CONSTRAINED_FLASH - void handle_message_request_event(mavlink_message_t *msg); void CheckHeartbeats(const hrt_abstime &t, bool force = false); @@ -245,7 +247,8 @@ private: MavlinkLogHandler _mavlink_log_handler; MavlinkMissionManager _mission_manager; MavlinkParametersManager _parameters_manager; - MavlinkTimesync _mavlink_timesync; + + Timesync _timesync; MavlinkStatustextHandler _mavlink_statustext_handler; mavlink_status_t _status{}; ///< receiver status, used for mavlink_parse_char() @@ -393,6 +396,8 @@ private: hrt_abstime _heartbeat_component_udp_bridge{0}; hrt_abstime _heartbeat_component_uart_bridge{0}; + uint32_t _high_rtt_count{0}; + param_t _handle_sens_flow_maxhgt{PARAM_INVALID}; param_t _handle_sens_flow_maxr{PARAM_INVALID}; param_t _handle_sens_flow_minhgt{PARAM_INVALID}; @@ -407,6 +412,7 @@ private: float _param_ekf2_min_rng{NAN}; float _param_ekf2_rng_a_hmax{NAN}; + DEFINE_PARAMETERS( (ParamFloat) _param_bat_crit_thr, (ParamFloat) _param_bat_emergen_thr, diff --git a/src/modules/mavlink/mavlink_timesync.cpp b/src/modules/mavlink/mavlink_timesync.cpp deleted file mode 100644 index ee0d2e3272..0000000000 --- a/src/modules/mavlink/mavlink_timesync.cpp +++ /dev/null @@ -1,238 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mavlink_timesync.cpp - * Mavlink timesync implementation. - * - * @author Mohammed Kabir - */ - -#include "mavlink_timesync.h" -#include "mavlink_main.h" - -#include - -MavlinkTimesync::MavlinkTimesync(Mavlink *mavlink) : - _mavlink(mavlink) -{ -} - -void -MavlinkTimesync::handle_message(const mavlink_message_t *msg) -{ - switch (msg->msgid) { - case MAVLINK_MSG_ID_TIMESYNC: { - - mavlink_timesync_t tsync = {}; - mavlink_msg_timesync_decode(msg, &tsync); - - const uint64_t now = hrt_absolute_time(); - - if (tsync.tc1 == 0) { // Message originating from remote system, timestamp and return it - - mavlink_timesync_t rsync; - - rsync.tc1 = now * 1000ULL; - rsync.ts1 = tsync.ts1; - - mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &rsync); - - return; - - } else if (tsync.tc1 > 0) { // Message originating from this system, compute time offset from it - - // Calculate time offset between this system and the remote system, assuming RTT for - // the timesync packet is roughly equal both ways. - int64_t offset_us = (int64_t)((tsync.ts1 / 1000ULL) + now - (tsync.tc1 / 1000ULL) * 2) / 2 ; - - // Calculate the round trip time (RTT) it took the timesync packet to bounce back to us from remote system - uint64_t rtt_us = now - (tsync.ts1 / 1000ULL); - - // Calculate the difference of this sample from the current estimate - uint64_t deviation = llabs((int64_t)_time_offset - offset_us); - - if (rtt_us < MAX_RTT_SAMPLE) { // Only use samples with low RTT - - if (sync_converged() && (deviation > MAX_DEVIATION_SAMPLE)) { - - // Increment the counter if we have a good estimate and are getting samples far from the estimate - _high_deviation_count++; - - // We reset the filter if we received 5 consecutive samples which violate our present estimate. - // This is most likely due to a time jump on the offboard system. - if (_high_deviation_count > MAX_CONSECUTIVE_HIGH_DEVIATION) { - PX4_ERR("[timesync] Time jump detected. Resetting time synchroniser."); - // Reset the filter - reset_filter(); - } - - } else { - - // Filter gain scheduling - if (!sync_converged()) { - // Interpolate with a sigmoid function - double progress = (double)_sequence / (double)CONVERGENCE_WINDOW; - double p = 1.0 - exp(0.5 * (1.0 - 1.0 / (1.0 - progress))); - _filter_alpha = p * ALPHA_GAIN_FINAL + (1.0 - p) * ALPHA_GAIN_INITIAL; - _filter_beta = p * BETA_GAIN_FINAL + (1.0 - p) * BETA_GAIN_INITIAL; - - } else { - _filter_alpha = ALPHA_GAIN_FINAL; - _filter_beta = BETA_GAIN_FINAL; - } - - // Perform filter update - add_sample(offset_us); - - // Increment sequence counter after filter update - _sequence++; - - // Reset high deviation count after filter update - _high_deviation_count = 0; - - // Reset high RTT count after filter update - _high_rtt_count = 0; - } - - } else { - // Increment counter if round trip time is too high for accurate timesync - _high_rtt_count++; - - if (_high_rtt_count > MAX_CONSECUTIVE_HIGH_RTT) { - PX4_WARN("[timesync] RTT too high for timesync: %llu ms (sender: %i)", rtt_us / 1000ULL, msg->compid); - // Reset counter to rate-limit warnings - _high_rtt_count = 0; - } - - } - - // Publish status message - timesync_status_s tsync_status{}; - - tsync_status.timestamp = hrt_absolute_time(); - tsync_status.source_protocol = timesync_status_s::SOURCE_PROTOCOL_MAVLINK; - tsync_status.remote_timestamp = tsync.tc1 / 1000ULL; - tsync_status.observed_offset = offset_us; - tsync_status.estimated_offset = (int64_t)_time_offset; - tsync_status.round_trip_time = rtt_us; - - _timesync_status_pub.publish(tsync_status); - } - - break; - } - - case MAVLINK_MSG_ID_SYSTEM_TIME: { - - 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 * 1000000ULL; - - 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("[timesync] Failed setting realtime clock"); - } - } - - break; - } - - default: - break; - } -} - -uint64_t -MavlinkTimesync::sync_stamp(uint64_t usec) -{ - // Only return synchronised stamp if we have converged to a good value - if (sync_converged()) { - return usec + (int64_t)_time_offset; - - } else { - return hrt_absolute_time(); - } -} - -bool -MavlinkTimesync::sync_converged() -{ - return _sequence >= CONVERGENCE_WINDOW; -} - -void -MavlinkTimesync::add_sample(int64_t offset_us) -{ - /* Online exponential smoothing filter. The derivative of the estimate is also - * estimated in order to produce an estimate without steady state lag: - * https://en.wikipedia.org/wiki/Exponential_smoothing#Double_exponential_smoothing - */ - - double time_offset_prev = _time_offset; - - if (_sequence == 0) { // First offset sample - _time_offset = offset_us; - - } else { - // Update the clock offset estimate - _time_offset = _filter_alpha * offset_us + (1.0 - _filter_alpha) * (_time_offset + _time_skew); - - // Update the clock skew estimate - _time_skew = _filter_beta * (_time_offset - time_offset_prev) + (1.0 - _filter_beta) * _time_skew; - } - -} - -void -MavlinkTimesync::reset_filter() -{ - // Do a full reset of all statistics and parameters - _sequence = 0; - _time_offset = 0.0; - _time_skew = 0.0; - _filter_alpha = ALPHA_GAIN_INITIAL; - _filter_beta = BETA_GAIN_INITIAL; - _high_deviation_count = 0; - _high_rtt_count = 0; - -} diff --git a/src/modules/replay/Replay.cpp b/src/modules/replay/Replay.cpp index 68b6cc7b76..3b60bf22af 100644 --- a/src/modules/replay/Replay.cpp +++ b/src/modules/replay/Replay.cpp @@ -953,8 +953,7 @@ Replay::run() hrt_abstime t = hrt_absolute_time(); for (int i = 0; i < 1000; ++i) { - struct timespec ts; - abstime_to_ts(&ts, t); + struct timespec ts = abstime_to_ts(t); px4_clock_settime(CLOCK_MONOTONIC, &ts); t += 10_ms; } @@ -998,8 +997,7 @@ Replay::handleTopicDelay(uint64_t next_file_time, uint64_t timestamp_offset) } // adjust the lockstep time to the publication time - struct timespec ts; - abstime_to_ts(&ts, publish_timestamp); + struct timespec ts = abstime_to_ts(publish_timestamp); px4_clock_settime(CLOCK_MONOTONIC, &ts); } diff --git a/src/modules/sensors/vehicle_gps_position/CMakeLists.txt b/src/modules/sensors/vehicle_gps_position/CMakeLists.txt index 12b625ead6..831dcb5596 100644 --- a/src/modules/sensors/vehicle_gps_position/CMakeLists.txt +++ b/src/modules/sensors/vehicle_gps_position/CMakeLists.txt @@ -41,6 +41,7 @@ target_link_libraries(vehicle_gps_position PRIVATE geo px4_work_queue + timesync ) px4_add_unit_gtest(SRC gps_blending_test.cpp LINKLIBS vehicle_gps_position) diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index f30aef9b5a..ff02dbee9e 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -43,6 +43,8 @@ VehicleGPSPosition::VehicleGPSPosition() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) { + _timesync[0].set_source(timesync_status_s::SOURCE_GPS1); + _timesync[1].set_source(timesync_status_s::SOURCE_GPS2); } VehicleGPSPosition::~VehicleGPSPosition() @@ -118,8 +120,98 @@ void VehicleGPSPosition::Run() if (gps_updated) { any_gps_updated = true; - _sensor_gps_sub[i].copy(&gps_data); - _gps_blending.setGpsData(gps_data, i); + if (_sensor_gps_sub[i].copy(&gps_data)) { + + const bool sync_converged = _timesync[i].sync_converged(); + + if ((gps_data.fix_type >= 3) && (gps_data.time_utc_usec > _gps_utc_time_us[i])) { + // Calculate time offset between GPS time and hrt + int64_t offset_us = (int64_t)gps_data.time_utc_usec - (int64_t)gps_data.timestamp_sample; + + _timesync[i].update(offset_us); + + } else { + _timesync[i].reset_filter(); + } + + _gps_utc_time_us[i] = gps_data.time_utc_usec; + + if (!sync_converged && _timesync[i].sync_converged()) { +#if defined(__PX4_NUTTX) + auto flags = px4_enter_critical_section(); +#endif // __PX4_NUTTX + + // use converged offset between HRT and GPS utc to update system CLOCK_REALTIME + struct timespec ts {}; + px4_clock_gettime(CLOCK_REALTIME, &ts); + + // convert to date time + char datetime_prev[80] {}; + struct tm date_time; + time_t utc_time_sec_current = ts.tv_sec + (ts.tv_nsec / 1e9); + localtime_r(&utc_time_sec_current, &date_time); + strftime(datetime_prev, sizeof(datetime_prev), "%Y-%m-%d %H:%M:%S %Z", &date_time); + + uint64_t ts_abstime_current_us = ts.tv_sec * 1e6 + ts.tv_nsec / 1000; + + uint64_t ts_abstime_us = hrt_absolute_time() + _timesync[i].time_offset(); + + if (llabs((int64_t)ts_abstime_current_us - (int64_t)ts_abstime_us) > (int64_t)10_ms) { + struct timespec ts_new {}; + ts_new.tv_sec = ts_abstime_us / 1000000; + ts_abstime_us -= ts_new.tv_sec * 1000000; + ts_new.tv_nsec = ts_abstime_us * 1000; // microseconds -> nanoseconds + + // TODO: only update if the difference exceeds threshold + if (px4_clock_settime(CLOCK_REALTIME, &ts_new) == 0) { + // convert to date time + char datetime_new[80] {}; + time_t utc_time_sec = ts_new.tv_sec + (ts_new.tv_nsec / 1e9); + localtime_r(&utc_time_sec, &date_time); + strftime(datetime_new, sizeof(datetime_new), "%Y-%m-%d %H:%M:%S %Z", &date_time); + + int64_t old_us = ts.tv_sec * 1e6 + ts.tv_nsec / 1e3; + int64_t new_us = ts_new.tv_sec * 1e6 + ts_new.tv_nsec / 1e3; + + PX4_INFO("sensor_gps:%d updated system time: %s-> %s (%" PRIi64 " ms)", i, datetime_prev, datetime_new, + (new_us - old_us) / 1000); + + _gps_time_set = true; + + // reset all + for (auto ×ync : _timesync) { + timesync.reset_filter(); + } + } + } + +#if defined(__PX4_NUTTX) + px4_leave_critical_section(flags); +#endif // __PX4_NUTTX + } + + // set timestamp_sample from GPS UTC time + if (_timesync[i].sync_converged()) { + const int64_t time_orig = gps_data.timestamp_sample; + const int64_t time_adjusted = gps_data.time_utc_usec - _timesync[i].time_offset(); + + // warn if there was a significant change + int64_t max_diff_us = (gps_data.timestamp - gps_data.timestamp_sample); + int64_t diff = time_adjusted - time_orig; + + if (diff > max_diff_us || diff < -max_diff_us) { + PX4_ERR("bad timestamp sample update %" PRIi64 " -> %" PRIi64 " (diff %" PRIi64 "us), resetting", + time_orig, time_adjusted, diff); + + _timesync[i].reset_filter(); + + } else { + gps_data.timestamp_sample = gps_data.time_utc_usec - _timesync[i].time_offset(); + } + } + + _gps_blending.setGpsData(gps_data, i); + } if (!_sensor_gps_sub[i].registered()) { _sensor_gps_sub[i].registerCallback(); @@ -142,7 +234,8 @@ void VehicleGPSPosition::Publish(const sensor_gps_s &gps, uint8_t selected) { vehicle_gps_position_s gps_output{}; - gps_output.timestamp = gps.timestamp; + gps_output.timestamp_sample = gps.timestamp_sample; + gps_output.time_utc_usec = gps.time_utc_usec; gps_output.lat = gps.lat; gps_output.lon = gps.lon; @@ -154,6 +247,7 @@ void VehicleGPSPosition::Publish(const sensor_gps_s &gps, uint8_t selected) gps_output.epv = gps.epv; gps_output.hdop = gps.hdop; gps_output.vdop = gps.vdop; + gps_output.pdop = sqrtf(gps.hdop * gps.hdop + gps.vdop * gps.vdop); gps_output.noise_per_ms = gps.noise_per_ms; gps_output.jamming_indicator = gps.jamming_indicator; gps_output.jamming_state = gps.jamming_state; @@ -162,7 +256,6 @@ void VehicleGPSPosition::Publish(const sensor_gps_s &gps, uint8_t selected) gps_output.vel_e_m_s = gps.vel_e_m_s; gps_output.vel_d_m_s = gps.vel_d_m_s; gps_output.cog_rad = gps.cog_rad; - gps_output.timestamp_time_relative = gps.timestamp_time_relative; gps_output.heading = gps.heading; gps_output.heading_offset = gps.heading_offset; gps_output.fix_type = gps.fix_type; @@ -170,13 +263,20 @@ void VehicleGPSPosition::Publish(const sensor_gps_s &gps, uint8_t selected) gps_output.satellites_used = gps.satellites_used; gps_output.selected = selected; - + gps_output.timestamp = hrt_absolute_time(); _vehicle_gps_position_pub.publish(gps_output); } void VehicleGPSPosition::PrintStatus() { //PX4_INFO_RAW("[vehicle_gps_position] selected GPS: %d\n", _gps_select_index); + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_timesync[i].sync_converged()) { + + } + } + } }; // namespace sensors diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp index 8126fd5efd..046ba6ea31 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -90,10 +91,16 @@ private: {this, ORB_ID(sensor_gps), 1}, }; + Timesync _timesync[GPS_MAX_RECEIVERS] {}; + + uint64_t _gps_utc_time_us[GPS_MAX_RECEIVERS] {}; + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; GpsBlending _gps_blending; + bool _gps_time_set{false}; + DEFINE_PARAMETERS( (ParamInt) _param_sens_gps_mask, (ParamFloat) _param_sens_gps_tau, diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp index b37d91c6ba..4ac9b6649c 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp @@ -108,22 +108,22 @@ bool GpsBlending::blend_gps_data(uint64_t hrt_now_us) float raw_dt = 0.f; - if (_gps_state[i].timestamp > _time_prev_us[i]) { - raw_dt = 1e-6f * (_gps_state[i].timestamp - _time_prev_us[i]); + if (_gps_state[i].timestamp_sample > _time_prev_us[i]) { + raw_dt = 1e-6f * (_gps_state[i].timestamp_sample - _time_prev_us[i]); } float present_dt = 0.f; - if (hrt_now_us > _gps_state[i].timestamp) { - present_dt = 1e-6f * (hrt_now_us - _gps_state[i].timestamp); + if (hrt_now_us > _gps_state[i].timestamp_sample) { + present_dt = 1e-6f * (hrt_now_us - _gps_state[i].timestamp_sample); } if (raw_dt > 0.0f && raw_dt < GPS_TIMEOUT_S) { _gps_dt[i] = 0.1f * raw_dt + 0.9f * _gps_dt[i]; - } else if ((present_dt >= GPS_TIMEOUT_S) && (_gps_state[i].timestamp > 0)) { + } else if ((present_dt >= GPS_TIMEOUT_S) && (_gps_state[i].timestamp_sample > 0)) { // Timed out - kill the stored fix for this receiver and don't track its (stale) gps_dt - _gps_state[i].timestamp = 0; + _gps_state[i].timestamp_sample = 0; _gps_state[i].fix_type = 0; _gps_state[i].satellites_used = 0; _gps_state[i].vel_ned_valid = 0; @@ -160,13 +160,13 @@ bool GpsBlending::blend_gps_data(uint64_t hrt_now_us) for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { // Find largest and smallest times - if (_gps_state[i].timestamp > max_us) { - max_us = _gps_state[i].timestamp; + if (_gps_state[i].timestamp_sample > max_us) { + max_us = _gps_state[i].timestamp_sample; _gps_newest_index = i; } - if ((_gps_state[i].timestamp < min_us) && (_gps_state[i].timestamp > 0)) { - min_us = _gps_state[i].timestamp; + if ((_gps_state[i].timestamp_sample < min_us) && (_gps_state[i].timestamp_sample > 0)) { + min_us = _gps_state[i].timestamp_sample; } } @@ -197,7 +197,7 @@ bool GpsBlending::blend_gps_data(uint64_t hrt_now_us) // both receivers running at different rates _gps_time_ref_index = _gps_slowest_index; - if (_gps_state[_gps_time_ref_index].timestamp > _time_prev_us[_gps_time_ref_index]) { + if (_gps_state[_gps_time_ref_index].timestamp_sample > _time_prev_us[_gps_time_ref_index]) { // blend data at the rate of the slower receiver gps_new_output_data = true; } @@ -360,7 +360,7 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS // combine the the GPS states into a blended solution using the weights calculated in calc_blend_weights() for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { // blend the timing data - gps_blended_state.timestamp += (uint64_t)((double)_gps_state[i].timestamp * (double)blend_weights[i]); + gps_blended_state.timestamp_sample += (uint64_t)((double)_gps_state[i].timestamp_sample * (double)blend_weights[i]); // use the highest status if (_gps_state[i].fix_type > gps_blended_state.fix_type) { @@ -499,12 +499,12 @@ void GpsBlending::update_gps_offsets(const sensor_gps_s &gps_blended_state) float omega_lpf = 1.0f / fmaxf(_blending_time_constant, 1.0f); for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - if (_gps_state[i].timestamp - _time_prev_us[i] > 0) { + if (_gps_state[i].timestamp_sample - _time_prev_us[i] > 0) { // calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter - alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]), + alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp_sample - _time_prev_us[i]), 0.0f, 1.0f); - _time_prev_us[i] = _gps_state[i].timestamp; + _time_prev_us[i] = _gps_state[i].timestamp_sample; } } diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp index 8d46073cb1..e1c296b173 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp @@ -58,7 +58,7 @@ public: sensor_gps_s GpsBlendingTest::getDefaultGpsData() { sensor_gps_s gps_data{}; - gps_data.timestamp = _time_now_us - 10e3; + gps_data.timestamp_sample = _time_now_us - 10e3; gps_data.time_utc_usec = 0; gps_data.lat = 47e7; gps_data.lon = 9e7; @@ -77,7 +77,6 @@ sensor_gps_s GpsBlendingTest::getDefaultGpsData() gps_data.vel_e_m_s = 1.f; gps_data.vel_d_m_s = 1.f; gps_data.cog_rad = 0.f; - gps_data.timestamp_time_relative = 0; gps_data.heading = NAN; gps_data.heading_offset = 0.f; gps_data.fix_type = 4; @@ -97,7 +96,7 @@ void GpsBlendingTest::runSeconds(float duration_s, GpsBlending &gps_blending, se gps_blending.update(_time_now_us); _time_now_us += dt_us; - gps_data.timestamp += dt_us; + gps_data.timestamp_sample += dt_us; } } @@ -114,8 +113,8 @@ void GpsBlendingTest::runSeconds(float duration_s, GpsBlending &gps_blending, se gps_blending.update(_time_now_us); _time_now_us += dt_us; - gps_data0.timestamp += dt_us; - gps_data1.timestamp += dt_us; + gps_data0.timestamp_sample += dt_us; + gps_data1.timestamp_sample += dt_us; } } @@ -143,7 +142,7 @@ TEST_F(GpsBlendingTest, singleReceiver) gps_blending.update(_time_now_us); _time_now_us += 200e3; - gps_data.timestamp = _time_now_us - 10e3; + gps_data.timestamp_sample = _time_now_us - 10e3; gps_blending.setGpsData(gps_data, 1); gps_blending.update(_time_now_us); @@ -261,7 +260,7 @@ TEST_F(GpsBlendingTest, dualReceiverFailover) // AND IF: the primary receiver is available again and has // better metrics than the secondary one - gps_data0.timestamp = gps_data1.timestamp; + gps_data0.timestamp_sample = gps_data1.timestamp_sample; gps_data0.satellites_used = gps_data1.satellites_used + 2; runSeconds(1.f, gps_blending, gps_data0, gps_data1); diff --git a/src/modules/simulator/sensor_gps_sim/SensorGpsSim.cpp b/src/modules/simulator/sensor_gps_sim/SensorGpsSim.cpp index 366f8a8348..7d1bfe70c0 100644 --- a/src/modules/simulator/sensor_gps_sim/SensorGpsSim.cpp +++ b/src/modules/simulator/sensor_gps_sim/SensorGpsSim.cpp @@ -150,7 +150,7 @@ void SensorGpsSim::Run() sensor_gps.vdop = 100.f; } - // sensor_gps.timestamp_sample = gpos.timestamp; + sensor_gps.timestamp_sample = gpos.timestamp; sensor_gps.time_utc_usec = 0; sensor_gps.device_id = device_id.devid; sensor_gps.lat = roundf(latitude * 1e7); // Latitude in 1E-7 degrees @@ -165,7 +165,6 @@ void SensorGpsSim::Run() sensor_gps.vel_d_m_s = gps_vel(2); sensor_gps.cog_rad = atan2(gps_vel(1), gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) - sensor_gps.timestamp_time_relative = 0; sensor_gps.heading = NAN; sensor_gps.heading_offset = NAN; sensor_gps.heading_accuracy = 0; @@ -173,7 +172,6 @@ void SensorGpsSim::Run() sensor_gps.jamming_state = 0; sensor_gps.vel_ned_valid = true; sensor_gps.satellites_used = _sim_gps_used.get(); - sensor_gps.timestamp = hrt_absolute_time(); _sensor_gps_pub.publish(sensor_gps); } diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 4663f4d6f1..0975893037 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -391,7 +391,7 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg) if (!_gps_blocked) { sensor_gps_s gps{}; - + gps.timestamp_sample = hrt_absolute_time(); gps.lat = hil_gps.lat; gps.lon = hil_gps.lon; gps.alt = hil_gps.alt; @@ -419,7 +419,6 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg) gps.cog_rad = ((hil_gps.cog == 65535) ? NAN : matrix::wrap_2pi(math::radians(hil_gps.cog * 1e-2f))); // cdeg -> rad gps.vel_ned_valid = true; - gps.timestamp_time_relative = 0; gps.time_utc_usec = hil_gps.time_usec; gps.satellites_used = hil_gps.satellites_visible; @@ -463,8 +462,7 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg) mavlink_hil_sensor_t imu; mavlink_msg_hil_sensor_decode(msg, &imu); - struct timespec ts; - abstime_to_ts(&ts, imu.time_usec); + struct timespec ts = abstime_to_ts(imu.time_usec); px4_clock_settime(CLOCK_MONOTONIC, &ts); hrt_abstime now_us = hrt_absolute_time(); diff --git a/src/systemcmds/system_time/system_time.cpp b/src/systemcmds/system_time/system_time.cpp index 4d4b2d8feb..153c736af4 100644 --- a/src/systemcmds/system_time/system_time.cpp +++ b/src/systemcmds/system_time/system_time.cpp @@ -124,4 +124,4 @@ $ system_time get PRINT_MODULE_USAGE_COMMAND_DESCR("set", "Set the system time, provide time in unix epoch time format"); PRINT_MODULE_USAGE_COMMAND_DESCR("get", "Get the system time"); -} \ No newline at end of file +}