diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 52f595cd43..7b6b4594f0 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -40,6 +40,8 @@ #include #include +extern "C" __EXPORT hrt_abstime hrt_reset(void); + #define SEND_INTERVAL 20 #define UDP_PORT 14560 #define PIXHAWK_DEVICE "/dev/ttyACM0" @@ -406,6 +408,8 @@ void Simulator::pollForMAVLinkMessages(bool publish) } PX4_WARN("Found initial message, pret = %d",pret); _initialized = true; + // reset system time + (void)hrt_reset(); if (fds[0].revents & POLLIN) { len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 0a45644481..2fbe8cd70a 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -66,6 +66,8 @@ static hrt_abstime px4_timestart = 0; static void hrt_call_invoke(void); +__EXPORT hrt_abstime hrt_reset(void); + static void hrt_lock(void) { //printf("hrt_lock\n"); @@ -128,6 +130,12 @@ hrt_abstime hrt_absolute_time(void) return ts_to_abstime(&ts) - px4_timestart; } +__EXPORT hrt_abstime hrt_reset(void) +{ + px4_timestart = 0; + return hrt_absolute_time(); +} + /* * Convert a timespec to absolute time. */