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

[WIP]: GPS clock set time updates

Browse Source 
- remove drivers/gps set clock callback
 - drivers/gps try to set sensor_gps timestamp_sample as closely as possible
 - move mavlink timesync to new lib/timesync library
 - sensors/vehicle_gps_position consume all sensor_gps sources and determine time GPS utc time offset using lib/timesync
 - once time offset is stable update system clock if sufficiently different
This commit is contained in:
Daniel Agar
2022-01-28 13:48:35 -05:00
parent 0104b84289
commit 3df000f77b
39 changed files with 599 additions and 428 deletions
Download Patch File Download Diff File Expand all files Collapse all files
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
+2 -2
View File
@@ -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};
platforms/common/include/px4_platform_common/tasks.h
+3 -3
View File
@@ -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)
platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c
+20 -5
View File
@@ -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;
}
/**
platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c
+33 -12
View File
@@ -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;
}
/**
platforms/nuttx/src/px4/nxp/s32k1xx/hrt/hrt.c
+20 -5
View File
@@ -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;
}
/**
platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c
+38 -16
View File
@@ -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) {
platforms/posix/src/px4/common/drv_hrt.cpp
+7 -5
View File
@@ -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)