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/drivers/drv_hrt.h

@@ -109,6 +109,15 @@ __EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then);
  */
 __EXPORT extern hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now);
 
+#ifdef __PX4_QURT
+/**
+ * Set a time offset to hrt_absolute_time on the DSP.
+ * @param time_diff_us: time difference of the DSP clock to Linux clock.
+ *   This param is positive because the Linux clock is ahead of the DSP one.
+ */
+__EXPORT extern int hrt_set_absolute_time_offset(int32_t time_diff_us);
+#endif
+
 /**
  * Call callout(arg) after delay has elapsed.
  *

src/firmware/qurt/px4muorb.idl

@@ -1,6 +1,6 @@
 //=======================================================================
 // Copyright (c) 2016, Mark Charlebois. All rights reserved.
-// 
+//
 // Redistribution and use in source and binary forms, with or without
 // modification, are permitted provided that the following conditions are
 // met:
@@ -13,7 +13,7 @@
 //     * Neither the name of The Linux Foundation 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 "AS IS" AND ANY EXPRESS OR IMPLIED
 // WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 // MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
@@ -30,12 +30,26 @@
 #include "AEEStdDef.idl"
 
 interface px4muorb{
-  
+
    /**
     * interface method to start the uorb service and initialize the muorb
     */
    AEEResult orb_initialize();
 
+   /**
+    * Interface to set an offset to hrt_absolute_time on the DSP.
+    *
+    * @param time_diff_us: time difference of the DSP clock to Linux clock.
+    *   A positive number means the Linux clock is ahead of the DSP one.
+    */
+   AEEResult set_absolute_time_offset( in long time_diff_us );
+
+   /**
+    * Interface to request hrt_absolute_time on the DSP.
+    * @param time_us: pointer to time in us
+    */
+   AEEResult get_absolute_time(rout unsigned long long time_us);
+
    /**
     * Interface to add a subscriber to the identified topic
     * @param topic_name
@@ -130,6 +144,6 @@ interface px4muorb{
      * @return status
      *    0 = success
      *    all others is a failure.
-     **/ 
+     **/
     AEEResult receive_bulk_data( rout sequence<octet> data, rout long bytes_returned, rout long topic_count );
 };

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

src/platforms/posix/px4_layer/drv_hrt.c

@@ -66,7 +66,11 @@ static void		hrt_call_reschedule(void);
 
 static px4_sem_t 	_hrt_lock;
 static struct work_s	_hrt_work;
+#ifndef __PX4_QURT
 static hrt_abstime px4_timestart = 0;
+#else
+static int32_t dsp_offset = 0;
+#endif
 static hrt_abstime _start_delay_time = 0;
 static hrt_abstime _delay_interval = 0;
 static hrt_abstime max_time = 0;
@@ -150,6 +154,19 @@ hrt_abstime _hrt_absolute_time_internal(void)
 {
 	struct timespec ts;
 
+#if defined(__PX4_QURT)
+	// Don't use the timestart on the DSP on Snapdragon because we manually
+	// set the px4_timestart using the hrt_set_absolute_time_offset().
+	px4_clock_gettime(CLOCK_MONOTONIC, &ts);
+	return ts_to_abstime(&ts) + dsp_offset;
+
+#elif defined(__PX4_POSIX_EAGLE)
+	// Don't do any offseting on the Linux side on the Snapdragon.
+	px4_clock_gettime(CLOCK_MONOTONIC, &ts);
+	return ts_to_abstime(&ts);
+
+#else
+
 	if (!px4_timestart) {
 		px4_clock_gettime(CLOCK_MONOTONIC, &ts);
 		px4_timestart = ts_to_abstime(&ts);
@@ -157,8 +174,17 @@ hrt_abstime _hrt_absolute_time_internal(void)
 
 	px4_clock_gettime(CLOCK_MONOTONIC, &ts);
 	return ts_to_abstime(&ts) - px4_timestart;
+#endif
 }
 
+#ifdef __PX4_QURT
+int hrt_set_absolute_time_offset(int32_t time_diff_us)
+{
+	dsp_offset = time_diff_us;
+	return 0;
+}
+#endif
+
 /*
  * Get absolute time.
  */
@@ -190,7 +216,9 @@ hrt_abstime hrt_absolute_time(void)
 
 __EXPORT hrt_abstime hrt_reset(void)
 {
+#ifndef __PX4_QURT
 	px4_timestart = 0;
+#endif
 	max_time = 0;
 	return _hrt_absolute_time_internal();
 }