Implement AUTO mode for automatic flights

Support SET_ATTITUDE_TARGET, SET_ACTUATOR_CONTROL_TARGET in mavlink. ACTUATOR_OUTPUT_STATUS is changed ACTUATOR_CONTROL_TARGET to match used message for setting motor outputs. Add support for changing mode from mavlink. Support automatic flights in pyflix.
2026-01-10 13:06:56 +00:00 · 2025-08-28 00:49:24 +03:00
parent 10fafbc4a0
commit 40fc4b96b5
8 changed files with 181 additions and 33 deletions
--- a/docs/usage.md
+++ b/docs/usage.md
@@ -205,7 +205,7 @@ To start the motors, you should **arm** the drone. To do that, move the left sti

 <img src="img/arming.svg" width="150">

-After that, the motors will start spinning at low speed, indicating that the drone is armed and ready to fly.
+After that, the motors **will start spinning** at low speed, indicating that the drone is armed and ready to fly.

 When finished flying, **disarm** the drone, moving the left stick to the bottom left corner:

@@ -230,6 +230,12 @@ In this mode, the pilot controls the angular rates. This control method is diffi

 Manual mode disables all the stabilization, and the pilot inputs are passed directly to the motors. This mode is intended for testing and demonstration purposes only, and basically the drone **cannot fly in this mode**.

+### AUTO
+
+In this mode, the pilot inputs are ignored (except the mode switch, if configured). The drone can be controlled using [pyflix](../tools/pyflix/) Python library, or by modifying the firmware to implement the needed autonomous behavior.
+
+If the pilot moves the control sticks, the drone will switch back to *STAB* mode.
+
 ## Adjusting parameters

 You can adjust some of the drone's parameters (include PID coefficients) in QGroundControl app. In order to do that, go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Parameters*.
--- a/flix/cli.ino
+++ b/flix/cli.ino
@@ -8,7 +8,7 @@
 #include "util.h"

 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
-extern const int ACRO, STAB;
+extern const int ACRO, STAB, AUTO;
 extern float loopRate, dt;
 extern double t;
 extern uint16_t channels[16];
@@ -36,7 +36,7 @@ const char* motd =
 "imu - show IMU data\n"
 "arm - arm the drone\n"
 "disarm - disarm the drone\n"
-"stab/acro - set mode\n"
+"stab/acro/auto - set mode\n"
 "rc - show RC data\n"
 "mot - show motor output\n"
 "log - dump in-RAM log\n"
@@ -120,6 +120,8 @@ void doCommand(String str, bool echo = false) {
 		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++) {
--- a/flix/control.ino
+++ b/flix/control.ino
@@ -35,7 +35,7 @@
 #define TILT_MAX radians(30)
 #define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz

-const int MANUAL = 0, ACRO = 1, STAB = 2; // flight modes
+const int MANUAL = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
 int mode = STAB;
 bool armed = false;

@@ -71,6 +71,8 @@ void interpretControls() {
 	if (controlMode < 0.75) mode = STAB;
 	if (controlMode > 0.75) mode = STAB;

+	if (mode == AUTO) return; // pilot is not effective in AUTO mode
+
 	if (controlThrottle < 0.05 && controlYaw > 0.95) armed = true; // arm gesture
 	if (controlThrottle < 0.05 && controlYaw < -0.95) armed = false; // disarm gesture

@@ -168,6 +170,7 @@ const char* getModeName() {
 		case MANUAL: return "MANUAL";
 		case ACRO: return "ACRO";
 		case STAB: return "STAB";
+		case AUTO: return "AUTO";
 		default: return "UNKNOWN";
 	}
 }
--- a/flix/failsafe.ino
+++ b/flix/failsafe.ino
@@ -11,6 +11,7 @@ extern float controlRoll, controlPitch, controlThrottle, controlYaw;

 void failsafe() {
 	rcLossFailsafe();
+	autoFailsafe();
 }

 // RC loss failsafe
@@ -32,3 +33,16 @@ void descend() {
 		controlThrottle = 0;
 	}
 }
+
+// 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) {
+		// controls changed
+		if (mode == AUTO) mode = STAB; // regain control by the pilot
+	}
+	roll = controlRoll;
+	pitch = controlPitch;
+	yaw = controlYaw;
+	throttle = controlThrottle;
+}
--- a/flix/mavlink.ino
+++ b/flix/mavlink.ino
@@ -35,7 +35,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 : 0) |
+			((mode == STAB) ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0) |
+			((mode == AUTO) ? MAV_MODE_FLAG_AUTO_ENABLED : MAV_MODE_FLAG_MANUAL_INPUT_ENABLED),
 			mode, MAV_STATE_STANDBY);
 		sendMessage(&msg);

@@ -56,9 +58,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];
-		memcpy(actuator, motors, sizeof(motors));
-		mavlink_msg_actuator_output_status_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 4, actuator);
+		float controls[8];
+		memcpy(controls, motors, sizeof(motors));
+		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,
@@ -172,6 +174,42 @@ 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;
+
+		attitudeTarget.invalidate();
+		ratesTarget.invalidate();
+		torqueTarget.invalidate();
+		memcpy(motors, m.controls, sizeof(motors)); // copy motor thrusts
+		armed = motors[0] > 0 || motors[1] > 0 || motors[2] > 0 || motors[3] > 0;
+	}
+
 	// Handle commands
 	if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
 		mavlink_command_long_t m;
@@ -192,6 +230,12 @@ void handleMavlink(const void *_msg) {
 			armed = m.param1 == 1;
 		}

+		if (m.command == MAV_CMD_DO_SET_MODE) {
+			if (!(m.param2 >= 0 && m.param2 <= AUTO)) return; // incorrect mode
+			accepted = true;
+			mode = m.param2;
+		}
+
 		// send command ack
 		mavlink_message_t ack;
 		mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, accepted ? MAV_RESULT_ACCEPTED : MAV_RESULT_UNSUPPORTED, UINT8_MAX, 0, msg.sysid, msg.compid);
--- a/tools/pyflix/README.md
+++ b/tools/pyflix/README.md
@@ -66,13 +66,14 @@ gyro = flix.wait('gyro')  # wait for gyroscope update
 attitude = flix.wait('attitude', timeout=3)  # wait for attitude update, raise TimeoutError after 3 seconds
 ```

-The `value` argument specifies a condition for filtering events. It can be either an expected value or a callable:
+The second argument (`value`) specifies a condition for filtering events. It can be either an expected value or a callable:

 ```python
-flix.wait('armed', value=True)  # wait until armed
-flix.wait('armed', value=False)  # wait until disarmed
-flix.wait('motors', value=lambda motors: not any(motors))  # wait until all motors stop
-flix.wait('attitude_euler', value=lambda att: att[0] > 0)  # wait until roll angle is positive
+flix.wait('armed', True)  # wait until armed
+flix.wait('armed', False)  # wait until disarmed
+flix.wait('mode', 'AUTO')  # wait until flight mode is switched to AUTO
+flix.wait('motors', lambda motors: not any(motors))  # wait until all motors stop
+flix.wait('attitude_euler', lambda att: att[0] > 0)  # wait until roll angle is positive
 ```

 Full list of events:
@@ -107,7 +108,7 @@ Get and set firmware parameters using `get_param` and `set_param` methods:

 ```python
 pitch_p = flix.get_param('PITCH_P')  # get parameter value
-flix.set_param('PITCH_P', 5)      # set parameter value
+flix.set_param('PITCH_P', 5)         # set parameter value
 ```

 Execute CLI commands using `cli` method. This method returns command response:
@@ -121,17 +122,54 @@ flix.cli('reboot')       # reboot the drone
 > [!TIP]
 > Use `help` command to get the list of available commands.

+You can imitate pilot's controls using `set_controls` method:
+
+```python
+flix.set_controls(roll=0, pitch=0, yaw=0, throttle=0.6)
+```
+
+> [!WARNING]
+> This method **is not intended for automatic flights**, only for adding support for a custom pilot input device.
+
 ### Automatic flight

-The flight control feature is in development. List of methods intended for automatic flight control:
+To perform automatic flight, switch the mode to *AUTO*, either from the remote control, or from the code:

-* `set_position`
-* `set_velocity`
-* `set_attitude`
-* `set_rates`
-* `set_motors`
-* `set_controls`
-* `set_mode`
+```python
+flix.set_mode('AUTO')
+```
+
+In this mode you can set flight control targets. Setting attitude target:
+
+```python
+flix.set_attitude([0.1, 0.2, 0.3], 0.6)  # set target roll, pitch, yaw and thrust
+flix.set_attitude([1, 0, 0, 0], 0.6)  # set target attitude quaternion and thrust
+```
+
+Setting angular rates target:
+
+```python
+flix.set_rates([0.1, 0.2, 0.3], 0.6)  # set target roll rate, pitch rate, yaw rate and thrust
+```
+
+You also can control raw motors outputs directly:
+
+```python
+flix.set_motors([0.5, 0.5, 0.5, 0.5])  # set motors outputs in range [0, 1]
+```
+
+In *AUTO* mode, the drone will arm automatically if the thrust is greater than zero, and disarm if thrust is zero. Therefore, to disarm the drone, set thrust to zero:
+
+```python
+flix.set_attitude([0, 0, 0], 0)  # disarm the drone
+```
+
+The following methods are in development and are not functional yet:
+
+* `set_position` — set target position.
+* `set_velocity` — set target velocity.
+
+To exit from *AUTO* mode move control sticks and the drone will switch to *STAB* mode.

 ## Usage alongside QGroundControl

--- a/tools/pyflix/flix.py
+++ b/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,6 +169,11 @@ 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)
@@ -231,6 +237,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 +294,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 +306,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(int(time.time() * 1000000), 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"""
@@ -300,10 +344,7 @@ class Flix:
            raise ValueError('roll, pitch, yaw must be in range [-1, 1]')
        if not 0 <= throttle <= 1:
            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')
+        self.mavlink.manual_control_send(self.system_id, int(roll * 1000), int(pitch * 1000), int(yaw * 1000), int(throttle * 1000), 0)  # type: ignore

    def cli(self, cmd: str, wait_response: bool = True) -> str:
        cmd = cmd.strip()
--- a/tools/pyproject.toml
+++ b/tools/pyproject.toml
@@ -1,6 +1,6 @@
 [project]
 name = "pyflix"
-version = "0.5"
+version = "0.6"
 description = "Python API for Flix drone"
 authors = [{ name="Oleg Kalachev", email="okalachev@gmail.com" }]
 license = "MIT"