Support MAVLink console

Implement receiving and sending SERIAL_CONTROL message
Use global defined print function instead of Serial.printf

docs/build.md

@@ -191,6 +191,12 @@ You can adjust some of the drone's parameters (include PID coefficients) in QGro
 
 <img src="img/parameters.png" width="400">
 
+#### CLI access
+
+In addition to accessing the drone's command line interface (CLI) using the serial port, you can also access it with QGroundControl using Wi-Fi connection. To do that, go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*.
+
+<img src="img/cli.png" width="400">
+
 > [!NOTE]
 > If something goes wrong, go to the [Troubleshooting](troubleshooting.md) article.
 

docs/troubleshooting.md

@@ -14,7 +14,7 @@ Do the following:
 * **Check the battery voltage**. Use a multimeter to measure the battery voltage. It should be in range of 3.7-4.2 V.
 * **Check if there are some startup errors**. Connect the ESP32 to the computer and check the Serial Monitor output. Use the Reset button to make sure you see the whole ESP32 output.
 * **Make sure correct IMU model is chosen**. If using ICM-20948 board, change `MPU9250` to `ICM20948` everywhere in the `imu.ino` file.
-* **Check if the CLI is working**. Perform `help` command in Serial Monitor. You should see the list of available commands.
+* **Check if the CLI is working**. Perform `help` command in Serial Monitor. You should see the list of available commands. You can also access the CLI using QGroundControl (*Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*).
 * **Configure QGroundControl correctly before connecting to the drone** if you use it to control the drone. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
 * **Make sure you're not moving the drone several seconds after the power on**. The drone calibrates its gyroscope on the start so it should stay still for a while.
 * **Check the IMU is working**. Perform `imu` command and check its output:

flix/cli.ino

@@ -5,6 +5,7 @@
 
 #include "pid.h"
 #include "vector.h"
+#include "util.h"
 
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
 extern float loopRate, dt;
@@ -39,52 +40,69 @@ const char* motd =
 "reset - reset drone's state\n"
 "reboot - reboot the drone\n";
 
-void doCommand(String str) {
+void print(const char* format, ...) {
+	char buf[1000];
+	va_list args;
+	va_start(args, format);
+	vsnprintf(buf, sizeof(buf), format, args);
+	va_end(args);
+	Serial.print(buf);
+#if WIFI_ENABLED
+	mavlinkPrint(buf);
+#endif
+}
+
+void doCommand(String str, bool echo = false) {
 	// parse command
 	String command, arg0, arg1;
 	splitString(str, command, arg0, arg1);
 
+	// echo command
+	if (echo && !command.isEmpty()) {
+		print("> %s\n", str.c_str());
+	}
+
 	// execute command
 	if (command == "help" || command == "motd") {
-		Serial.println(motd);
+		print("%s\n", motd);
 	} else if (command == "p" && arg0 == "") {
 		printParameters();
 	} else if (command == "p" && arg0 != "" && arg1 == "") {
-		Serial.printf("%s = %g\n", arg0.c_str(), getParameter(arg0.c_str()));
+		print("%s = %g\n", arg0.c_str(), getParameter(arg0.c_str()));
 	} else if (command == "p") {
 		bool success = setParameter(arg0.c_str(), arg1.toFloat());
 		if (success) {
-			Serial.printf("%s = %g\n", arg0.c_str(), arg1.toFloat());
+			print("%s = %g\n", arg0.c_str(), arg1.toFloat());
 		} else {
-			Serial.printf("Parameter not found: %s\n", arg0.c_str());
+			print("Parameter not found: %s\n", arg0.c_str());
 		}
 	} else if (command == "preset") {
 		resetParameters();
 	} else if (command == "time") {
-		Serial.printf("Time: %f\n", t);
+		print("Time: %f\n", t);
-		Serial.printf("Loop rate: %f\n", loopRate);
+		print("Loop rate: %f\n", loopRate);
-		Serial.printf("dt: %f\n", dt);
+		print("dt: %f\n", dt);
 	} else if (command == "ps") {
 		Vector a = attitude.toEulerZYX();
-		Serial.printf("roll: %f pitch: %f yaw: %f\n", degrees(a.x), degrees(a.y), degrees(a.z));
+		print("roll: %f pitch: %f yaw: %f\n", degrees(a.x), degrees(a.y), degrees(a.z));
 	} else if (command == "psq") {
-		Serial.printf("qx: %f qy: %f qz: %f qw: %f\n", attitude.x, attitude.y, attitude.z, attitude.w);
+		print("qx: %f qy: %f qz: %f qw: %f\n", attitude.x, attitude.y, attitude.z, attitude.w);
 	} else if (command == "imu") {
 		printIMUInfo();
-		Serial.printf("gyro: %f %f %f\n", rates.x, rates.y, rates.z);
+		print("gyro: %f %f %f\n", rates.x, rates.y, rates.z);
-		Serial.printf("acc: %f %f %f\n", acc.x, acc.y, acc.z);
+		print("acc: %f %f %f\n", acc.x, acc.y, acc.z);
 		printIMUCal();
-		Serial.printf("rate: %f\n", loopRate);
+		print("rate: %f\n", loopRate);
 	} else if (command == "rc") {
-		Serial.printf("Raw: throttle %d yaw %d pitch %d roll %d armed %d mode %d\n",
+		print("Raw: throttle %d yaw %d pitch %d roll %d armed %d mode %d\n",
 			channels[throttleChannel], channels[yawChannel], channels[pitchChannel],
 			channels[rollChannel], channels[armedChannel], channels[modeChannel]);
-		Serial.printf("Control: throttle %g yaw %g pitch %g roll %g armed %g mode %g\n",
+		print("Control: throttle %g yaw %g pitch %g roll %g armed %g mode %g\n",
 			controls[throttleChannel], controls[yawChannel], controls[pitchChannel],
 			controls[rollChannel], controls[armedChannel], controls[modeChannel]);
-		Serial.printf("Mode: %s\n", getModeName());
+		print("Mode: %s\n", getModeName());
 	} else if (command == "mot") {
-		Serial.printf("Motors: front-right %g front-left %g rear-right %g rear-left %g\n",
+		print("Motors: front-right %g front-left %g rear-right %g rear-left %g\n",
 			motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]);
 	} else if (command == "log") {
 		dumpLog();
@@ -109,7 +127,7 @@ void doCommand(String str) {
 	} else if (command == "") {
 		// do nothing
 	} else {
-		Serial.println("Invalid command: " + command);
+		print("Invalid command: %s\n", command.c_str());
 	}
 }
 
@@ -118,7 +136,7 @@ void handleInput() {
 	static String input;
 
 	if (showMotd) {
-		Serial.println(motd);
+		print("%s\n", motd);
 		showMotd = false;
 	}
 

flix/flix.ino

@@ -22,7 +22,7 @@ float motors[4]; // normalized motors thrust in range [-1..1]
 
 void setup() {
 	Serial.begin(SERIAL_BAUDRATE);
-	Serial.println("Initializing flix");
+	print("Initializing flix");
 	disableBrownOut();
 	setupParameters();
 	setupLED();
@@ -34,7 +34,7 @@ void setup() {
 	setupIMU();
 	setupRC();
 	setLED(false);
-	Serial.println("Initializing complete");
+	print("Initializing complete");
 }
 
 void loop() {

flix/imu.ino

@@ -14,7 +14,7 @@ Vector gyroBias;
 Vector accScale(1, 1, 1);
 
 void setupIMU() {
-	Serial.println("Setup IMU");
+	print("Setup IMU\n");
 	IMU.begin();
 	configureIMU();
 	delay(500); // wait a bit before calibrating
@@ -49,7 +49,7 @@ void rotateIMU(Vector& data) {
 
 void calibrateGyro() {
 	const int samples = 1000;
-	Serial.println("Calibrating gyro, stand still");
+	print("Calibrating gyro, stand still\n");
 	IMU.setGyroRange(IMU.GYRO_RANGE_250DPS); // the most sensitive mode
 
 	gyroBias = Vector(0, 0, 0);
@@ -65,7 +65,7 @@ void calibrateGyro() {
 }
 
 void calibrateAccel() {
-	Serial.println("Calibrating accelerometer");
+	print("Calibrating accelerometer\n");
 	IMU.setAccelRange(IMU.ACCEL_RANGE_2G); // the most sensitive mode
 
 	Serial.setTimeout(60000);
@@ -108,22 +108,22 @@ void calibrateAccelOnce() {
 	if (acc.x < accMin.x) accMin.x = acc.x;
 	if (acc.y < accMin.y) accMin.y = acc.y;
 	if (acc.z < accMin.z) accMin.z = acc.z;
-	Serial.printf("acc %f %f %f\n", acc.x, acc.y, acc.z);
+	print("acc %f %f %f\n", acc.x, acc.y, acc.z);
-	Serial.printf("max %f %f %f\n", accMax.x, accMax.y, accMax.z);
+	print("max %f %f %f\n", accMax.x, accMax.y, accMax.z);
-	Serial.printf("min %f %f %f\n", accMin.x, accMin.y, accMin.z);
+	print("min %f %f %f\n", accMin.x, accMin.y, accMin.z);
 	// Compute scale and bias
 	accScale = (accMax - accMin) / 2 / ONE_G;
 	accBias = (accMax + accMin) / 2;
 }
 
 void printIMUCal() {
-	Serial.printf("gyro bias: %f %f %f\n", gyroBias.x, gyroBias.y, gyroBias.z);
+	print("gyro bias: %f %f %f\n", gyroBias.x, gyroBias.y, gyroBias.z);
-	Serial.printf("accel bias: %f %f %f\n", accBias.x, accBias.y, accBias.z);
+	print("accel bias: %f %f %f\n", accBias.x, accBias.y, accBias.z);
-	Serial.printf("accel scale: %f %f %f\n", accScale.x, accScale.y, accScale.z);
+	print("accel scale: %f %f %f\n", accScale.x, accScale.y, accScale.z);
 }
 
 void printIMUInfo() {
-	IMU.status() ? Serial.printf("status: ERROR %d\n", IMU.status()) : Serial.println("status: OK");
+	IMU.status() ? print("status: ERROR %d\n", IMU.status()) : print("status: OK\n");
-	Serial.printf("model: %s\n", IMU.getModel());
+	print("model: %s\n", IMU.getModel());
-	Serial.printf("who am I: 0x%02X\n", IMU.whoAmI());
+	print("who am I: 0x%02X\n", IMU.whoAmI());
 }

flix/log.ino

@@ -67,13 +67,13 @@ void logData() {
 void dumpLog() {
 	// Print header
 	for (int i = 0; i < logColumns; i++) {
-		Serial.printf("%s%s", logEntries[i].name, i < logColumns - 1 ? "," : "\n");
+		print("%s%s", logEntries[i].name, i < logColumns - 1 ? "," : "\n");
 	}
 	// Print data
 	for (int i = 0; i < LOG_SIZE; i++) {
 		if (logBuffer[i][0] == 0) continue; // skip empty records
 		for (int j = 0; j < logColumns; j++) {
-			Serial.printf("%g%s", logBuffer[i][j], j < logColumns - 1 ? "," : "\n");
+			print("%g%s", logBuffer[i][j], j < logColumns - 1 ? "," : "\n");
 		}
 	}
 }

flix/mavlink.ino

@@ -147,6 +147,14 @@ void handleMavlink(const void *_msg) {
 		sendMessage(&msg);
 	}
 
+	if (msg->msgid == MAVLINK_MSG_ID_SERIAL_CONTROL) {
+		mavlink_serial_control_t serialControl;
+		mavlink_msg_serial_control_decode(msg, &serialControl);
+		char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1];
+		strlcpy(data, (const char *)serialControl.data, serialControl.count); // data might be not null-terminated
+		doCommand(data, true);
+	}
+
 	// Handle commands
 	if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
 		mavlink_command_long_t commandLong;
@@ -167,6 +175,19 @@ void handleMavlink(const void *_msg) {
 	}
 }
 
+// Send shell output to GCS
+void mavlinkPrint(const char* str) {
+	// Send data in chunks
+	for (int i = 0; i < strlen(str); i += MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN) {
+		char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1];
+		strlcpy(data, str + i, sizeof(data));
+		mavlink_message_t msg;
+		mavlink_msg_serial_control_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
+			SERIAL_CONTROL_DEV_SHELL, 0, 0, 0, strlen(data), (uint8_t *)data, 0, 0);
+		sendMessage(&msg);
+	}
+}
+
 // Convert Forward-Left-Up to Forward-Right-Down quaternion
 inline Quaternion FLU2FRD(const Quaternion &q) {
 	return Quaternion(q.w, q.x, -q.y, -q.z);

flix/motors.ino

@@ -24,7 +24,7 @@ const int MOTOR_FRONT_RIGHT = 2;
 const int MOTOR_FRONT_LEFT = 3;
 
 void setupMotors() {
-	Serial.println("Setup Motors");
+	print("Setup Motors\n");
 
 	// configure pins
 	ledcAttach(MOTOR_0_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
@@ -33,7 +33,7 @@ void setupMotors() {
 	ledcAttach(MOTOR_3_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
 
 	sendMotors();
-	Serial.println("Motors initialized");
+	print("Motors initialized\n");
 }
 
 int getDutyCycle(float value) {
@@ -56,12 +56,12 @@ bool motorsActive() {
 }
 
 void testMotor(uint8_t n) {
-	Serial.printf("Testing motor %d\n", n);
+	print("Testing motor %d\n", 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);
 	motors[n] = 0;
 	sendMotors();
-	Serial.printf("Done\n");
+	print("Done\n");
 }

flix/parameters.ino

@@ -135,7 +135,7 @@ void syncParameters() {
 
 void printParameters() {
 	for (auto &parameter : parameters) {
-		Serial.printf("%s = %g\n", parameter.name, *parameter.variable);
+		print("%s = %g\n", parameter.name, *parameter.variable);
 	}
 }
 

flix/rc.ino

@@ -21,7 +21,7 @@ float channelNeutral[16] = {NAN}; // first element NAN means not calibrated
 float channelMax[16];
 
 void setupRC() {
-	Serial.println("Setup RC");
+	print("Setup RC\n");
 	RC.begin();
 }
 
@@ -44,15 +44,15 @@ void normalizeRC() {
 }
 
 void calibrateRC() {
-	Serial.println("Calibrate RC: move all sticks to maximum positions in 4 seconds");
+	print("Calibrate RC: move all sticks to maximum positions in 4 seconds\n");
-	Serial.println("··o     ··o\n···     ···\n···     ···");
+	print("··o     ··o\n···     ···\n···     ···\n");
 	delay(4000);
 	while (!readRC());
 	for (int i = 0; i < 16; i++) {
 		channelMax[i] = channels[i];
 	}
-	Serial.println("Calibrate RC: move all sticks to neutral positions in 4 seconds");
+	print("Calibrate RC: move all sticks to neutral positions in 4 seconds\n");
-	Serial.println("···     ···\n···     ·o·\n·o·     ···");
+	print("···     ···\n···     ·o·\n·o·     ···\n");
 	delay(4000);
 	while (!readRC());
 	for (int i = 0; i < 16; i++) {
@@ -62,8 +62,8 @@ void calibrateRC() {
 }
 
 void printRCCal() {
-	for (int i = 0; i < sizeof(channelNeutral) / sizeof(channelNeutral[0]); i++) Serial.printf("%g ", channelNeutral[i]);
+	for (int i = 0; i < sizeof(channelNeutral) / sizeof(channelNeutral[0]); i++) print("%g ", channelNeutral[i]);
-	Serial.printf("\n");
+	print("\n");
-	for (int i = 0; i < sizeof(channelMax) / sizeof(channelMax[0]); i++) Serial.printf("%g ", channelMax[i]);
+	for (int i = 0; i < sizeof(channelMax) / sizeof(channelMax[0]); i++) print("%g ", channelMax[i]);
-	Serial.printf("\n");
+	print("\n");
 }

flix/wifi.ino

@@ -17,12 +17,13 @@
 WiFiUDP udp;
 
 void setupWiFi() {
-	Serial.println("Setup Wi-Fi");
+	print("Setup Wi-Fi\n");
 	WiFi.softAP(WIFI_SSID, WIFI_PASSWORD);
 	udp.begin(WIFI_UDP_PORT);
 }
 
 void sendWiFi(const uint8_t *buf, int len) {
+	if (WiFi.softAPIP() == IPAddress(0, 0, 0, 0) && WiFi.status() != WL_CONNECTED) return;
 	udp.beginPacket(WIFI_UDP_IP, WIFI_UDP_PORT);
 	udp.write(buf, len);
 	udp.endPacket();

gazebo/flix.h

@@ -34,7 +34,8 @@ void controlTorque();
 void showTable();
 void sendMotors();
 bool motorsActive();
-void doCommand(String str);
+void print(const char* format, ...);
+void doCommand(String str, bool echo);
 void cliTestMotor(uint8_t n);
 void normalizeRC();
 void printRCCal();
@@ -43,6 +44,7 @@ void sendMavlink();
 void sendMessage(const void *msg);
 void receiveMavlink();
 void handleMavlink(const void *_msg);
+void mavlinkPrint(const char* str);
 void failsafe();
 void armingFailsafe();
 void rcLossFailsafe();
@@ -51,8 +53,8 @@ inline Quaternion FLU2FRD(const Quaternion &q);
 
 // mocks
 void setLED(bool on) {};
-void calibrateGyro() { printf("Skip gyro calibrating\n"); };
+void calibrateGyro() { print("Skip gyro calibrating\n"); };
-void calibrateAccel() { printf("Skip accel calibrating\n"); };
+void calibrateAccel() { print("Skip accel calibrating\n"); };
-void printIMUCal() { printf("cal: N/A\n"); };
+void printIMUCal() { print("cal: N/A\n"); };
 void printIMUInfo() {};
 Vector accBias, gyroBias, accScale(1, 1, 1);