Merge branch 'main' into dev/dshot_stuff2

platforms/common/apps.cpp.in

@@ -2,6 +2,7 @@
 #include <px4_platform_common/time.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/log.h>
+#include <uORB/uORB.h>
 
 #include "apps.h"
 
@@ -43,6 +44,7 @@ void list_builtins(apps_map_type &apps)
 int shutdown_main(int argc, char *argv[])
 {
 	printf("Exiting NOW.\n");
+	uorb_shutdown();
 	system_exit(0);
 }
 

platforms/common/shutdown.cpp

@@ -49,6 +49,7 @@
 
 #include <px4_platform_common/external_reset_lockout.h>
 #include <px4_platform_common/log.h>
+#include <uORB/uORB.h>
 
 #include <stdint.h>
 #include <errno.h>
@@ -173,6 +174,8 @@ static void shutdown_worker(void *arg)
 	const bool delay_elapsed = (now > shutdown_time_us);
 
 	if (delay_elapsed && ((done && shutdown_lock_counter == 0) || (now > (shutdown_time_us + shutdown_timeout_us)))) {
+		uorb_shutdown();
+
 		if (shutdown_args & SHUTDOWN_ARG_REBOOT) {
 #if defined(CONFIG_BOARDCTL_RESET)
 			PX4_INFO_RAW("Reboot NOW.");

platforms/common/uORB/uORB.cpp

@@ -51,6 +51,7 @@
 #include <sys/boardctl.h>
 #endif
 
+
 static uORB::DeviceMaster *g_dev = nullptr;
 
 int uorb_start(void)
@@ -113,6 +114,18 @@ int uorb_top(char **topic_filter, int num_filters)
 	return OK;
 }
 
+void uorb_shutdown(void)
+{
+#ifdef CONFIG_ORB_COMMUNICATOR
+	uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
+
+	if (ch) {
+		ch->shutdown();
+	}
+
+#endif /* CONFIG_ORB_COMMUNICATOR */
+}
+
 orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
 {
 	return uORB::Manager::get_instance()->orb_advertise(meta, data);

platforms/common/uORB/uORB.h

@@ -121,6 +121,7 @@ __BEGIN_DECLS
 int uorb_start(void);
 int uorb_status(void);
 int uorb_top(char **topic_filter, int num_filters);
+void uorb_shutdown(void);
 
 /**
  * ORB topic advertiser handle.

platforms/common/uORB/uORBCommunicator.hpp

@@ -129,6 +129,22 @@ public:
 
 	virtual int16_t send_message(const char *messageName, int32_t length, uint8_t *data) = 0;
 
+
+	//=========================================================================
+	//     INTERFACES FOR Lifecycle messages
+	//=========================================================================
+
+	/**
+	 * @brief Interface to notify the remote entity of a shutdown.
+	 *
+	 * @return
+	 * 	0 = success; This means the shutdown is successfully sent to the receiver
+	 * 		Note: This does not mean that the receiver has received it.
+	 *  otherwise = failure.
+	 */
+
+	virtual int16_t shutdown() { return 0; }
+
 };
 
 /**

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

@@ -51,7 +51,7 @@
 #include <errno.h>
 #include "hrt_work.h"
 
-// Voxl2 board specific API definitions to get time offset
+// Voxl board specific API definitions to get time
 #if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
 #include "fc_sensor.h"
 #endif
@@ -112,29 +112,6 @@ 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)
 }
@@ -476,8 +453,21 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
 	}
 
 #endif // defined(ENABLE_LOCKSTEP_SCHEDULER)
-	return system_clock_gettime(clk_id, tp);
 
+	int rv = system_clock_gettime(clk_id, tp);
+
+# if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
+
+	// On VOXL use DSP clock as reference for MONOTONIC
+	if (clk_id == CLOCK_MONOTONIC) {
+		hrt_abstime temp_abstime = fc_sensor_get_dsp_timestamp_us();
+		tp->tv_sec = temp_abstime / 1000000;
+		tp->tv_nsec = (temp_abstime % 1000000) * 1000;
+	}
+
+# endif // defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP)
+
+	return rv;
 }
 
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)

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

@@ -73,6 +73,7 @@
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/tasks.h>
 #include <px4_platform_common/posix.h>
+#include <uORB/uORB.h>
 
 #include "apps.h"
 #include "px4_daemon/client.h"
@@ -505,6 +506,7 @@ void sig_int_handler(int sig_num)
 	fflush(stdout);
 	printf("\nPX4 Exiting...\n");
 	fflush(stdout);
+	uorb_shutdown();
 	px4_daemon::Pxh::stop();
 	_exit_requested = true;
 }

platforms/qurt/src/px4/SerialImpl.cpp

@@ -416,12 +416,8 @@ bool SerialImpl::getSingleWireMode() const
 
 bool SerialImpl::setSingleWireMode()
 {
-	if (enable) {
-		PX4_ERR("Qurt platform does not support single wire mode");
-		return false;
-	}
-
-	return true;
+	PX4_ERR("Qurt platform does not support single wire mode");
+	return false;
 }
 
 bool SerialImpl::getSwapRxTxMode() const
@@ -431,12 +427,8 @@ bool SerialImpl::getSwapRxTxMode() const
 
 bool SerialImpl::setSwapRxTxMode()
 {
-	if (enable) {
-		PX4_ERR("Qurt platform does not support swap rx tx mode");
-		return false;
-	}
-
-	return true;
+	PX4_ERR("Qurt platform does not support swap rx tx mode");
+	return false;
 }
 
 bool SerialImpl::setInvertedMode(bool enable)