Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Support MAVLink console Implement receiving and sending SERIAL_CONTROL message Use global defined print function instead of Serial.printf
Oleg Kalachevd60628e14d34
diff --git a/docs/build.md b/docs/build.mdindex 5893a85..4d49aaa 100644--- a/docs/build.md+++ b/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.diff --git a/docs/img/cli.png b/docs/img/cli.pngnew file mode 100644index 0000000..04190d1Binary files /dev/null and b/docs/img/cli.png differdiff --git a/docs/troubleshooting.md b/docs/troubleshooting.mdindex fb1b466..c95dca9 100644--- a/docs/troubleshooting.md+++ b/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:diff --git a/flix/cli.ino b/flix/cli.inoindex 79d002b..b70ffe8 100644--- a/flix/cli.ino+++ b/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 commandString command, arg0, arg1;splitString(str, command, arg0, arg1);+ // echo command+ if (echo && !command.isEmpty()) {+ print("> %s\n", str.c_str());+ }+// execute commandif (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);- Serial.printf("Loop rate: %f\n", loopRate);- Serial.printf("dt: %f\n", dt);+ print("Time: %f\n", t);+ print("Loop rate: %f\n", loopRate);+ 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);- Serial.printf("acc: %f %f %f\n", acc.x, acc.y, acc.z);+ print("gyro: %f %f %f\n", rates.x, rates.y, rates.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;}diff --git a/flix/flix.ino b/flix/flix.inoindex b33f1db..2d89177 100644--- a/flix/flix.ino+++ b/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() {diff --git a/flix/imu.ino b/flix/imu.inoindex 2bdcbcc..d75d86a 100644--- a/flix/imu.ino+++ b/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 modegyroBias = 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 modeSerial.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);- Serial.printf("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("acc %f %f %f\n", acc.x, acc.y, acc.z);+ print("max %f %f %f\n", accMax.x, accMax.y, accMax.z);+ print("min %f %f %f\n", accMin.x, accMin.y, accMin.z);// Compute scale and biasaccScale = (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);- Serial.printf("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("gyro bias: %f %f %f\n", gyroBias.x, gyroBias.y, gyroBias.z);+ print("accel bias: %f %f %f\n", accBias.x, accBias.y, accBias.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");- Serial.printf("model: %s\n", IMU.getModel());- Serial.printf("who am I: 0x%02X\n", IMU.whoAmI());+ IMU.status() ? print("status: ERROR %d\n", IMU.status()) : print("status: OK\n");+ print("model: %s\n", IMU.getModel());+ print("who am I: 0x%02X\n", IMU.whoAmI());}diff --git a/flix/log.ino b/flix/log.inoindex d6be3e9..0257da4 100644--- a/flix/log.ino+++ b/flix/log.ino@@ -67,13 +67,13 @@ void logData() {void dumpLog() {// Print headerfor (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 datafor (int i = 0; i < LOG_SIZE; i++) {if (logBuffer[i][0] == 0) continue; // skip empty recordsfor (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");}}}diff --git a/flix/mavlink.ino b/flix/mavlink.inoindex 94e8490..b358316 100644--- a/flix/mavlink.ino+++ b/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 commandsif (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 quaternioninline Quaternion FLU2FRD(const Quaternion &q) {return Quaternion(q.w, q.x, -q.y, -q.z);diff --git a/flix/motors.ino b/flix/motors.inoindex 5c28aea..bf792d9 100644--- a/flix/motors.ino+++ b/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 pinsledcAttach(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/5306sendMotors();delay(3000);motors[n] = 0;sendMotors();- Serial.printf("Done\n");+ print("Done\n");}diff --git a/flix/parameters.ino b/flix/parameters.inoindex acfe7e5..4447219 100644--- a/flix/parameters.ino+++ b/flix/parameters.ino@@ -135,7 +135,7 @@ void syncParameters() {void printParameters() {for (auto ¶meter : parameters) {- Serial.printf("%s = %g\n", parameter.name, *parameter.variable);+ print("%s = %g\n", parameter.name, *parameter.variable);}}diff --git a/flix/rc.ino b/flix/rc.inoindex 23112b4..1227229 100644--- a/flix/rc.ino+++ b/flix/rc.ino@@ -21,7 +21,7 @@ float channelNeutral[16] = {NAN}; // first element NAN means not calibratedfloat 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");- Serial.println("··o ··o\n··· ···\n··· ···");+ print("Calibrate RC: move all sticks to maximum positions in 4 seconds\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");- Serial.println("··· ···\n··· ·o·\n·o· ···");+ print("Calibrate RC: move all sticks to neutral positions in 4 seconds\n");+ 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]);- Serial.printf("\n");- for (int i = 0; i < sizeof(channelMax) / sizeof(channelMax[0]); i++) Serial.printf("%g ", channelMax[i]);- Serial.printf("\n");+ for (int i = 0; i < sizeof(channelNeutral) / sizeof(channelNeutral[0]); i++) print("%g ", channelNeutral[i]);+ print("\n");+ for (int i = 0; i < sizeof(channelMax) / sizeof(channelMax[0]); i++) print("%g ", channelMax[i]);+ print("\n");}diff --git a/flix/wifi.ino b/flix/wifi.inoindex 5343433..0ebd19d 100644--- a/flix/wifi.ino+++ b/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();diff --git a/gazebo/flix.h b/gazebo/flix.hindex 1437623..0d4c24d 100644--- a/gazebo/flix.h+++ b/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);// mocksvoid setLED(bool on) {};-void calibrateGyro() { printf("Skip gyro calibrating\n"); };-void calibrateAccel() { printf("Skip accel calibrating\n"); };-void printIMUCal() { printf("cal: N/A\n"); };+void calibrateGyro() { print("Skip gyro calibrating\n"); };+void calibrateAccel() { print("Skip accel calibrating\n"); };+void printIMUCal() { print("cal: N/A\n"); };void printIMUInfo() {};Vector accBias, gyroBias, accScale(1, 1, 1);