diff --git a/gazebo/Arduino.h b/gazebo/Arduino.h index b902257..7ea6f90 100644 --- a/gazebo/Arduino.h +++ b/gazebo/Arduino.h @@ -134,7 +134,8 @@ void delay(uint32_t ms) { } unsigned long __micros; +unsigned long __resetTime = 0; unsigned long micros() { - return __micros; + return __micros + __resetTime; // keep the time monotonic } diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp index d789bf0..e67ea10 100644 --- a/gazebo/simulator.cpp +++ b/gazebo/simulator.cpp @@ -55,6 +55,7 @@ public: void OnReset() { attitude = Quaternion(); // reset estimated attitude + __resetTime += __micros; gzmsg << "Flix plugin reset" << endl; }