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:
Eric Katzfey
2024-03-22 12:24:51 -07:00
committed by GitHub
parent c024ea396a
commit 4a553938fb
10 changed files with 87 additions and 8 deletions
@@ -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)