Minor docs change

Add methods to Quaternion for consistency with getYaw and setYaw

README.md

@@ -63,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>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">(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|
+|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|
+|<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|
 |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|
@@ -77,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|
-|*RC transmitter (optional)*|*KINGKONG TINY X8 (warning: lacks USB support) or other⁵*|<img src="docs/img/tx.jpg" width=100>|1|
+|RC transmitter (recommended)|BetaFPV LiteRadio (CC2500) — with USB support (can control via Wi-Fi).<br>KINGKONG TINY X8 — warning: lacks USB support.<br>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||||
@@ -86,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 may use any transmitter-receiver pair with SBUS interface.*
+*⁵ — you may use any transmitter-receiver pair with SBUS interface, or any transmitter with USB support*
 
 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/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) — project's vector and quaternion libraries implementation.
+* [`vector.h`](../flix/vector.h), [`quaternion.h`](../flix/quaternion.h) — 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,6 +6,7 @@ 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,6 +4,24 @@ 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.
+
+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,13 +8,10 @@
 #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"
@@ -34,9 +31,6 @@ 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"
@@ -115,16 +109,6 @@ void doCommand(String str, bool echo = false) {
 		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++) {
@@ -133,7 +117,6 @@ 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
 
-const int MANUAL = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
-int mode = STAB;
+enum { MANUAL, ACRO, STAB, USER } mode = STAB;
+enum { YAW, YAW_RATE } yawMode = YAW;
 bool armed = false;
 
 PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);
@@ -49,7 +49,6 @@ float tiltMax = TILT_MAX;
 
 Quaternion attitudeTarget;
 Vector ratesTarget;
-Vector ratesExtra; // feedforward rates
 Vector torqueTarget;
 float thrustTarget;
 
@@ -57,50 +56,63 @@ extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRO
 extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
 
 void control() {
-	interpretControls();
+	interpretRC();
 	failsafe();
-	controlAttitude();
-	controlRates();
-	controlTorque();
+	if (mode == STAB) {
+		controlAttitude();
+		controlRate();
+		controlTorque();
+	} else if (mode == ACRO) {
+		controlRate();
+		controlTorque();
+	} else if (mode == MANUAL) {
+		controlTorque();
+	}
 }
 
-void interpretControls() {
+void interpretRC() {
+	armed = controlThrottle >= 0.05 && controlArmed >= 0.5;
+
 	// NOTE: put ACRO or MANUAL modes there if you want to use them
-	if (controlMode < 0.25) mode = STAB;
-	if (controlMode < 0.75) mode = STAB;
-	if (controlMode > 0.75) mode = AUTO;
-	if (controlArmed < 0.5) armed = false;
-
-	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
+	if (controlMode < 0.25) {
+		mode = STAB;
+	} else if (controlMode < 0.75) {
+		mode = STAB;
+	} else {
+		mode = STAB;
+	}
 
 	thrustTarget = controlThrottle;
 
-	if (mode == STAB) {
-		float yawTarget = attitudeTarget.getYaw();
-		if (invalid(yawTarget) || controlYaw != 0) yawTarget = attitude.getYaw(); // reset yaw target if NAN or pilot commands yaw rate
-		attitudeTarget = Quaternion::fromEuler(Vector(controlRoll * tiltMax, controlPitch * tiltMax, yawTarget));
-		ratesExtra = Vector(0, 0, -controlYaw * maxRate.z); // positive yaw stick means clockwise rotation in FLU
-	}
-
 	if (mode == ACRO) {
-		attitudeTarget.invalidate();
+		yawMode = YAW_RATE;
 		ratesTarget.x = controlRoll * maxRate.x;
-		ratesTarget.y = controlPitch * maxRate.y;
+		ratesTarget.y = controlPitch* maxRate.y;
 		ratesTarget.z = -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 (mode == MANUAL) { // passthrough mode
-		attitudeTarget.invalidate();
-		ratesTarget.invalidate();
-		torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01;
+	if (yawMode == YAW_RATE || !motorsActive()) {
+		// update yaw target as we don't have control over the yaw
+		attitudeTarget.setYaw(attitude.getYaw());
 	}
 }
 
 void controlAttitude() {
-	if (!armed || attitudeTarget.invalid()) { // skip attitude control
+	if (!armed) {
 		rollPID.reset();
 		pitchPID.reset();
 		yawPID.reset();
@@ -113,15 +125,17 @@ void controlAttitude() {
 
 	Vector error = Vector::rotationVectorBetween(upTarget, upActual);
 
-	ratesTarget.x = rollPID.update(error.x, dt) + ratesExtra.x;
-	ratesTarget.y = pitchPID.update(error.y, dt) + ratesExtra.y;
+	ratesTarget.x = rollPID.update(error.x, dt);
+	ratesTarget.y = pitchPID.update(error.y, dt);
 
-	float yawError = wrapAngle(attitudeTarget.getYaw() - attitude.getYaw());
-	ratesTarget.z = yawPID.update(yawError, dt) + ratesExtra.z;
+	if (yawMode == YAW) {
+		float yawError = wrapAngle(attitudeTarget.getYaw() - attitude.getYaw());
+		ratesTarget.z = yawPID.update(yawError, dt);
+	}
 }
 
-void controlRates() {
-	if (!armed || ratesTarget.invalid()) { // skip rates control
+void controlRate() {
+	if (!armed) {
 		rollRatePID.reset();
 		pitchRatePID.reset();
 		yawRatePID.reset();
@@ -137,10 +151,8 @@ void controlRates() {
 }
 
 void controlTorque() {
-	if (!torqueTarget.valid()) return; // skip torque control
-
-	if (!armed || thrustTarget < 0.05) {
-		memset(motors, 0, sizeof(motors)); // stop motors if no thrust
+	if (!armed) {
+		memset(motors, 0, sizeof(motors));
 		return;
 	}
 
@@ -160,7 +172,7 @@ const char* getModeName() {
 		case MANUAL: return "MANUAL";
 		case ACRO: return "ACRO";
 		case STAB: return "STAB";
-		case AUTO: return "AUTO";
+		case USER: return "USER";
 		default: return "UNKNOWN";
 	}
 }

flix/estimate.ino

@@ -11,8 +11,6 @@
 #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();
@@ -20,6 +18,7 @@ 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,8 +3,6 @@
 
 // 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
 
@@ -12,39 +10,32 @@ 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 = AUTO;
-	thrustTarget -= dt / DESCEND_TIME;
-	if (thrustTarget < 0) thrustTarget = 0;
-	if (thrustTarget == 0) armed = false;
+	mode = STAB;
+	controlRoll = 0;
+	controlPitch = 0;
+	controlYaw = 0;
+	controlThrottle -= dt / DESCEND_TIME;
+	if (controlThrottle < 0) controlThrottle = 0;
 }

flix/flix.ino

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

flix/mavlink.ino

@@ -36,9 +36,7 @@ void sendMavlink() {
 		lastSlow = t;
 
 		mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC,
-			(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),
+			MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (armed * MAV_MODE_FLAG_SAFETY_ARMED) | ((mode == STAB) * MAV_MODE_FLAG_STABILIZE_ENABLED),
 			mode, MAV_STATE_STANDBY);
 		sendMessage(&msg);
 
@@ -59,9 +57,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 controls[8];
-		memcpy(controls, motors, sizeof(motors));
-		mavlink_msg_actuator_control_target_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, controls);
+		float actuator[32];
+		memcpy(actuator, motors, sizeof(motors));
+		mavlink_msg_actuator_output_status_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 4, actuator);
 		sendMessage(&msg);
 
 		mavlink_msg_scaled_imu_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time,
@@ -104,8 +102,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 = NAN; // keep mode
-		controlArmed = NAN;
+		controlMode = 1; // STAB mode
+		controlArmed = 1; // armed
 		controlTime = t;
 
 		if (abs(controlYaw) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controlYaw = 0;
@@ -176,39 +174,6 @@ 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;
@@ -223,18 +188,8 @@ 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);
-		}
-
-		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);
+		} 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);
 			sendMessage(&ack);
 		}
 	}

flix/quaternion.h

@@ -64,21 +64,6 @@ 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);
 	}
@@ -131,29 +116,31 @@ public:
 		return euler;
 	}
 
+	float getRoll() const {
+		return toEuler().x;
+	}
+
+	float getPitch() const {
+		return toEuler().y;
+	}
+
 	float getYaw() const {
-		// https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L122
-		float yaw;
-		float sqx = x * x;
-		float sqy = y * y;
-		float sqz = z * z;
-		float sqw = w * w;
-		double sarg = -2 * (x * z - w * y) / (sqx + sqy + sqz + sqw);
-		if (sarg <= -0.99999) {
-			yaw = -2 * atan2(y, x);
-		} else if (sarg >= 0.99999) {
-			yaw = 2 * atan2(y, x);
-		} else {
-			yaw = atan2(2 * (x * y + w * z), sqw + sqx - sqy - sqz);
-		}
-		return yaw;
+		return toEuler().z;
+	}
+
+	void setRoll(float roll) {
+		Vector euler = toEuler();
+		*this = Quaternion::fromEuler(Vector(roll, euler.y, euler.z));
+	}
+
+	void setPitch(float pitch) {
+		Vector euler = toEuler();
+		*this = Quaternion::fromEuler(Vector(euler.x, pitch, euler.z));
 	}
 
 	void setYaw(float yaw) {
-		// TODO: optimize?
 		Vector euler = toEuler();
-		euler.z = yaw;
-		(*this) = Quaternion::fromEuler(euler);
+		*this = Quaternion::fromEuler(Vector(euler.x, euler.y, yaw));
 	}
 
 	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] : NAN;
+	controlArmed = armedChannel >= 0 ? controls[(int)armedChannel] : 1; // assume armed by default
 	controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN;
 }
 

flix/util.h

@@ -19,14 +19,6 @@ 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,20 +21,6 @@ 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,8 +15,7 @@
 double t = NAN;
 float dt;
 float motors[4];
-float controlRoll, controlPitch, controlYaw, controlThrottle = NAN;
-float controlArmed = NAN, controlMode = NAN;
+float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode;
 Vector acc;
 Vector gyro;
 Vector rates;
@@ -29,9 +28,9 @@ void computeLoopRate();
 void applyGyro();
 void applyAcc();
 void control();
-void interpretControls();
+void interpretRC();
 void controlAttitude();
-void controlRates();
+void controlRate();
 void controlTorque();
 const char* getModeName();
 void sendMotors();
@@ -55,8 +54,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, Sequence
+from typing import Literal, Optional, Callable, List, Dict, Any, Union
 import logging
 import errno
 from threading import Thread, Timer
@@ -40,7 +40,6 @@ 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):
@@ -56,7 +55,7 @@ class Flix:
             if e.errno != errno.EADDRINUSE:
                 raise
             # Port busy - using proxy
-            logger.debug('Listening on port 14555 (proxy)')
+            logger.debug('Listening on port 14560 (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
@@ -148,7 +147,7 @@ class Flix:
 
     def _handle_mavlink_message(self, msg: mavlink.MAVLink_message):
         if isinstance(msg, mavlink.MAVLink_heartbeat_message):
-            self.mode = self._modes[msg.custom_mode]
+            self.mode = ['MANUAL', 'ACRO', 'STAB', 'USER'][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)
@@ -169,13 +168,9 @@ 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])
@@ -236,19 +231,6 @@ 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()
@@ -293,11 +275,6 @@ 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')
 
@@ -305,37 +282,17 @@ class Flix:
         raise NotImplementedError('Velocity control is not implemented yet')
 
     def set_attitude(self, attitude: List[float], thrust: float):
-        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)
+        raise NotImplementedError('Automatic flight is not implemented yet')
 
     def set_rates(self, rates: List[float], thrust: float):
-        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)
+        raise NotImplementedError('Automatic flight is not implemented yet')
 
     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]')
-        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
+        raise NotImplementedError
 
     def set_controls(self, roll: float, pitch: float, yaw: float, throttle: float):
         """Send pilot's controls. Warning: not intended for automatic control"""
@@ -345,6 +302,9 @@ 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':