From 85afe405cbf681e7eb31c1913425095ce936ec25 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 12 Jul 2025 09:29:47 +0300 Subject: [PATCH] Improve pause function work Fix disconnecting from qgc while pausing in the simulation. Consider total delay time in micros() in simulation to increase t while delaying. Simplify and get rid of ARDUINO macro check. --- flix/cli.ino | 6 +----- gazebo/Arduino.h | 5 ++++- 2 files changed, 5 insertions(+), 6 deletions(-) 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 }