muorb: abs time on aDSP in sync with Linux side

This adds a call on startup of the muorb on the aDSP side to use an
offset for hrt_absolute_call(). This means that the hrt_absolute_call()
on the appsproc (Linux) side should now match the one on the aDSP
(QURT) side.
The accuracy still needs to be determined.

src/modules/muorb/adsp/px4muorb.cpp

@@ -30,6 +30,7 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
+
 #include "px4muorb.hpp"
 #include "uORBFastRpcChannel.hpp"
 #include "uORBManager.hpp"
@@ -49,15 +50,27 @@ __END_DECLS
 int px4muorb_orb_initialize()
 {
 	HAP_power_request(100, 100, 1000);
+
 	// register the fastrpc muorb with uORBManager.
 	uORB::Manager::get_instance()->set_uorb_communicator(uORB::FastRpcChannel::GetInstance());
-	const char *argv[] = { "dspal", "start" };
+	const char *argv[] = {"dspal", "start"};
 	int argc = 2;
 	int rc;
 	rc = dspal_main(argc, (char **)argv);
 	return rc;
 }
 
+int px4muorb_set_absolute_time_offset(int32_t time_diff_us)
+{
+	return hrt_set_absolute_time_offset(time_diff_us);
+}
+
+int px4muorb_get_absolute_time(uint64_t *time_us)
+{
+	*time_us = hrt_absolute_time();
+	return 0;
+}
+
 int px4muorb_add_subscriber(const char *name)
 {
 	int rc = 0;

src/modules/muorb/adsp/px4muorb.hpp

@@ -39,6 +39,10 @@ extern "C" {
 
 	int px4muorb_orb_initialize() __EXPORT;
 
+	int px4muorb_set_absolute_time_offset(int32_t time_diff_us) __EXPORT;
+
+	int px4muorb_get_absolute_time(uint64_t *time_us) __EXPORT;
+
 	int px4muorb_add_subscriber(const char *name) __EXPORT;
 
 	int px4muorb_remove_subscriber(const char *name) __EXPORT;

src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp

@@ -30,6 +30,10 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
+
+#include <time.h>
+#include <unistd.h>
+#include <fcntl.h>
 #include "px4muorb_KraitRpcWrapper.hpp"
 #include <rpcmem.h>
 #include "px4muorb.h"
@@ -59,6 +63,73 @@ static const uint32_t _MAX_TOPICS = 64;
 static const uint32_t _MAX_BULK_TRANSFER_BUFFER_SIZE = _MAX_TOPIC_DATA_BUFFER_SIZE * _MAX_TOPICS;
 static uint8_t *_BulkTransferBuffer = 0;
 
+// The DSP timer can be read from this file.
+#define DSP_TIMER_FILE "/sys/kernel/boot_adsp/qdsp_qtimer"
+
+/**
+ * Helper function to get timer difference between time on DSP and appsproc side.
+ * Usually the DSP gets started around 2s before the appsproc (Linux) side and
+ * therefore the clocks are not in sync. We change the clock on the DSP side but
+ * for this we need to find the offset first and then tell code on the DSP side.
+ *
+ * @param time_diff_us: pointer to time offset to set.
+ * @return: 0 on success, < 0 on error.
+ */
+int calc_timer_diff_to_dsp_us(int32_t *time_diff_us);
+
+int calc_timer_diff_to_dsp_us(int32_t *time_diff_us)
+{
+	int fd = open(DSP_TIMER_FILE, O_RDONLY);
+
+	if (fd < 0) {
+		PX4_ERR("Could not open DSP timer file %s.", DSP_TIMER_FILE);
+		return -1;
+	}
+
+	char buffer[21];
+	memset(buffer, 0, sizeof(buffer));
+	int bytes_read = read(fd, buffer, sizeof(buffer));
+
+	if (bytes_read < 0) {
+		PX4_ERR("Could not read DSP timer file %s.", DSP_TIMER_FILE);
+		close(fd);
+		return -2;
+	}
+
+	// Do this call right after reading to avoid latency here.W
+	timespec ts;
+	clock_gettime(CLOCK_MONOTONIC, &ts);
+	uint64_t time_appsproc = ((uint64_t)ts.tv_sec) * 1000000llu + (ts.tv_nsec / 1000);
+
+	close(fd);
+
+	uint64_t time_dsp;
+	int ret  = sscanf(buffer, "%llx", &time_dsp);
+
+	if (ret < 0) {
+		PX4_ERR("Could not parse DSP timer.");
+		return -3;
+	}
+
+	time_dsp /= 19.2;
+
+	// Before casting to in32_t, check if it fits.
+	uint64_t abs_diff = (time_appsproc > time_dsp)
+			    ? (time_appsproc - time_dsp) : (time_dsp - time_appsproc);
+
+	if (abs_diff > INT32_MAX) {
+		PX4_ERR("Timer difference too big");
+		return -4;
+	}
+
+	*time_diff_us = time_appsproc - time_dsp;
+
+	PX4_DEBUG("found time_dsp: %llu us, time_appsproc: %llu us", time_dsp, time_appsproc);
+	PX4_DEBUG("found time_diff: %li us, %.6f s", *time_diff_us, ((double)*time_diff_us) / 1e6);
+
+	return 0;
+}
+
 
 px4muorb::KraitRpcWrapper::KraitRpcWrapper()
 {
@@ -122,13 +193,32 @@ bool px4muorb::KraitRpcWrapper::Initialize()
 		PX4_DEBUG("%s rpcmem_alloc passed for data_buffer", __FUNCTION__);
 	}
 
-	// call myorb intiialize rotine.
+	int32_t time_diff_us;
+
+	if (calc_timer_diff_to_dsp_us(&time_diff_us) != 0) {
+		rc = false;
+		return rc;
+	}
+
+	// call muorb initialize routine.
 	if (px4muorb_orb_initialize() != 0) {
 		PX4_ERR("%s Error calling the uorb fastrpc initalize method..", __FUNCTION__);
 		rc = false;
 		return rc;
 	}
 
+	// TODO FIXME: remove this check or make it less verbose later
+	px4muorb_set_absolute_time_offset(time_diff_us);
+
+	uint64_t time_dsp;
+	px4muorb_get_absolute_time(&time_dsp);
+
+	uint64_t time_appsproc = hrt_absolute_time();
+
+	int diff = (time_dsp - time_appsproc);
+
+	PX4_INFO("time_dsp: %llu us, time appsproc: %llu us, diff: %d us", time_dsp, time_appsproc, diff);
+
 	_Initialized = true;
 	return rc;
 }