|
| 1 | +""" |
| 2 | +ControlPacket Module for Drone Communication |
| 3 | +============================================ |
| 4 | +
|
| 5 | +Overview: |
| 6 | +--------- |
| 7 | +This Python module provides the ControlPacket class, which is central to creating, |
| 8 | +packing, and unpacking control packets used for drone control operations via MAVSDK |
| 9 | +or other compatible UAV control systems. It supports a variety of control modalities |
| 10 | +including position, velocity, acceleration, and direct attitude control. |
| 11 | +
|
| 12 | +The control packets are designed to be transmitted over UDP, enabling real-time |
| 13 | +control of drones with high reliability and minimal latency. The packets include |
| 14 | +comprehensive setpoints for sophisticated flight dynamics and maneuvers. |
| 15 | +
|
| 16 | +Features: |
| 17 | +--------- |
| 18 | +- Multiple Control Modes: Supports various modes such as POSITION_GLOBAL_LATLON, |
| 19 | + POSITION_LOCAL_NED, VELOCITY_NED, and ATTITUDE_CONTROL, among others. |
| 20 | +- Yaw Control: Optional direct yaw control alongside standard setpoints, allowing |
| 21 | + for dynamic adjustment of the drone's orientation. |
| 22 | +- Binary Packing: Uses Python's `struct` module for efficient binary packing and |
| 23 | + unpacking of control data, suitable for high-performance network transmission. |
| 24 | +- Error Handling: Robust error handling for data integrity checks using CRC and |
| 25 | + structured exception management. |
| 26 | +
|
| 27 | +Setpoint Modes Enum: |
| 28 | +-------------------- |
| 29 | +- POSITION_GLOBAL_LATLON: Global coordinates (latitude, longitude, altitude) |
| 30 | +- POSITION_LOCAL_NED: Local coordinates (north, east, down) |
| 31 | +- VELOCITY_NED: Local frame velocity (north, east, down) |
| 32 | +- VELOCITY_BODY: Body frame velocity (forward, right, down) |
| 33 | +- POSITION_VELOCITY_NED: Combined local position and velocity |
| 34 | +- POSITION_VELOCITY_ACCELERATION_NED: Combined position, velocity, and acceleration |
| 35 | +- ACCELERATION_NED: Local frame acceleration |
| 36 | +- ATTITUDE_CONTROL: Direct control of roll, pitch, yaw, and thrust |
| 37 | +- YAW_CONTROL: Separate flag for explicit yaw control |
| 38 | +
|
| 39 | +Usage: |
| 40 | +------ |
| 41 | +Instantiate a `ControlPacket` with desired setpoints and mode flags, then use |
| 42 | +`pack()` to serialize for transmission and `unpack()` to deserialize received packets. |
| 43 | +Example usage is provided in the sender and receiver scripts accompanying this module. |
| 44 | +
|
| 45 | +Author: |
| 46 | +------- |
| 47 | +Alireza Ghaderi |
| 48 | +GitHub: alireza787b |
| 49 | +Date: April 2024 |
| 50 | +
|
| 51 | +Example: |
| 52 | +-------- |
| 53 | +```python |
| 54 | +packet = ControlPacket( |
| 55 | + mode=SetpointMode.POSITION_VELOCITY_NED | SetpointMode.YAW_CONTROL, |
| 56 | + enable_flag=True, |
| 57 | + yaw_control_flag=True, |
| 58 | + position=(10.0, 20.0, -5.0), |
| 59 | + velocity=(0.5, 0.5, 0.0), |
| 60 | + acceleration=(0.0, 0.0, 0.0), |
| 61 | + attitude=(0.0, 0.0, 30.0, 0.6), # Yaw to 30 degrees, 60% thrust |
| 62 | + attitude_rate=(0, 0, 0, 0) |
| 63 | +) |
| 64 | +packed_data = packet.pack() |
| 65 | +unpacked_packet = ControlPacket.unpack(packed_data) |
| 66 | +unpacked_packet.debug_print() |
| 67 | +""" |
| 68 | +import struct |
| 69 | +import zlib |
| 70 | +import time |
| 71 | +from enum import Enum |
| 72 | + |
| 73 | +class SetpointMode(Enum): |
| 74 | + POSITION_GLOBAL_LATLON = 0x40 |
| 75 | + POSITION_LOCAL_NED = 0x20 |
| 76 | + VELOCITY_NED = 0x80 |
| 77 | + VELOCITY_BODY = 0x100 |
| 78 | + POSITION_VELOCITY_NED = 0x01 |
| 79 | + POSITION_VELOCITY_ACCELERATION_NED = 0x02 |
| 80 | + ACCELERATION_NED = 0x04 |
| 81 | + ATTITUDE_CONTROL = 0x08 # Direct attitude control including thrust |
| 82 | + YAW_CONTROL = 0x200 # Separate flag for yaw control |
| 83 | + |
| 84 | +class ControlPacket: |
| 85 | + HEADER = 0xDEADBEEFDEADBEEF |
| 86 | + HEADER_FORMAT = ">Q" # 8 bytes for header |
| 87 | + DATA_FORMAT = ">QII3d3d3d4d4dI" # Updated format to include all required fields |
| 88 | + CRC_FORMAT = ">I" # 4 bytes for CRC |
| 89 | + |
| 90 | + def __init__(self, mode, enable_flag, yaw_control_flag, position, velocity, acceleration, attitude, attitude_rate, timestamp=None): |
| 91 | + self.timestamp = timestamp if timestamp is not None else int(time.time() * 1e9) |
| 92 | + self.setpoint_flags = mode.value |
| 93 | + self.enable_flag = int(enable_flag) # Ensure enable_flag is integer |
| 94 | + self.yaw_control_flag = int(yaw_control_flag) # Ensure yaw_control_flag is integer |
| 95 | + self.position = position |
| 96 | + self.velocity = velocity |
| 97 | + self.acceleration = acceleration |
| 98 | + self.attitude = attitude |
| 99 | + self.attitude_rate = attitude_rate |
| 100 | + |
| 101 | + def pack(self): |
| 102 | + # Ensure all values are properly converted and match the expected types |
| 103 | + enable_flag_int = 1 if self.enable_flag else 0 # Correct conversion of boolean to integer |
| 104 | + yaw_control_flag_int = 1 if self.yaw_control_flag else 0 # Correct conversion of boolean to integer |
| 105 | + |
| 106 | + # Ensure the timestamp is an integer |
| 107 | + timestamp_int = int(self.timestamp) |
| 108 | + |
| 109 | + # Pack all data components |
| 110 | + try: |
| 111 | + payload = struct.pack( |
| 112 | + self.DATA_FORMAT, |
| 113 | + timestamp_int, |
| 114 | + self.setpoint_flags, |
| 115 | + enable_flag_int, |
| 116 | + yaw_control_flag_int, |
| 117 | + *self.position, |
| 118 | + *self.velocity, |
| 119 | + *self.acceleration, |
| 120 | + *self.attitude, |
| 121 | + *self.attitude_rate |
| 122 | + ) |
| 123 | + except struct.error as e: |
| 124 | + print(f"Struct error: {e}") |
| 125 | + print(f"Data being packed: {timestamp_int}, {self.setpoint_flags}, {enable_flag_int}, {yaw_control_flag_int}, {self.position}, {self.velocity}, {self.acceleration}, {self.attitude}, {self.attitude_rate}") |
| 126 | + raise |
| 127 | + |
| 128 | + crc = zlib.crc32(payload) & 0xffffffff |
| 129 | + return struct.pack(self.HEADER_FORMAT, self.HEADER) + payload + struct.pack(self.CRC_FORMAT, crc) |
| 130 | + |
| 131 | + |
| 132 | + @staticmethod |
| 133 | + def unpack(packet): |
| 134 | + header = struct.unpack(ControlPacket.HEADER_FORMAT, packet[:8])[0] |
| 135 | + if header != ControlPacket.HEADER: |
| 136 | + raise ValueError("Invalid packet header") |
| 137 | + payload, crc = packet[8:-4], packet[-4:] |
| 138 | + data = struct.unpack(ControlPacket.DATA_FORMAT, payload) |
| 139 | + if crc != struct.pack(ControlPacket.CRC_FORMAT, zlib.crc32(payload) & 0xffffffff): |
| 140 | + raise ValueError("CRC check failed") |
| 141 | + |
| 142 | + timestamp, setpoint_flags, enable_flag_int, yaw_control_flag_int = data[:4] |
| 143 | + position = data[4:7] |
| 144 | + velocity = data[7:10] |
| 145 | + acceleration = data[10:13] |
| 146 | + attitude = data[13:17] |
| 147 | + attitude_rate = data[17:21] |
| 148 | + |
| 149 | + # Convert integers back to booleans |
| 150 | + enable_flag = bool(enable_flag_int) |
| 151 | + yaw_control_flag = bool(yaw_control_flag_int) |
| 152 | + |
| 153 | + return ControlPacket(SetpointMode(setpoint_flags), enable_flag, yaw_control_flag, |
| 154 | + position, velocity, acceleration, attitude, attitude_rate, timestamp) |
| 155 | + |
| 156 | + def debug_print(self): |
| 157 | + print("Control Packet:") |
| 158 | + print(f"Timestamp: {self.timestamp}") |
| 159 | + mode_name = self.get_mode_name() |
| 160 | + yaw_control = "enabled" if self.yaw_control_flag else "initial yaw maintained" |
| 161 | + enable_mode = "enabled" if self.enable_flag else "disabled" |
| 162 | + print(f"Setpoint Flags: {bin(self.setpoint_flags)} ({mode_name})") |
| 163 | + print(f"Enable Flag: {enable_mode}") |
| 164 | + print(f"Yaw Control: {yaw_control}") |
| 165 | + print("Position: ", self.position) |
| 166 | + print("Velocity: ", self.velocity) |
| 167 | + print("Acceleration: ", self.acceleration) |
| 168 | + print("Attitude: ", self.attitude) |
| 169 | + print("Attitude Rate: ", self.attitude_rate) |
| 170 | + |
| 171 | + def get_mode_name(self): |
| 172 | + for mode in SetpointMode: |
| 173 | + if mode.value & self.setpoint_flags: |
| 174 | + return mode.name |
| 175 | + return "Unknown Mode" |
0 commit comments