Add commands for switching modes

Make mode simple int instead of enum for simplify using in other subsystems

README.md

@@ -17,11 +17,11 @@
 
 * Dedicated for education and research.
 * Made from general-purpose components.
-* Simple and clean source code in Arduino (<2k lines firmware).
+* Simple and clean source code in Arduino.
-* Control using USB gamepad, remote control or smartphone.
+* Control using remote control or smartphone.
+* Precise simulation with Gazebo.
 * Wi-Fi and MAVLink support.
 * Wireless command line interface and analyzing.
-* Precise simulation with Gazebo.
 * Python library.
 * Textbook on flight control theory and practice ([in development](https://quadcopter.dev)).
 * *Position control (using external camera) and autonomous flights¹*.
@@ -38,11 +38,7 @@ Version 0 demo video: https://youtu.be/8GzzIQ3C6DQ.
 
 <a href="https://youtu.be/8GzzIQ3C6DQ"><img width=500 src="https://i3.ytimg.com/vi/8GzzIQ3C6DQ/maxresdefault.jpg"></a>
 
-Usage in education (RoboCamp): https://youtu.be/Wd3yaorjTx0.
+See the [user builds gallery](docs/user.md).
-
-<a href="https://youtu.be/Wd3yaorjTx0"><img width=500 src="https://i3.ytimg.com/vi/Wd3yaorjTx0/sddefault.jpg"></a>
-
-See the [user builds gallery](docs/user.md):
 
 <a href="docs/user.md"><img src="docs/img/user/user.jpg" width=500></a>
 
@@ -67,8 +63,8 @@ The simulator is implemented using Gazebo and runs the original Arduino code:
 |Type|Part|Image|Quantity|
 |-|-|:-:|:-:|
 |Microcontroller board|ESP32 Mini|<img src="docs/img/esp32.jpg" width=100>|1|
-|IMU (and barometer²) board|GY‑91, MPU-9265 (or other MPU‑9250/MPU‑6500 board)<br>ICM20948V2 (ICM‑20948)³<br>GY-521 (MPU-6050)³⁻¹|<img src="docs/img/gy-91.jpg" width=90 align=center><br><img src="docs/img/icm-20948.jpg" width=100><br><img src="docs/img/gy-521.jpg" width=100>|1|
+|IMU (and barometer²) board|GY‑91, MPU-9265 (or other MPU‑9250/MPU‑6500 board)<br>ICM‑20948³<br>GY-521 (MPU-6050)³⁻¹|<img src="docs/img/gy-91.jpg" width=90 align=center><br><img src="docs/img/icm-20948.jpg" width=100><br><img src="docs/img/gy-521.jpg" width=100>|1|
-|<span style="background:yellow">Buck-boost converter</span> (recommended)|To be determined, output 5V or 3.3V, see [user-contributed schematics](https://miro.com/app/board/uXjVN-dTjoo=/?moveToWidget=3458764612179508274&cot=14)|<img src="docs/img/buck-boost.jpg" width=100>|1|
+|<span style="background:yellow">(Recommended) Buck-boost converter</span>|To be determined, output 5V or 3.3V, see [user-contributed schematics](https://miro.com/app/board/uXjVN-dTjoo=/?moveToWidget=3458764612179508274&cot=14)|<img src="docs/img/buck-boost.jpg" width=100>|1|
 |Motor|8520 3.7V brushed motor (shaft 0.8mm).<br>Motor with exact 3.7V voltage is needed, not ranged working voltage (3.7V — 6V).|<img src="docs/img/motor.jpeg" width=100>|4|
 |Propeller|Hubsan 55 mm|<img src="docs/img/prop.jpg" width=100>|4|
 |MOSFET (transistor)|100N03A or [analog](https://t.me/opensourcequadcopter/33)|<img src="docs/img/100n03a.jpg" width=100>|4|
@@ -81,7 +77,7 @@ The simulator is implemented using Gazebo and runs the original Arduino code:
 |Frame main part|3D printed⁴:<br>[`flix-frame-1.1.stl`](docs/assets/flix-frame-1.1.stl) [`flix-frame-1.1.step`](docs/assets/flix-frame-1.1.step)<br>Recommended settings: layer 0.2 mm, line 0.4 mm, infill 100%.|<img src="docs/img/frame1.jpg" width=100>|1|
 |Frame top part|3D printed:<br>[`esp32-holder.stl`](docs/assets/esp32-holder.stl) [`esp32-holder.step`](docs/assets/esp32-holder.step)|<img src="docs/img/esp32-holder.jpg" width=100>|1|
 |Washer for IMU board mounting|3D printed:<br>[`washer-m3.stl`](docs/assets/washer-m3.stl) [`washer-m3.step`](docs/assets/washer-m3.step)|<img src="docs/img/washer-m3.jpg" width=100>|2|
-|Controller (recommended)|CC2500 transmitter, like BetaFPV LiteRadio CC2500 (RC receiver/Wi-Fi).<br>Two-sticks gamepad (Wi-Fi only) — see [recommended gamepads](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/setup_view/joystick.html#supported-joysticks).<br>Other⁵|<img src="docs/img/betafpv.jpg" width=100><img src="docs/img/logitech.jpg" width=80>|1|
+|*RC transmitter (optional)*|*KINGKONG TINY X8 (warning: lacks USB support) or other⁵*|<img src="docs/img/tx.jpg" width=100>|1|
 |*RC receiver (optional)*|*DF500 or other⁵*|<img src="docs/img/rx.jpg" width=100>|1|
 |Wires|28 AWG recommended|<img src="docs/img/wire-28awg.jpg" width=100>||
 |Tape, double-sided tape||||
@@ -90,7 +86,7 @@ The simulator is implemented using Gazebo and runs the original Arduino code:
 *³ — change `MPU9250` to `ICM20948` in `imu.ino` file if using ICM-20948 board.*<br>
 *³⁻¹ — MPU-6050 supports I²C interface only (not recommended). To use it change IMU declaration to `MPU6050 IMU(Wire)`.*<br>
 *⁴ — this frame is optimized for GY-91 board, if using other, the board mount holes positions should be modified.*<br>
-*⁵ — you also may use any transmitter-receiver pair with SBUS interface.*
+*⁵ — you may use any transmitter-receiver pair with SBUS interface.*
 
 Tools required for assembly:
 

docs/book/firmware.md

@@ -27,6 +27,6 @@
 
 Вспомогательные файлы включают:
 
-* [`vector.h`](https://github.com/okalachev/flix/blob/canonical/flix/vector.h), [`quaternion.h`](https://github.com/okalachev/flix/blob/canonical/flix/quaternion.h) — реализация библиотек векторов и кватернионов.
+* [`vector.h`](https://github.com/okalachev/flix/blob/canonical/flix/vector.h), [`quaternion.h`](https://github.com/okalachev/flix/blob/canonical/flix/quaternion.h) — реализация библиотек векторов и кватернионов проекта.
 * [`pid.h`](https://github.com/okalachev/flix/blob/canonical/flix/pid.h) — реализация общего ПИД-регулятора.
 * [`lpf.h`](https://github.com/okalachev/flix/blob/canonical/flix/lpf.h) — реализация общего фильтра нижних частот.

docs/build.md

@@ -173,7 +173,7 @@ Before flight using remote control, you need to calibrate it:
 2. Type `cr` command there and follow the instructions.
 3. Use the remote control to fly the drone!
 
-#### Control with USB remote control (Wi-Fi)
+#### Control with USB remote control
 
 If your drone doesn't have RC receiver installed, you can use USB remote control and QGroundControl app to fly it.
 

docs/firmware.md

@@ -30,7 +30,7 @@ Firmware source files are located in `flix` directory. The key files are:
 
 Utility files include:
 
-* [`vector.h`](../flix/vector.h), [`quaternion.h`](../flix/quaternion.h) — vector and quaternion libraries implementation.
+* [`vector.h`](../flix/vector.h), [`quaternion.h`](../flix/quaternion.h) — project's vector and quaternion libraries implementation.
 * [`pid.h`](../flix/pid.h) — generic PID controller implementation.
 * [`lpf.h`](../flix/lpf.h) — generic low-pass filter implementation.
 

docs/troubleshooting.md

@@ -6,7 +6,6 @@ Do the following:
 
 * **Check ESP32 core is installed**. Check if the version matches the one used in the [tutorial](build.md#firmware).
 * **Check libraries**. Install all the required libraries from the tutorial. Make sure there are no MPU9250 or other peripherals libraries that may conflict with the ones used in the tutorial.
-* **Check the chosen board**. The correct board to choose in Arduino IDE for ESP32 Mini is *WEMOS D1 MINI ESP32*.
 
 ## The drone doesn't fly
 

docs/user.md

@@ -4,25 +4,6 @@ This page contains user-built drones based on the Flix project. Publish your pro
 
 ---
 
-## RoboCamp
-
-Author: RoboCamp participants.<br>
-Description: 3D-printed and wooden frames, ESP32 Mini, DC-DC buck-boost converters. BetaFPV LiteRadio 3 to control the drones via Wi-Fi connection.<br>
-Features: altitude hold, obstacle avoidance, autonomous flight elements.<br>
-Some of the designed model files: https://drive.google.com/drive/folders/18YHWGquKeIevzrMH4-OUT-zKXMETTEUu?usp=share_link.
-
-RoboCamp took place in July 2025, Saint Petersburg, where 9 participants designed and built their own drones using the Flix project, and then modified the firmware to complete specific flight tasks.
-
-See the detailed video about the event:
-
-<a href="https://youtu.be/Wd3yaorjTx0"><img width=500 src="https://img.youtube.com/vi/Wd3yaorjTx0/sddefault.jpg"></a>
-
-Built drones:
-
-<img src="img/user/robocamp/1.jpg" width=500>
-
----
-
 Author: chkroko.<br>
 Description: the first Flix drone built with **brushless motors** (DShot interface).<br>
 Features: SpeedyBee BLS 35A Mini V2 ESC, ESP32-S3 board, EMAX ECO 2 2207 1700kv motors, ICM20948V2 IMU, INA226 power monitor and Bluetooth gamepad for control.<br>

flix/cli.ino

@@ -8,10 +8,13 @@
 #include "util.h"
 
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
+extern const int ACRO, STAB, AUTO;
 extern float loopRate, dt;
 extern double t;
 extern uint16_t channels[16];
 extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
+extern int mode;
+extern bool armed;
 
 const char* motd =
 "\nWelcome to\n"
@@ -31,6 +34,9 @@ const char* motd =
 "ps - show pitch/roll/yaw\n"
 "psq - show attitude quaternion\n"
 "imu - show IMU data\n"
+"arm - arm the drone (when no armed switch)\n"
+"disarm - disarm the drone (when no armed switch)\n"
+"stab/acro/auto - set mode (when no mode switch)\n"
 "rc - show RC data\n"
 "mot - show motor output\n"
 "log - dump in-RAM log\n"
@@ -104,8 +110,21 @@ void doCommand(String str, bool echo = false) {
 		print("qx: %f qy: %f qz: %f qw: %f\n", attitude.x, attitude.y, attitude.z, attitude.w);
 	} else if (command == "imu") {
 		printIMUInfo();
+		print("gyro: %f %f %f\n", rates.x, rates.y, rates.z);
+		print("acc: %f %f %f\n", acc.x, acc.y, acc.z);
 		printIMUCalibration();
+		print("rate: %.0f\n", loopRate);
 		print("landed: %d\n", landed);
+	} else if (command == "arm") {
+		armed = true;
+	} else if (command == "disarm") {
+		armed = false;
+	} else if (command == "stab") {
+		mode = STAB;
+	} else if (command == "acro") {
+		mode = ACRO;
+	} else if (command == "auto") {
+		mode = AUTO;
 	} else if (command == "rc") {
 		print("channels: ");
 		for (int i = 0; i < 16; i++) {
@@ -114,6 +133,7 @@ void doCommand(String str, bool echo = false) {
 		print("\nroll: %g pitch: %g yaw: %g throttle: %g armed: %g mode: %g\n",
 			controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode);
 		print("mode: %s\n", getModeName());
+		print("armed: %d\n", armed);
 	} else if (command == "mot") {
 		print("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]);

flix/control.ino

@@ -34,8 +34,8 @@
 #define TILT_MAX radians(30)
 #define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
 
-enum { MANUAL, ACRO, STAB, USER } mode = STAB;
+const int MANUAL = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
-enum { YAW, YAW_RATE } yawMode = YAW;
+int mode = STAB;
 bool armed = false;
 
 PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);
@@ -49,6 +49,7 @@ float tiltMax = TILT_MAX;
 
 Quaternion attitudeTarget;
 Vector ratesTarget;
+Vector ratesExtra; // feedforward rates
 Vector torqueTarget;
 float thrustTarget;
 
@@ -56,63 +57,50 @@ extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRO
 extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
 
 void control() {
-	interpretRC();
+	interpretControls();
 	failsafe();
-	if (mode == STAB) {
+	controlAttitude();
-		controlAttitude();
+	controlRates();
-		controlRate();
+	controlTorque();
-		controlTorque();
-	} else if (mode == ACRO) {
-		controlRate();
-		controlTorque();
-	} else if (mode == MANUAL) {
-		controlTorque();
-	}
 }
 
-void interpretRC() {
+void interpretControls() {
-	armed = controlThrottle >= 0.05 && controlArmed >= 0.5;
-
 	// NOTE: put ACRO or MANUAL modes there if you want to use them
-	if (controlMode < 0.25) {
+	if (controlMode < 0.25) mode = STAB;
-		mode = STAB;
+	if (controlMode < 0.75) mode = STAB;
-	} else if (controlMode < 0.75) {
+	if (controlMode > 0.75) mode = AUTO;
-		mode = STAB;
+	if (controlArmed < 0.5) armed = false;
-	} else {
+
-		mode = STAB;
+	if (mode == AUTO) return; // pilot is not effective in AUTO mode
-	}
+
+	if (landed && controlThrottle == 0 && controlYaw > 0.95) armed = true; // arm gesture
+	if (landed && controlThrottle == 0 && controlYaw < -0.95) armed = false; // disarm gesture
 
 	thrustTarget = controlThrottle;
 
-	if (mode == ACRO) {
+	if (mode == STAB) {
-		yawMode = YAW_RATE;
+		float yawTarget = attitudeTarget.getYaw();
-		ratesTarget.x = controlRoll * maxRate.x;
+		if (invalid(yawTarget) || controlYaw != 0) yawTarget = attitude.getYaw(); // reset yaw target if NAN or pilot commands yaw rate
-		ratesTarget.y = controlPitch* maxRate.y;
+		attitudeTarget = Quaternion::fromEuler(Vector(controlRoll * tiltMax, controlPitch * tiltMax, yawTarget));
-		ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
+		ratesExtra = Vector(0, 0, -controlYaw * maxRate.z); // positive yaw stick means clockwise rotation in FLU
-
-	} else if (mode == STAB) {
-		yawMode = controlYaw == 0 ? YAW : YAW_RATE;
-
-		attitudeTarget = Quaternion::fromEuler(Vector(
-			controlRoll * tiltMax,
-			controlPitch * tiltMax,
-			attitudeTarget.getYaw()));
-		ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
-
-	} else if (mode == MANUAL) {
-		// passthrough mode
-		yawMode = YAW_RATE;
-		torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01;
 	}
 
-	if (yawMode == YAW_RATE || !motorsActive()) {
+	if (mode == ACRO) {
-		// update yaw target as we don't have control over the yaw
+		attitudeTarget.invalidate();
-		attitudeTarget.setYaw(attitude.getYaw());
+		ratesTarget.x = controlRoll * maxRate.x;
+		ratesTarget.y = controlPitch * maxRate.y;
+		ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
+	}
+
+	if (mode == MANUAL) { // passthrough mode
+		attitudeTarget.invalidate();
+		ratesTarget.invalidate();
+		torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01;
 	}
 }
 
 void controlAttitude() {
-	if (!armed) {
+	if (!armed || attitudeTarget.invalid()) { // skip attitude control
 		rollPID.reset();
 		pitchPID.reset();
 		yawPID.reset();
@@ -125,17 +113,15 @@ void controlAttitude() {
 
 	Vector error = Vector::rotationVectorBetween(upTarget, upActual);
 
-	ratesTarget.x = rollPID.update(error.x, dt);
+	ratesTarget.x = rollPID.update(error.x, dt) + ratesExtra.x;
-	ratesTarget.y = pitchPID.update(error.y, dt);
+	ratesTarget.y = pitchPID.update(error.y, dt) + ratesExtra.y;
 
-	if (yawMode == YAW) {
+	float yawError = wrapAngle(attitudeTarget.getYaw() - attitude.getYaw());
-		float yawError = wrapAngle(attitudeTarget.getYaw() - attitude.getYaw());
+	ratesTarget.z = yawPID.update(yawError, dt) + ratesExtra.z;
-		ratesTarget.z = yawPID.update(yawError, dt);
-	}
 }
 
-void controlRate() {
+void controlRates() {
-	if (!armed) {
+	if (!armed || ratesTarget.invalid()) { // skip rates control
 		rollRatePID.reset();
 		pitchRatePID.reset();
 		yawRatePID.reset();
@@ -151,8 +137,10 @@ void controlRate() {
 }
 
 void controlTorque() {
-	if (!armed) {
+	if (!torqueTarget.valid()) return; // skip torque control
-		memset(motors, 0, sizeof(motors));
+
+	if (!armed || thrustTarget < 0.05) {
+		memset(motors, 0, sizeof(motors)); // stop motors if no thrust
 		return;
 	}
 
@@ -172,7 +160,7 @@ const char* getModeName() {
 		case MANUAL: return "MANUAL";
 		case ACRO: return "ACRO";
 		case STAB: return "STAB";
-		case USER: return "USER";
+		case AUTO: return "AUTO";
 		default: return "UNKNOWN";
 	}
 }

flix/estimate.ino

@@ -11,6 +11,8 @@
 #define WEIGHT_ACC 0.003
 #define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
 
+LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
+
 void estimate() {
 	applyGyro();
 	applyAcc();
@@ -18,7 +20,6 @@ void estimate() {
 
 void applyGyro() {
 	// filter gyro to get angular rates
-	static LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
 	rates = ratesFilter.update(gyro);
 
 	// apply rates to attitude

flix/failsafe.ino

@@ -3,6 +3,8 @@
 
 // Fail-safe functions
 
+#include "util.h"
+
 #define RC_LOSS_TIMEOUT 0.5
 #define DESCEND_TIME 3.0 // time to descend from full throttle to zero
 
@@ -10,32 +12,39 @@ extern double controlTime;
 extern float controlRoll, controlPitch, controlThrottle, controlYaw;
 
 void failsafe() {
-	armingFailsafe();
 	rcLossFailsafe();
-}
+	autoFailsafe();
-
-// Prevent arming without zero throttle input
-void armingFailsafe() {
-	static double zeroThrottleTime;
-	static double armingTime;
-	if (!armed) armingTime = t; // stores the last time when the drone was disarmed, therefore contains arming time
-	if (controlTime > 0 && controlThrottle < 0.05) zeroThrottleTime = controlTime;
-	if (armingTime - zeroThrottleTime > 0.1) armed = false; // prevent arming if there was no zero throttle for 0.1 sec
 }
 
 // RC loss failsafe
 void rcLossFailsafe() {
+	if (mode == AUTO) return;
+	if (!armed) return;
 	if (t - controlTime > RC_LOSS_TIMEOUT) {
 		descend();
 	}
 }
 
+// Allow pilot to interrupt automatic flight
+void autoFailsafe() {
+	static float roll, pitch, yaw, throttle;
+
+	if (roll != controlRoll || pitch != controlPitch || yaw != controlYaw || abs(throttle - controlThrottle) > 0.05) {
+		if (mode == AUTO && invalid(controlMode)) {
+			mode = STAB; // regain control to the pilot
+		}
+	}
+
+	roll = controlRoll;
+	pitch = controlPitch;
+	yaw = controlYaw;
+	throttle = controlThrottle;
+}
+
 // Smooth descend on RC lost
 void descend() {
-	mode = STAB;
+	mode = AUTO;
-	controlRoll = 0;
+	thrustTarget -= dt / DESCEND_TIME;
-	controlPitch = 0;
+	if (thrustTarget < 0) thrustTarget = 0;
-	controlYaw = 0;
+	if (thrustTarget == 0) armed = false;
-	controlThrottle -= dt / DESCEND_TIME;
-	if (controlThrottle < 0) controlThrottle = 0;
 }

flix/flix.ino

@@ -12,7 +12,8 @@
 
 double t = NAN; // current step time, s
 float dt; // time delta from previous step, s
-float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode; // pilot's inputs, range [-1, 1]
+float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
+float controlArmed = NAN, controlMode = NAN;
 Vector gyro; // gyroscope data
 Vector acc; // accelerometer data, m/s/s
 Vector rates; // filtered angular rates, rad/s

flix/imu.ino

@@ -122,12 +122,4 @@ void printIMUInfo() {
 	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());
-	print("rate: %.0f\n", loopRate);
-	print("gyro: %f %f %f\n", rates.x, rates.y, rates.z);
-	print("acc: %f %f %f\n", acc.x, acc.y, acc.z);
-	Vector rawGyro, rawAcc;
-	IMU.getGyro(rawGyro.x, rawGyro.y, rawGyro.z);
-	IMU.getAccel(rawAcc.x, rawAcc.y, rawAcc.z);
-	print("raw gyro: %f %f %f\n", rawGyro.x, rawGyro.y, rawGyro.z);
-	print("raw acc: %f %f %f\n", rawAcc.x, rawAcc.y, rawAcc.z);
 }

flix/mavlink.ino

@@ -36,7 +36,9 @@ void sendMavlink() {
 		lastSlow = t;
 
 		mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC,
-			MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (armed * MAV_MODE_FLAG_SAFETY_ARMED) | ((mode == STAB) * MAV_MODE_FLAG_STABILIZE_ENABLED),
+			(armed * MAV_MODE_FLAG_SAFETY_ARMED) |
+			(mode == STAB) * MAV_MODE_FLAG_STABILIZE_ENABLED |
+			((mode == AUTO) ? MAV_MODE_FLAG_AUTO_ENABLED : MAV_MODE_FLAG_MANUAL_INPUT_ENABLED),
 			mode, MAV_STATE_STANDBY);
 		sendMessage(&msg);
 
@@ -57,9 +59,9 @@ void sendMavlink() {
 			channels[0], channels[1], channels[2], channels[3], channels[4], channels[5], channels[6], channels[7], UINT8_MAX);
 		if (channels[0] != 0) sendMessage(&msg); // 0 means no RC input
 
-		float actuator[32];
+		float controls[8];
-		memcpy(actuator, motors, sizeof(motors));
+		memcpy(controls, motors, sizeof(motors));
-		mavlink_msg_actuator_output_status_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 4, actuator);
+		mavlink_msg_actuator_control_target_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, controls);
 		sendMessage(&msg);
 
 		mavlink_msg_scaled_imu_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time,
@@ -102,8 +104,8 @@ void handleMavlink(const void *_msg) {
 		controlPitch = m.x / 1000.0f * mavlinkControlScale;
 		controlRoll = m.y / 1000.0f * mavlinkControlScale;
 		controlYaw = m.r / 1000.0f * mavlinkControlScale;
-		controlMode = 1; // STAB mode
+		controlMode = NAN; // keep mode
-		controlArmed = 1; // armed
+		controlArmed = NAN;
 		controlTime = t;
 
 		if (abs(controlYaw) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controlYaw = 0;
@@ -174,6 +176,39 @@ void handleMavlink(const void *_msg) {
 		doCommand(data, true);
 	}
 
+	if (msg.msgid == MAVLINK_MSG_ID_SET_ATTITUDE_TARGET) {
+		if (mode != AUTO) return;
+
+		mavlink_set_attitude_target_t m;
+		mavlink_msg_set_attitude_target_decode(&msg, &m);
+		if (m.target_system && m.target_system != SYSTEM_ID) return;
+
+		// copy attitude, rates and thrust targets
+		ratesTarget.x = m.body_roll_rate;
+		ratesTarget.y = -m.body_pitch_rate; // convert to flu
+		ratesTarget.z = -m.body_yaw_rate;
+		attitudeTarget.w = m.q[0];
+		attitudeTarget.x = m.q[1];
+		attitudeTarget.y = -m.q[2];
+		attitudeTarget.z = -m.q[3];
+		thrustTarget = m.thrust;
+		ratesExtra = Vector(0, 0, 0);
+
+		if (m.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE) attitudeTarget.invalidate();
+
+		armed = m.thrust > 0;
+	}
+
+	if (msg.msgid == MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET) {
+		if (mode != AUTO) return;
+
+		mavlink_set_actuator_control_target_t m;
+		mavlink_msg_set_actuator_control_target_decode(&msg, &m);
+		if (m.target_system && m.target_system != SYSTEM_ID) return;
+
+		memcpy(motors, m.controls, sizeof(motors)); // copy motor thrusts
+	}
+
 	// Handle commands
 	if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
 		mavlink_command_long_t m;
@@ -188,8 +223,18 @@ void handleMavlink(const void *_msg) {
 			mavlink_msg_autopilot_version_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &response,
 				MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT | MAV_PROTOCOL_CAPABILITY_MAVLINK2, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0);
 			sendMessage(&response);
-		} else {
+		}
-			mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, MAV_RESULT_UNSUPPORTED, UINT8_MAX, 0, msg.sysid, msg.compid);
+
+		if (m.command == MAV_CMD_DO_SET_MODE) {
+			if (!(m.param2 >= 0 && m.param2 <= AUTO)) return; // incorrect mode
+			mode = m.param2;
+			mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, MAV_RESULT_ACCEPTED, UINT8_MAX, 0, msg.sysid, msg.compid);
+			sendMessage(&ack);
+		}
+
+		if (m.command == MAV_CMD_COMPONENT_ARM_DISARM) {
+			armed = m.param1 == 1;
+			mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, MAV_RESULT_ACCEPTED, UINT8_MAX, 0, msg.sysid, msg.compid);
 			sendMessage(&ack);
 		}
 	}

flix/quaternion.h

@@ -64,6 +64,21 @@ public:
 		return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z);
 	}
 
+	bool valid() const {
+		return finite();
+	}
+
+	bool invalid() const {
+		return !valid();
+	}
+
+	void invalidate() {
+		w = NAN;
+		x = NAN;
+		y = NAN;
+		z = NAN;
+	}
+
 	float norm() const {
 		return sqrt(w * w + x * x + y * y + z * z);
 	}
@@ -116,31 +131,29 @@ public:
 		return euler;
 	}
 
-	float getRoll() const {
-		return toEuler().x;
-	}
-
-	float getPitch() const {
-		return toEuler().y;
-	}
-
 	float getYaw() const {
-		return toEuler().z;
+		// https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L122
-	}
+		float yaw;
-
+		float sqx = x * x;
-	void setRoll(float roll) {
+		float sqy = y * y;
-		Vector euler = toEuler();
+		float sqz = z * z;
-		*this = Quaternion::fromEuler(Vector(roll, euler.y, euler.z));
+		float sqw = w * w;
-	}
+		double sarg = -2 * (x * z - w * y) / (sqx + sqy + sqz + sqw);
-
+		if (sarg <= -0.99999) {
-	void setPitch(float pitch) {
+			yaw = -2 * atan2(y, x);
-		Vector euler = toEuler();
+		} else if (sarg >= 0.99999) {
-		*this = Quaternion::fromEuler(Vector(euler.x, pitch, euler.z));
+			yaw = 2 * atan2(y, x);
+		} else {
+			yaw = atan2(2 * (x * y + w * z), sqw + sqx - sqy - sqz);
+		}
+		return yaw;
 	}
 
 	void setYaw(float yaw) {
+		// TODO: optimize?
 		Vector euler = toEuler();
-		*this = Quaternion::fromEuler(Vector(euler.x, euler.y, yaw));
+		euler.z = yaw;
+		(*this) = Quaternion::fromEuler(euler);
 	}
 
 	Quaternion operator * (const Quaternion& q) const {

flix/rc.ino

@@ -42,7 +42,7 @@ void normalizeRC() {
 	controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : NAN;
 	controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : NAN;
 	controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : NAN;
-	controlArmed = armedChannel >= 0 ? controls[(int)armedChannel] : 1; // assume armed by default
+	controlArmed = armedChannel >= 0 ? controls[(int)armedChannel] : NAN;
 	controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN;
 }
 

flix/util.h

@@ -19,6 +19,14 @@ float mapff(float x, float in_min, float in_max, float out_min, float out_max) {
 	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
 
+bool invalid(float x) {
+	return !isfinite(x);
+}
+
+bool valid(float x) {
+	return isfinite(x);
+}
+
 // Wrap angle to [-PI, PI)
 float wrapAngle(float angle) {
 	angle = fmodf(angle, 2 * PI);

flix/vector.h

@@ -21,6 +21,20 @@ public:
 		return isfinite(x) && isfinite(y) && isfinite(z);
 	}
 
+	bool valid() const {
+		return finite();
+	}
+
+	bool invalid() const {
+		return !valid();
+	}
+
+	void invalidate() {
+		x = NAN;
+		y = NAN;
+		z = NAN;
+	}
+
 	float norm() const {
 		return sqrt(x * x + y * y + z * z);
 	}

gazebo/flix.h

@@ -15,7 +15,8 @@
 double t = NAN;
 float dt;
 float motors[4];
-float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode;
+float controlRoll, controlPitch, controlYaw, controlThrottle = NAN;
+float controlArmed = NAN, controlMode = NAN;
 Vector acc;
 Vector gyro;
 Vector rates;
@@ -28,9 +29,9 @@ void computeLoopRate();
 void applyGyro();
 void applyAcc();
 void control();
-void interpretRC();
+void interpretControls();
 void controlAttitude();
-void controlRate();
+void controlRates();
 void controlTorque();
 const char* getModeName();
 void sendMotors();
@@ -54,8 +55,8 @@ void mavlinkPrint(const char* str);
 void sendMavlinkPrint();
 inline Quaternion fluToFrd(const Quaternion &q);
 void failsafe();
-void armingFailsafe();
 void rcLossFailsafe();
+void autoFailsafe();
 void descend();
 int parametersCount();
 const char *getParameterName(int index);

tools/pyflix/flix.py

@@ -6,7 +6,7 @@
 import os
 import time
 from queue import Queue, Empty
-from typing import Literal, Optional, Callable, List, Dict, Any, Union
+from typing import Literal, Optional, Callable, List, Dict, Any, Union, Sequence
 import logging
 import errno
 from threading import Thread, Timer
@@ -40,6 +40,7 @@ class Flix:
 
     _connection_timeout = 3
     _print_buffer: str = ''
+    _modes = ['MANUAL', 'ACRO', 'STAB', 'AUTO']
 
     def __init__(self, system_id: int=1, wait_connection: bool=True):
         if not (0 <= system_id < 256):
@@ -55,7 +56,7 @@ class Flix:
             if e.errno != errno.EADDRINUSE:
                 raise
             # Port busy - using proxy
-            logger.debug('Listening on port 14560 (proxy)')
+            logger.debug('Listening on port 14555 (proxy)')
             self.connection: mavutil.mavfile = mavutil.mavlink_connection('udpin:0.0.0.0:14555', source_system=254)  # type: ignore
         self.connection.target_system = system_id
         self.mavlink: mavlink.MAVLink = self.connection.mav
@@ -147,7 +148,7 @@ class Flix:
 
     def _handle_mavlink_message(self, msg: mavlink.MAVLink_message):
         if isinstance(msg, mavlink.MAVLink_heartbeat_message):
-            self.mode = ['MANUAL', 'ACRO', 'STAB', 'USER'][msg.custom_mode]
+            self.mode = self._modes[msg.custom_mode]
             self.armed = msg.base_mode & mavlink.MAV_MODE_FLAG_SAFETY_ARMED != 0
             self._trigger('mode', self.mode)
             self._trigger('armed', self.armed)
@@ -168,9 +169,13 @@ class Flix:
                              msg.chan5_raw, msg.chan6_raw, msg.chan7_raw, msg.chan8_raw]
             self._trigger('channels', self.channels)
 
+        if isinstance(msg, mavlink.MAVLink_actuator_control_target_message):
+            self.motors = msg.controls[:4]  # type: ignore
+            self._trigger('motors', self.motors)
+
+        # TODO: to be removed: the old way of passing motor outputs
         if isinstance(msg, mavlink.MAVLink_actuator_output_status_message):
             self.motors = msg.actuator[:4]  # type: ignore
-            self._trigger('motors', self.motors)
 
         if isinstance(msg, mavlink.MAVLink_scaled_imu_message):
             self.acc = self._mavlink_to_flu([msg.xacc / 1000, msg.yacc / 1000, msg.zacc / 1000])
@@ -231,6 +236,19 @@ class Flix:
     def _flu_to_mavlink(v: List[float]) -> List[float]:
         return Flix._mavlink_to_flu(v)
 
+    def _command_send(self, command: int, params: Sequence[float]):
+        if len(params) != 7:
+            raise ValueError('Command must have 7 parameters')
+        for attempt in range(3):
+            try:
+                logger.debug(f'Send command {command} with params {params} (attempt #{attempt + 1})')
+                self.mavlink.command_long_send(self.system_id, 0, command, 0, *params)  # type: ignore
+                self.wait('mavlink.COMMAND_ACK', value=lambda msg: msg.command == command and msg.result == mavlink.MAV_RESULT_ACCEPTED, timeout=0.1)
+                return
+            except TimeoutError:
+                continue
+        raise RuntimeError(f'Failed to send command {command} after 3 attempts')
+
     def _connected(self):
         # Reset disconnection timer
         self._disconnected_timer.cancel()
@@ -275,6 +293,11 @@ class Flix:
                 continue
         raise RuntimeError(f'Failed to set parameter {name} to {value} after 3 attempts')
 
+    def set_mode(self, mode: Union[str, int]):
+        if isinstance(mode, str):
+            mode = self._modes.index(mode.upper())
+        self._command_send(mavlink.MAV_CMD_DO_SET_MODE, (0, mode, 0, 0, 0, 0, 0))
+
     def set_position(self, position: List[float], yaw: Optional[float] = None, wait: bool = False, tolerance: float = 0.1):
         raise NotImplementedError('Position control is not implemented yet')
 
@@ -282,17 +305,37 @@ class Flix:
         raise NotImplementedError('Velocity control is not implemented yet')
 
     def set_attitude(self, attitude: List[float], thrust: float):
-        raise NotImplementedError('Automatic flight is not implemented yet')
+        if len(attitude) == 3:
+            attitude = Quaternion([attitude[0], attitude[1], attitude[2]]).q  # type: ignore
+        elif len(attitude) != 4:
+            raise ValueError('Attitude must be [roll, pitch, yaw] or [w, x, y, z] quaternion')
+        if not (0 <= thrust <= 1):
+            raise ValueError('Thrust must be in range [0, 1]')
+        attitude = self._flu_to_mavlink(attitude)
+        for _ in range(2): # duplicate to ensure delivery
+            self.mavlink.set_attitude_target_send(0, self.system_id, 0, 0,
+                [attitude[0], attitude[1], attitude[2], attitude[3]],
+                0, 0, 0, thrust)
 
     def set_rates(self, rates: List[float], thrust: float):
-        raise NotImplementedError('Automatic flight is not implemented yet')
+        if len(rates) != 3:
+            raise ValueError('Rates must be [roll_rate, pitch_rate, yaw_rate]')
+        if not (0 <= thrust <= 1):
+            raise ValueError('Thrust must be in range [0, 1]')
+        rates = self._flu_to_mavlink(rates)
+        for _ in range(2):  # duplicate to ensure delivery
+            self.mavlink.set_attitude_target_send(0, self.system_id, 0,
+                mavlink.ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE,
+                [1, 0, 0, 0],
+                rates[0], rates[1], rates[2], thrust)
 
     def set_motors(self, motors: List[float]):
         if len(motors) != 4:
             raise ValueError('motors must have 4 values')
         if not all(0 <= m <= 1 for m in motors):
             raise ValueError('motors must be in range [0, 1]')
-        raise NotImplementedError
+        for _ in range(2):  # duplicate to ensure delivery
+            self.mavlink.set_actuator_control_target_send(time.time() * 1000, 0, self.system_id, 0, motors + [0] * 4)  # type: ignore
 
     def set_controls(self, roll: float, pitch: float, yaw: float, throttle: float):
         """Send pilot's controls. Warning: not intended for automatic control"""
@@ -302,9 +345,6 @@ class Flix:
             raise ValueError('throttle must be in range [0, 1]')
         self.mavlink.manual_control_send(self.system_id, roll * 1000, pitch * 1000, yaw * 1000, throttle * 1000, 0)  # type: ignore
 
-    def set_mode(self, mode: Literal['MANUAL', 'ACRO', 'STAB', 'USER']):
-        raise NotImplementedError('Setting mode is not implemented yet')
-
     def cli(self, cmd: str, wait_response: bool = True) -> str:
         cmd = cmd.strip()
         if cmd == 'reboot':