diff --git a/docs/usage.md b/docs/usage.md index eb4d3dc..c49a6f2 100644 --- 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 -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*. diff --git a/flix/cli.ino b/flix/cli.ino index 1343fe2..a901320 100644 --- 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++) { diff --git a/flix/control.ino b/flix/control.ino index 7fc1967..1c157c1 100644 --- 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"; } } diff --git a/flix/failsafe.ino b/flix/failsafe.ino index 466f091..b70f079 100644 --- 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; +} diff --git a/flix/mavlink.ino b/flix/mavlink.ino index 6f76711..18b70af 100644 --- 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); diff --git a/tools/pyflix/README.md b/tools/pyflix/README.md index 96815f7..8eefd4e 100644 --- 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 diff --git a/tools/pyflix/flix.py b/tools/pyflix/flix.py index cb73cf1..17869f3 100644 --- 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() diff --git a/tools/pyproject.toml b/tools/pyproject.toml index fcf6bac..1055a3e 100644 --- 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"