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:
parent
c024ea396a
commit
4a553938fb
|
@ -81,5 +81,5 @@
|
|||
url = https://github.com/PX4/PX4-gazebo-models.git
|
||||
branch = main
|
||||
[submodule "boards/modalai/voxl2/libfc-sensor-api"]
|
||||
path = src/modules/muorb/apps/libfc-sensor-api
|
||||
path = boards/modalai/voxl2/libfc-sensor-api
|
||||
url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git
|
||||
|
|
|
@ -30,5 +30,5 @@ exec find boards msg src platforms test \
|
|||
-path src/lib/cdrstream/cyclonedds -prune -o \
|
||||
-path src/lib/cdrstream/rosidl -prune -o \
|
||||
-path src/modules/zenoh/zenoh-pico -prune -o \
|
||||
-path src/modules/muorb/apps/libfc-sensor-api -prune -o \
|
||||
-path boards/modalai/voxl2/libfc-sensor-api -prune -o \
|
||||
-type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN
|
||||
|
|
|
@ -0,0 +1,34 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2024 ModalAI, Inc. 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_directories(${PX4_BOARD_DIR}/libfc-sensor-api/inc)
|
|
@ -1,7 +1,7 @@
|
|||
|
||||
# Link against the public stub version of the proprietary fc sensor library
|
||||
target_link_libraries(px4 PRIVATE
|
||||
${PX4_SOURCE_DIR}/src//modules/muorb/apps/libfc-sensor-api/build/libfc_sensor.so
|
||||
${PX4_BOARD_DIR}/libfc-sensor-api/build/libfc_sensor.so
|
||||
px4_layer
|
||||
${module_libraries}
|
||||
)
|
||||
|
|
|
@ -1,10 +1,9 @@
|
|||
#!/bin/bash
|
||||
|
||||
cd src/modules/muorb/apps/libfc-sensor-api
|
||||
cd boards/modalai/voxl2/libfc-sensor-api
|
||||
rm -fR build
|
||||
mkdir build
|
||||
cd build
|
||||
CC=/home/4.1.0.4/tools/linaro64/bin/aarch64-linux-gnu-gcc cmake ..
|
||||
make
|
||||
cd ../../../../../..
|
||||
|
||||
cd ../../../../..
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -162,7 +162,16 @@ static inline void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
|
|||
*/
|
||||
static inline hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
|
||||
{
|
||||
return hrt_absolute_time() - *then;
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
// Cannot allow a negative elapsed time as this would appear
|
||||
// to be a huge positive elapsed time when represented as an
|
||||
// unsigned value!
|
||||
if (*then > now) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
return now - *then;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
@ -39,7 +39,7 @@ px4_add_module(
|
|||
INCLUDES
|
||||
../test
|
||||
../aggregator
|
||||
libfc-sensor-api/inc
|
||||
${PX4_BOARD_DIR}/libfc-sensor-api/inc
|
||||
SRCS
|
||||
uORBAppsProtobufChannel.cpp
|
||||
muorb_main.cpp
|
||||
|
|
|
@ -4,3 +4,11 @@ menuconfig MODULES_MUORB_APPS
|
|||
depends on PLATFORM_POSIX
|
||||
---help---
|
||||
Enable support for muorb apps
|
||||
|
||||
|
||||
config MUORB_APPS_SYNC_TIMESTAMP
|
||||
bool "Sync timestamp with external processor"
|
||||
depends on MODULES_MUORB_APPS
|
||||
default y
|
||||
help
|
||||
causes HRT timestamp to use an externally calculated offset for synchronization
|
||||
|
|
Loading…
Reference in New Issue