mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-28 10:46:33 +08:00
VOXL2: HRT updates for synchronization with Qurt time (#22881)
- Added offset to Posix hrt time to account for synchronization with Qurt hrt time - Added new Kconfig to configure synchronization of HRT timestamps on VOXL2 - Moved voxl2 libfc sensor library submodule from muorb module to boards directory - Added check to make sure hrt_elapsed_time can never be negative
This commit is contained in:
@@ -51,6 +51,11 @@
|
||||
#include <errno.h>
|
||||
#include "hrt_work.h"
|
||||
|
||||
// Voxl2 board specific API definitions to get time offset
|
||||
#if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
|
||||
#include "fc_sensor.h"
|
||||
#endif
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
#include <lockstep_scheduler/lockstep_scheduler.h>
|
||||
static LockstepScheduler lockstep_scheduler {true};
|
||||
@@ -107,6 +112,29 @@ hrt_abstime hrt_absolute_time()
|
||||
#else // defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
struct timespec ts;
|
||||
px4_clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||
|
||||
# if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
|
||||
hrt_abstime temp_abstime = ts_to_abstime(&ts);
|
||||
int apps_time_offset = fc_sensor_get_time_offset();
|
||||
|
||||
if (apps_time_offset < 0) {
|
||||
hrt_abstime temp_offset = -apps_time_offset;
|
||||
|
||||
if (temp_offset >= temp_abstime) {
|
||||
temp_abstime = 0;
|
||||
|
||||
} else {
|
||||
temp_abstime -= temp_offset;
|
||||
}
|
||||
|
||||
} else {
|
||||
temp_abstime += (hrt_abstime) apps_time_offset;
|
||||
}
|
||||
|
||||
ts.tv_sec = temp_abstime / 1000000;
|
||||
ts.tv_nsec = (temp_abstime % 1000000) * 1000;
|
||||
# endif // defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
|
||||
|
||||
return ts_to_abstime(&ts);
|
||||
#endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
}
|
||||
@@ -449,6 +477,7 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
|
||||
|
||||
#endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
return system_clock_gettime(clk_id, tp);
|
||||
|
||||
}
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
|
||||
Reference in New Issue
Block a user