From e7100f03ce8a8cc1ac3bfd35d7d213686108570e Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 18 Aug 2026 11:00:59 +0300 Subject: [PATCH 1/3] Simplify and improve control code Add lower bound desaturation. Remove constrain calls as they are performed in motors subsystem. --- flix/control.ino | 41 +++++++++++++++++++++-------------------- gazebo/flix.h | 2 +- 2 files changed, 22 insertions(+), 21 deletions(-) diff --git a/flix/control.ino b/flix/control.ino index 1164a2d..1c09067 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -107,15 +107,12 @@ void controlTorque() { if (!torqueTarget.valid()) return; // skip torque control if (!armed) { - memset(motors, 0, sizeof(motors)); // stop motors if disarmed + for (float& m : motors) m = 0; // stop motors if disarmed return; } if (thrustTarget < 0.1) { - motors[0] = 0.1; // idle thrust - motors[1] = 0.1; - motors[2] = 0.1; - motors[3] = 0.1; + for (float& m : motors) m = 0.1; // idle thrust return; } @@ -124,23 +121,27 @@ void controlTorque() { motors[MOT_RL] = thrustTarget + torqueTarget.x + torqueTarget.y - torqueTarget.z; motors[MOT_RR] = thrustTarget - torqueTarget.x + torqueTarget.y + torqueTarget.z; - // Prioritize angle control over thrust control - desaturate(motors[MOT_FL], motors[MOT_FR], motors[MOT_RL], motors[MOT_RR]); - - motors[0] = constrain(motors[0], 0, 1); - motors[1] = constrain(motors[1], 0, 1); - motors[2] = constrain(motors[2], 0, 1); - motors[3] = constrain(motors[3], 0, 1); + desaturate(); // prioritize angle control over thrust control } -void desaturate(float& a, float& b, float& c, float& d) { - float maxThrust = max(max(a, b), max(c, d)); - if (maxThrust > 1) { - float diff = maxThrust - 1; - a -= diff; - b -= diff; - c -= diff; - d -= diff; +void desaturate() { + float max_ = -INFINITY, min_ = INFINITY; + float correction = 0; + + // Find maximum and minimum thrust + for (float m : motors) { + if (m > max_) max_ = m; + if (m < min_) min_ = m; + } + + if (max_ > 1) { // thrust is above maximum + correction = max_ - 1; + } else if (min_ < 0) { // thrust is below minimum + correction = min_; + } + + for (float& m : motors) { + m -= correction; } } diff --git a/gazebo/flix.h b/gazebo/flix.h index d9a33fc..cdfb624 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -36,7 +36,7 @@ void interpretControls(); void controlAttitude(); void controlRates(); void controlTorque(); -void desaturate(float& a, float& b, float& c, float& d); +void desaturate(); const char* getModeName(); void sendMotors(); int getDutyCycle(float value); From b6c0bebdeb5229c5ba4dd183eb8125609ae3a6b5 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 18 Aug 2026 15:07:16 +0300 Subject: [PATCH 2/3] Better names for control parameters (breaking change) Divide all parameters into attitude (ATT) and rates (RATE) control groups, which is much more obvious and clear. Improves alphabetic sort representation. Also makes more sense when filtering parameters using prefix in `p` command. Remove i and d parameters for attitude control, since they are unlikely to be useful in practice. --- docs/usage.md | 2 +- flix/parameters.ino | 48 +++++++++++++++++++++------------------------ 2 files changed, 23 insertions(+), 27 deletions(-) diff --git a/docs/usage.md b/docs/usage.md index b109276..e7fbc50 100644 --- a/docs/usage.md +++ b/docs/usage.md @@ -252,7 +252,7 @@ There are several ways to control the drone's flight: using **smartphone** (Wi-F 6. Use the virtual joystick to fly the drone! > [!TIP] -> Decrease `CTL_TILT_MAX` parameter when flying using the smartphone to make the controls less sensitive. +> Decrease `CTL_ATT_MAX` parameter when flying using the smartphone to make the controls less sensitive. ### Control with a remote control diff --git a/flix/parameters.ino b/flix/parameters.ino index 32c874d..03bad37 100644 --- a/flix/parameters.ino +++ b/flix/parameters.ino @@ -33,32 +33,28 @@ struct Parameter { Parameter parameters[] = { // control - {"CTL_R_RATE_P", &rollRatePID.p}, - {"CTL_R_RATE_I", &rollRatePID.i}, - {"CTL_R_RATE_D", &rollRatePID.d}, - {"CTL_R_RATE_WU", &rollRatePID.windup}, - {"CTL_R_RATE_D_A", &rollRatePID.lpf.alpha}, - {"CTL_P_RATE_P", &pitchRatePID.p}, - {"CTL_P_RATE_I", &pitchRatePID.i}, - {"CTL_P_RATE_D", &pitchRatePID.d}, - {"CTL_P_RATE_WU", &pitchRatePID.windup}, - {"CTL_P_RATE_D_A", &pitchRatePID.lpf.alpha}, - {"CTL_Y_RATE_P", &yawRatePID.p}, - {"CTL_Y_RATE_I", &yawRatePID.i}, - {"CTL_Y_RATE_D", &yawRatePID.d}, - {"CTL_Y_RATE_WU", &yawRatePID.windup}, - {"CTL_Y_RATE_D_A", &yawRatePID.lpf.alpha}, - {"CTL_R_P", &rollPID.p}, - {"CTL_R_I", &rollPID.i}, - {"CTL_R_D", &rollPID.d}, - {"CTL_P_P", &pitchPID.p}, - {"CTL_P_I", &pitchPID.i}, - {"CTL_P_D", &pitchPID.d}, - {"CTL_Y_P", &yawPID.p}, - {"CTL_P_RATE_MAX", &maxRate.y}, - {"CTL_R_RATE_MAX", &maxRate.x}, - {"CTL_Y_RATE_MAX", &maxRate.z}, - {"CTL_TILT_MAX", &tiltMax}, + {"CTL_RATE_R_P", &rollRatePID.p}, + {"CTL_RATE_R_I", &rollRatePID.i}, + {"CTL_RATE_R_D", &rollRatePID.d}, + {"CTL_RATE_R_WU", &rollRatePID.windup}, + {"CTL_RATE_R_D_A", &rollRatePID.lpf.alpha}, + {"CTL_RATE_P_P", &pitchRatePID.p}, + {"CTL_RATE_P_I", &pitchRatePID.i}, + {"CTL_RATE_P_D", &pitchRatePID.d}, + {"CTL_RATE_P_WU", &pitchRatePID.windup}, + {"CTL_RATE_P_D_A", &pitchRatePID.lpf.alpha}, + {"CTL_RATE_Y_P", &yawRatePID.p}, + {"CTL_RATE_Y_I", &yawRatePID.i}, + {"CTL_RATE_Y_D", &yawRatePID.d}, + {"CTL_RATE_Y_WU", &yawRatePID.windup}, + {"CTL_RATE_Y_D_A", &yawRatePID.lpf.alpha}, + {"CTL_RATE_P_MAX", &maxRate.y}, + {"CTL_RATE_R_MAX", &maxRate.x}, + {"CTL_RATE_Y_MAX", &maxRate.z}, + {"CTL_ATT_R_P", &rollPID.p}, + {"CTL_ATT_P_P", &pitchPID.p}, + {"CTL_ATT_Y_P", &yawPID.p}, + {"CTL_ATT_MAX", &tiltMax}, {"CTL_FLT_MODE_0", &flightModes[0]}, {"CTL_FLT_MODE_1", &flightModes[1]}, {"CTL_FLT_MODE_2", &flightModes[2]}, From 03084a672ac30d2e8bc2ca513b4025c2961c1ad4 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 19 Aug 2026 05:32:52 +0300 Subject: [PATCH 3/3] Refactor the console subsystem Rename cli.ino => console.ino. Rename handleInput => handleConsole. Separate console initialization function. Always use usb cdc by default on s3/c3 (no configuration needed). --- .github/workflows/build.yml | 8 ++++---- .vscode/c_cpp_properties.json | 6 +++--- docs/firmware.md | 4 ++-- docs/usage.md | 4 ++-- flix/{cli.ino => console.ino} | 17 +++++++++++++++-- flix/flix.ino | 4 ++-- gazebo/flix.h | 2 +- gazebo/simulator.cpp | 4 ++-- 8 files changed, 31 insertions(+), 18 deletions(-) rename flix/{cli.ino => console.ino} (95%) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 8024419..743662c 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -21,13 +21,13 @@ jobs: - name: Build firmware for ESP32-C3 run: make BOARD=esp32:esp32:esp32c3 - name: Build firmware for ESP32-S3 - run: make BOARD=esp32:esp32:esp32s3:CDCOnBoot=cdc + run: make BOARD=esp32:esp32:esp32s3 - name: Build firmware for ESP32-S3 with QSPI PSRAM - run: make BOARD=esp32:esp32:esp32s3:CDCOnBoot=cdc,PSRAM=enabled EXTRA=--output-dir=flix/build/esp32.esp32.esp32s3.qspi + run: make BOARD=esp32:esp32:esp32s3:PSRAM=enabled EXTRA=--output-dir=flix/build/esp32.esp32.esp32s3.qspi - name: Build firmware for ESP32-S3 with OPI PSRAM - run: make BOARD=esp32:esp32:esp32s3:CDCOnBoot=cdc,PSRAM=opi EXTRA=--output-dir=flix/build/esp32.esp32.esp32s3.opi + run: make BOARD=esp32:esp32:esp32s3:PSRAM=opi EXTRA=--output-dir=flix/build/esp32.esp32.esp32s3.opi - name: Build firmware for Flix2 - run: make BOARD=esp32:esp32:esp32s3:FlashSize=4M,CDCOnBoot=cdc,PSRAM=opi EXTRA='--build-property "compiler.cpp.extra_flags=-DFLIX2" --output-dir=flix/build/esp32.esp32.flix2' + run: make BOARD=esp32:esp32:esp32s3:PSRAM=opi EXTRA='--build-property "compiler.cpp.extra_flags=-DFLIX2" --output-dir=flix/build/esp32.esp32.flix2' - name: Upload binaries uses: actions/upload-artifact@v7 with: diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 3d7a7bb..51ca063 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -18,7 +18,7 @@ "${workspaceFolder}/.vscode/intellisense.h", "~/.arduino15/packages/esp32/hardware/esp32/3.3.10/cores/esp32/Arduino.h", "~/.arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32/pins_arduino.h", - "${workspaceFolder}/flix/cli.ino", + "${workspaceFolder}/flix/console.ino", "${workspaceFolder}/flix/control.ino", "${workspaceFolder}/flix/estimate.ino", "${workspaceFolder}/flix/flix.ino", @@ -66,7 +66,7 @@ "~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.10/cores/esp32/Arduino.h", "~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32/pins_arduino.h", "${workspaceFolder}/flix/flix.ino", - "${workspaceFolder}/flix/cli.ino", + "${workspaceFolder}/flix/console.ino", "${workspaceFolder}/flix/control.ino", "${workspaceFolder}/flix/estimate.ino", "${workspaceFolder}/flix/imu.ino", @@ -113,7 +113,7 @@ "${workspaceFolder}/.vscode/intellisense.h", "~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.10/cores/esp32/Arduino.h", "~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.10/variants/d1_mini32/pins_arduino.h", - "${workspaceFolder}/flix/cli.ino", + "${workspaceFolder}/flix/console.ino", "${workspaceFolder}/flix/control.ino", "${workspaceFolder}/flix/estimate.ino", "${workspaceFolder}/flix/flix.ino", diff --git a/docs/firmware.md b/docs/firmware.md index 8adb6cc..4b239c4 100644 --- a/docs/firmware.md +++ b/docs/firmware.md @@ -28,7 +28,7 @@ Firmware source files are located in `flix` directory. * [`control.ino`](../flix/control.ino) — control subsystem, three-dimensional two-level cascade PID controller. * [`motors.ino`](../flix/motors.ino) — PWM motor output control. * [`mavlink.ino`](../flix/mavlink.ino) — interaction with QGroundControl or [pyflix](../tools/pyflix) via MAVLink protocol. -* [`cli.ino`](../flix/cli.ino) — serial and MAVLink console. +* [`console.ino`](../flix/console.ino) — serial and MAVLink console. Utility files: @@ -60,7 +60,7 @@ To write into the console, `print()` function is used. This function sends data print("Test value: %.2f\n", testValue); ``` -In order to add a console command, modify the `doCommand()` function in `cli.ino` file. +In order to add a console command, modify the `doCommand()` function in `console.ino` file. > [!IMPORTANT] > Avoid using delays in in-flight commands, it will **crash** the drone! (The design is one-threaded.) diff --git a/docs/usage.md b/docs/usage.md index e7fbc50..fee4381 100644 --- a/docs/usage.md +++ b/docs/usage.md @@ -54,7 +54,7 @@ Beginners can [download the sources as a ZIP archive](https://github.com/okalach * `MAVLink`, version 2.0.25. 5. Open the `flix/flix.ino` sketch from downloaded firmware sources in Arduino IDE. 6. Connect your ESP32 board to the computer and choose correct board type in Arduino IDE (*WEMOS D1 MINI ESP32* for ESP32 Mini, *ESP32S3 Dev Module* for ESP32-S3 Super Mini) and the port. -7. Set *Tools* ⇒ *Core Debug Level* to *Error* to see the errors in the serial console. Set *Tools* ⇒ *USB CDC on Boot* to *Enabled* for ESP32-S3/ESP32-C3 boards. +7. Set *Tools* ⇒ *Core Debug Level* to *Error* to see the errors in the serial console. 8. [Build and upload](https://docs.arduino.cc/software/ide-v2/tutorials/getting-started/ide-v2-uploading-a-sketch) the firmware using Arduino IDE. #### Command line (Windows, Linux, macOS) @@ -89,7 +89,7 @@ Beginners can [download the sources as a ZIP archive](https://github.com/okalach For ESP32-S3/ESP32-C3 boards, set the appropriate [FQBN](https://docs.arduino.cc/arduino-cli/FAQ/#whats-the-fqbn-string) using `BOARD` parameter: ```bash - make BOARD=esp32:esp32:esp32s3:FlashSize=4M,CDCOnBoot=cdc upload + make BOARD=esp32:esp32:esp32s3 upload ``` See other available Make commands in [Makefile](../Makefile). diff --git a/flix/cli.ino b/flix/console.ino similarity index 95% rename from flix/cli.ino rename to flix/console.ino index 0ca5fc0..c69a950 100644 --- a/flix/cli.ino +++ b/flix/console.ino @@ -56,6 +56,19 @@ const char* motd = "reset - reset drone's state\n" "reboot - reboot the drone\n"; +// Always redirect Serial to USB on ESP32-S3/ESP32-C3 +#if SOC_USB_SERIAL_JTAG_SUPPORTED && ARDUINO_USB_MODE +#if !ARDUINO_USB_CDC_ON_BOOT +HWCDC HWCDCSerial; +#endif +#undef Serial +#define Serial HWCDCSerial +#endif + +void setupConsole() { + Serial.begin(115200); +} + void print(const char* format, ...) { char buf[3000]; va_list args; @@ -70,7 +83,7 @@ void pause(float duration) { float start = t; while (t - start < duration) { step(); - handleInput(); + handleConsole(); processMavlink(); delay(50); } @@ -199,7 +212,7 @@ void doCommand(String str, bool echo = false) { } } -void handleInput() { +void handleConsole() { static bool showMotd = true; static String input; diff --git a/flix/flix.ino b/flix/flix.ino index 615664b..cc8e291 100644 --- a/flix/flix.ino +++ b/flix/flix.ino @@ -16,7 +16,7 @@ extern bool landed; extern float motors[4]; void setup() { - Serial.begin(115200); + setupConsole(); print("Initializing Flix\n"); setupParameters(); setupPower(); @@ -37,7 +37,7 @@ void loop() { estimate(); control(); sendMotors(); - handleInput(); + handleConsole(); processMavlink(); readVoltage(); logData(); diff --git a/gazebo/flix.h b/gazebo/flix.h index cdfb624..351d371 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -45,7 +45,7 @@ void testMotor(int, float); void print(const char* format, ...); void pause(float duration); void doCommand(String str, bool echo); -void handleInput(); +void handleConsole(); void setupRC(); void normalizeRC(); void calibrateRC(); diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp index 65a4138..3ffd6e3 100644 --- a/gazebo/simulator.cpp +++ b/gazebo/simulator.cpp @@ -18,7 +18,7 @@ #include "Arduino.h" #include "flix.h" -#include "cli.ino" +#include "console.ino" #include "control.ino" #include "estimate.ino" #include "safety.ino" @@ -83,7 +83,7 @@ public: attitude.setYaw(this->model->WorldPose().Yaw()); control(); - handleInput(); + handleConsole(); processMavlink(); applyMotorForces();