diff --git a/flix/cli.ino b/flix/cli.ino index a21a746..9e21e8b 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -53,7 +53,6 @@ void print(const char* format, ...) { } void pause(float duration) { -#if ARDUINO double start = t; while (t - start < duration) { step(); @@ -61,11 +60,8 @@ void pause(float duration) { #if WIFI_ENABLED processMavlink(); #endif + delay(50); } -#else - // Code above won't work in the simulation - delay(duration * 1000); -#endif } void doCommand(String str, bool echo = false) { diff --git a/gazebo/Arduino.h b/gazebo/Arduino.h index 22c771f..1ca8089 100644 --- a/gazebo/Arduino.h +++ b/gazebo/Arduino.h @@ -156,8 +156,11 @@ public: void restart() { Serial.println("Ignore reboot in simulation"); } } ESP; +unsigned long __delayTime = 0; + void delay(uint32_t ms) { std::this_thread::sleep_for(std::chrono::milliseconds(ms)); + __delayTime += ms * 1000; } bool ledcAttach(uint8_t pin, uint32_t freq, uint8_t resolution) { return true; } @@ -167,5 +170,5 @@ unsigned long __micros; unsigned long __resetTime = 0; unsigned long micros() { - return __micros + __resetTime; // keep the time monotonic + return __micros + __resetTime + __delayTime; // keep the time monotonic }