VOXL2: HRT updates for synchronization with Qurt time (PX4#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

.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

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

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)

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}
 )

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 ../../../../..

platforms/posix/src/px4/common/drv_hrt.cpp

@@ -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)

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;
 }
 
 /**

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

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