diff --git a/include/zenoh-pico/system/platform/mbed.h b/include/zenoh-pico/system/platform/mbed.h index 87fdb4626..9d9d91aeb 100644 --- a/include/zenoh-pico/system/platform/mbed.h +++ b/include/zenoh-pico/system/platform/mbed.h @@ -33,7 +33,7 @@ typedef void *_z_mutex_t; // Workaround as MBED is a C++ library typedef void *_z_condvar_t; // Workaround as MBED is a C++ library #endif // Z_FEATURE_MULTI_THREAD == 1 -typedef void *z_clock_t; // Not defined +typedef struct timespec z_clock_t; typedef struct timeval z_time_t; typedef struct BufferedSerial BufferedSerial; // Forward declaration to be used in _z_sys_net_socket_t diff --git a/src/system/mbed/system.cpp b/src/system/mbed/system.cpp index e127d3518..9b8378ea0 100644 --- a/src/system/mbed/system.cpp +++ b/src/system/mbed/system.cpp @@ -136,23 +136,35 @@ z_result_t z_sleep_s(size_t time) { /*------------------ Instant ------------------*/ z_clock_t z_clock_now(void) { - // Not supported by default - return NULL; + auto now = Kernel::Clock::now(); + auto duration = now.time_since_epoch(); + auto secs = std::chrono::duration_cast(duration); + auto nanos = std::chrono::duration_cast(duration - secs); + + z_clock_t ts; + ts.tv_sec = secs.count(); + ts.tv_nsec = nanos.count(); + return ts; } unsigned long z_clock_elapsed_us(z_clock_t *instant) { - // Not supported by default - return -1; + z_clock_t now = z_clock_now(); + unsigned long elapsed = + (unsigned long)(1000000 * (now.tv_sec - instant->tv_sec) + (now.tv_nsec - instant->tv_nsec) / 1000); + return elapsed; } unsigned long z_clock_elapsed_ms(z_clock_t *instant) { - // Not supported by default - return -1; + z_clock_t now = z_clock_now(); + unsigned long elapsed = + (unsigned long)(1000 * (now.tv_sec - instant->tv_sec) + (now.tv_nsec - instant->tv_nsec) / 1000000); + return elapsed; } unsigned long z_clock_elapsed_s(z_clock_t *instant) { - // Not supported by default - return -1; + z_clock_t now = z_clock_now(); + unsigned long elapsed = (unsigned long)(now.tv_sec - instant->tv_sec); + return elapsed; } /*------------------ Time ------------------*/