diff --git a/.gitmodules b/.gitmodules index e91cf2b63a14..e443da19a290 100644 --- a/.gitmodules +++ b/.gitmodules @@ -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 diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index e0cc5693edb3..734d6f5323d5 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -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 diff --git a/boards/modalai/voxl2/cmake/init.cmake b/boards/modalai/voxl2/cmake/init.cmake new file mode 100644 index 000000000000..e5a4daad5f8b --- /dev/null +++ b/boards/modalai/voxl2/cmake/init.cmake @@ -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) \ No newline at end of file diff --git a/boards/modalai/voxl2/cmake/link_libraries.cmake b/boards/modalai/voxl2/cmake/link_libraries.cmake index f250f52e211b..e0b09beb7038 100644 --- a/boards/modalai/voxl2/cmake/link_libraries.cmake +++ b/boards/modalai/voxl2/cmake/link_libraries.cmake @@ -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} ) diff --git a/src/modules/muorb/apps/libfc-sensor-api b/boards/modalai/voxl2/libfc-sensor-api similarity index 100% rename from src/modules/muorb/apps/libfc-sensor-api rename to boards/modalai/voxl2/libfc-sensor-api diff --git a/boards/modalai/voxl2/scripts/build-deps.sh b/boards/modalai/voxl2/scripts/build-deps.sh index b76cd0d7fa79..fe7f85486888 100755 --- a/boards/modalai/voxl2/scripts/build-deps.sh +++ b/boards/modalai/voxl2/scripts/build-deps.sh @@ -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 ../../../../.. diff --git a/platforms/posix/src/px4/common/drv_hrt.cpp b/platforms/posix/src/px4/common/drv_hrt.cpp index 9634511bf3fc..b510229bc0cf 100644 --- a/platforms/posix/src/px4/common/drv_hrt.cpp +++ b/platforms/posix/src/px4/common/drv_hrt.cpp @@ -51,6 +51,11 @@ #include #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 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) diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 3e295abfc949..bb62da0189c3 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -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; } /** diff --git a/src/modules/muorb/apps/CMakeLists.txt b/src/modules/muorb/apps/CMakeLists.txt index 5a608bec5301..532ada62413b 100644 --- a/src/modules/muorb/apps/CMakeLists.txt +++ b/src/modules/muorb/apps/CMakeLists.txt @@ -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 diff --git a/src/modules/muorb/apps/Kconfig b/src/modules/muorb/apps/Kconfig index 21969d591d13..f0afac8b6204 100644 --- a/src/modules/muorb/apps/Kconfig +++ b/src/modules/muorb/apps/Kconfig @@ -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