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

@@ -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;

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();

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");

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];

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();