From 3fdebf39d80d1e5325c8e49eee15dd355ab523ff Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Fri, 28 Feb 2025 19:25:41 +0300
Subject: [PATCH] Fix mavlink disconnection in pauses in cli commands Implement
 pause function that proceeds processing mavlink. Use temporal workaround for
 simulation, as micros function gives the same result on the same simulation
 step.

---
 flix/cli.ino    | 16 ++++++++++++++++
 flix/imu.ino    | 19 ++++++++++++-------
 flix/motors.ino |  2 +-
 flix/rc.ino     |  8 ++++----
 gazebo/flix.h   |  1 +
 5 files changed, 34 insertions(+), 12 deletions(-)

diff --git a/flix/cli.ino b/flix/cli.ino
index b70ffe8..0143921 100644
--- a/flix/cli.ino
+++ b/flix/cli.ino
@@ -52,6 +52,22 @@ void print(const char* format, ...) {
 #endif
 }
 
+void pause(float duration) {
+#if ARDUINO
+	double start = t;
+	while (t - start < duration) {
+		step();
+		handleInput();
+#if WIFI_ENABLED
+		processMavlink();
+#endif
+	}
+#else
+	// Code above won't work in the simulation
+	delay(duration * 1000);
+#endif
+}
+
 void doCommand(String str, bool echo = false) {
 	// parse command
 	String command, arg0, arg1;
diff --git a/flix/imu.ino b/flix/imu.ino
index 10e12c0..c738229 100644
--- a/flix/imu.ino
+++ b/flix/imu.ino
@@ -68,18 +68,23 @@ void calibrateAccel() {
 	print("Calibrating accelerometer\n");
 	IMU.setAccelRange(IMU.ACCEL_RANGE_2G); // the most sensitive mode
 
-	Serial.setTimeout(5000);
-	print("Place level [enter | 5 sec] \n"); Serial.readStringUntil('\n');
+	print("Place level [8 sec]\n");
+	pause(8);
 	calibrateAccelOnce();
-	print("Place nose up [enter | 5 sec] \n"); Serial.readStringUntil('\n');
+	print("Place nose up [8 sec]\n");
+	pause(8);
 	calibrateAccelOnce();
-	print("Place nose down [enter | 5 sec] \n"); Serial.readStringUntil('\n');
+	print("Place nose down [8 sec]\n");
+	pause(8);
 	calibrateAccelOnce();
-	print("Place on right side [enter | 5 sec] \n"); Serial.readStringUntil('\n');
+	print("Place on right side [8 sec]\n");
+	pause(8);
 	calibrateAccelOnce();
-	print("Place on left side [enter | 5 sec] \n"); Serial.readStringUntil('\n');
+	print("Place on left side [8 sec]\n");
+	pause(8);
 	calibrateAccelOnce();
-	print("Place upside down [enter | 5 sec] \n"); Serial.readStringUntil('\n');
+	print("Place upside down [8 sec]\n");
+	pause(8);
 	calibrateAccelOnce();
 
 	printIMUCal();
diff --git a/flix/motors.ino b/flix/motors.ino
index bf792d9..0386552 100644
--- a/flix/motors.ino
+++ b/flix/motors.ino
@@ -60,7 +60,7 @@ void testMotor(uint8_t n) {
 	motors[n] = 1;
 	delay(50); // ESP32 may need to wait until the end of the current cycle to change duty https://github.com/espressif/arduino-esp32/issues/5306
 	sendMotors();
-	delay(3000);
+	pause(3);
 	motors[n] = 0;
 	sendMotors();
 	print("Done\n");
diff --git a/flix/rc.ino b/flix/rc.ino
index 1227229..e78cb42 100644
--- a/flix/rc.ino
+++ b/flix/rc.ino
@@ -44,16 +44,16 @@ void normalizeRC() {
 }
 
 void calibrateRC() {
-	print("Calibrate RC: move all sticks to maximum positions in 4 seconds\n");
+	print("Calibrate RC: move all sticks to maximum positions [4 sec]\n");
 	print("··o     ··o\n···     ···\n···     ···\n");
-	delay(4000);
+	pause(4);
 	while (!readRC());
 	for (int i = 0; i < 16; i++) {
 		channelMax[i] = channels[i];
 	}
-	print("Calibrate RC: move all sticks to neutral positions in 4 seconds\n");
+	print("Calibrate RC: move all sticks to neutral positions [4 sec]\n");
 	print("···     ···\n···     ·o·\n·o·     ···\n");
-	delay(4000);
+	pause(4);
 	while (!readRC());
 	for (int i = 0; i < 16; i++) {
 		channelNeutral[i] = channels[i];
diff --git a/gazebo/flix.h b/gazebo/flix.h
index d5b37eb..bcda0c3 100644
--- a/gazebo/flix.h
+++ b/gazebo/flix.h
@@ -37,6 +37,7 @@ void sendMotors();
 bool motorsActive();
 void testMotor(uint8_t n);
 void print(const char* format, ...);
+void pause(float duration);
 void doCommand(String str, bool echo);
 void handleInput();
 void calibrateRC();