diff --git a/docs/build.md b/docs/build.md
index 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
+#### 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*.
+
+
+
> [!NOTE]
> If something goes wrong, go to the [Troubleshooting](troubleshooting.md) article.
diff --git a/docs/img/cli.png b/docs/img/cli.png
new file mode 100644
index 0000000..04190d1
Binary files /dev/null and b/docs/img/cli.png differ
diff --git a/docs/troubleshooting.md b/docs/troubleshooting.md
index 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.ino
index 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 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);
- 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.ino
index 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.ino
index 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 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);
- 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 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);
- 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.ino
index d6be3e9..0257da4 100644
--- a/flix/log.ino
+++ b/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");
}
}
}
diff --git a/flix/mavlink.ino b/flix/mavlink.ino
index 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 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);
diff --git a/flix/motors.ino b/flix/motors.ino
index 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 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");
}
diff --git a/flix/parameters.ino b/flix/parameters.ino
index 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.ino
index 23112b4..1227229 100644
--- a/flix/rc.ino
+++ b/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");
- 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.ino
index 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.h
index 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);
// mocks
void 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);