From 2bce8f2803409e543319cc8ea3ed87b9037b8c8c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Apr 2016 15:31:30 +0200 Subject: [PATCH] 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 | 9 ++ src/firmware/qurt/px4muorb.idl | 22 ++++- src/modules/muorb/adsp/px4muorb.cpp | 15 ++- src/modules/muorb/adsp/px4muorb.hpp | 4 + .../muorb/krait/px4muorb_KraitRpcWrapper.cpp | 92 ++++++++++++++++++- src/platforms/posix/px4_layer/drv_hrt.c | 28 ++++++ 6 files changed, 164 insertions(+), 6 deletions(-) diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index d063561eb7..556257c169 100644 --- a/src/drivers/drv_hrt.h +++ b/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. * diff --git a/src/firmware/qurt/px4muorb.idl b/src/firmware/qurt/px4muorb.idl index 0777588c8c..49e1305ffd 100644 --- a/src/firmware/qurt/px4muorb.idl +++ b/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 data, rout long bytes_returned, rout long topic_count ); }; diff --git a/src/modules/muorb/adsp/px4muorb.cpp b/src/modules/muorb/adsp/px4muorb.cpp index df951eb967..a63145a056 100644 --- a/src/modules/muorb/adsp/px4muorb.cpp +++ b/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; diff --git a/src/modules/muorb/adsp/px4muorb.hpp b/src/modules/muorb/adsp/px4muorb.hpp index 1ec6b45566..56154becfa 100644 --- a/src/modules/muorb/adsp/px4muorb.hpp +++ b/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; diff --git a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp index d862384398..d633647f47 100644 --- a/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp +++ b/src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp @@ -30,6 +30,10 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ + +#include +#include +#include #include "px4muorb_KraitRpcWrapper.hpp" #include #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; } diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index ef01b1ee01..7b8d32e918 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/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,7 +174,16 @@ 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(); }