Skydio add parachute protocol

assets/services/parachute/parachute.py

@@ -0,0 +1,182 @@
+import datetime
+import os
+import threading
+import time
+
+from pymavlink import mavutil
+from pymavlink.dialects.v20 import common as mavlink
+
+
+class ParachuteMavlinkUDPConnection:
+    """Class to handle parachute's MAVLink v2 UDP connections"""
+
+    def __init__(self, udpin_conn: str, udpout_conn: str, dialect: str) -> None:
+        """Initialize UDP connections"""
+        # Setting MAVLink version and dialect
+        os.environ["MAVLINK20"] = "1"
+        os.environ["MAVLINK_DIALECT"] = dialect
+        # Setting up UDP connections
+        self.udpin = mavutil.mavlink_connection(udpin_conn, dialect)
+        self.udpout = mavutil.mavlink_connection(udpout_conn, dialect)
+
+    def close(self):
+        """Close UDP connections"""
+        self.udpin.close()
+        self.udpout.close()
+
+
+class Parachute:
+    """Parachute class that uses MAVLink v2 UDP to broadcast heartbeat/status and handle commands"""
+
+    def __init__(
+        self,
+        dialect: str,
+        time_manufacture_s: int,
+        vendor_name: bytes,
+        model_name: bytes,
+        software_version: bytes,
+        hardware_version: bytes,
+        serial_number: bytes,
+        parachute_packed_date: bytes,
+    ) -> None:
+        # Initialize parachute info and status
+        self.info = mavlink.MAVLink_component_information_basic_message(
+            time_boot_ms=0,
+            capabilities=0,
+            time_manufacture_s=time_manufacture_s,
+            vendor_name=vendor_name,
+            model_name=model_name,
+            software_version=software_version,
+            hardware_version=hardware_version,
+            serial_number=serial_number,
+        )
+        self.status = mavlink.MAVLink_parachute_status_message(
+            time_boot_ms=0,
+            error_status=0,
+            arm_status=0,
+            deployment_status=mavlink.PARACHUTE_DEPLOYMENT_TRIGGER_NONE,
+            safety_status=mavlink.PARACHUTE_SAFETY_FLAGS_GROUND_CLEARED,
+            ats_arm_altitude=50,
+            parachute_packed_date=parachute_packed_date,
+        )
+        # Start parachute MAVLink UDP connections
+        self.mavlink_udp_connection = ParachuteMavlinkUDPConnection(
+            udpin_conn="udpin:localhost:14540",
+            udpout_conn="udpout:localhost:14541",
+            dialect=dialect,
+        )
+        # Start publishing heartbeat at 1Hz
+        self.heartbeat_thread_event = threading.Event()
+        heartbeat_thread = threading.Thread(
+            target=self.stream_heartbeat,
+            daemon=True,
+        )
+        heartbeat_thread.start()
+        self.heartbeat_thread_event.set()
+        # Start publish parachute status at 1Hz
+        self.parachute_status_thread_event = threading.Event()
+        parachute_status_thread = threading.Thread(
+            target=self.stream_parachute_status,
+            daemon=True,
+        )
+        parachute_status_thread.start()
+        self.parachute_status_thread_event.set()
+
+    def stream_heartbeat(self) -> None:
+        """Stream parachute heartbeat at 1Hz"""
+        while True:
+            self.heartbeat_thread_event.wait()
+            self.mavlink_udp_connection.udpout.mav.heartbeat_send(
+                mavlink.MAV_TYPE_PARACHUTE, mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0
+            )
+            time.sleep(1)
+
+    def stream_parachute_status(self) -> None:
+        """Stream parachute status at 1Hz"""
+        while True:
+            self.parachute_status_thread_event.wait()
+            self.mavlink_udp_connection.udpout.mav.send(self.status)
+            time.sleep(1)
+
+    def listen_for_commands(self) -> None:
+        """Listen for MAVLink commands"""
+        while True:
+            msg = self.mavlink_udp_connection.udpin.recv_match(type="COMMAND_LONG", blocking=True)
+            if msg:
+                self.handle_command_long(msg)
+
+    def handle_command_long(self, msg: mavlink.MAVLink_command_long_message) -> None:
+        """Handle MAVLink command long messages"""
+        if msg.command == mavlink.MAV_CMD_REQUEST_MESSAGE:
+            self.handle_command_request_message(msg)
+        elif msg.command == mavlink.MAV_CMD_DO_PARACHUTE:
+            self.handle_command_do_parachute(msg)
+        elif msg.command == mavlink.MAV_CMD_SET_PARACHUTE_ARM:
+            self.handle_command_set_parachute_arm(msg)
+        else:
+            self.mavlink_udp_connection.udpout.mav.command_ack_send(
+                msg.command, result=mavlink.MAV_RESULT_UNSUPPORTED
+            )
+
+    def handle_command_request_message(self, msg: mavlink.MAVLink_command_long_message) -> None:
+        """Handle MAVLink request message command"""
+        if msg.param1 == mavlink.MAVLINK_MSG_ID_PARACHUTE_STATUS:
+            self.mavlink_udp_connection.udpout.mav.command_ack_send(
+                msg.command, result=mavlink.MAV_RESULT_ACCEPTED
+            )
+            self.mavlink_udp_connection.udpout.mav.send(self.status)
+        elif msg.param1 == mavlink.MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC:
+            self.mavlink_udp_connection.udpout.mav.command_ack_send(
+                msg.command, result=mavlink.MAV_RESULT_ACCEPTED
+            )
+            self.mavlink_udp_connection.udpout.mav.send(self.info)
+        else:
+            self.mavlink_udp_connection.udpout.mav.command_ack_send(
+                msg.command, result=mavlink.MAV_RESULT_DENIED
+            )
+
+    def handle_command_do_parachute(self, msg: mavlink.MAVLink_command_long_message) -> None:
+        """Handle MAVLink do parachute command"""
+        result = mavlink.MAV_RESULT_DENIED
+        if msg.param1 == mavlink.PARACHUTE_RELEASE:
+            if self.status.arm_status & mavlink.PARACHUTE_TRIGGER_FLAGS_FC:
+                if self.status.safety_status & mavlink.PARACHUTE_SAFETY_FLAGS_GROUND_CLEARED:
+                    result = mavlink.MAV_RESULT_ACCEPTED
+                    self.status.deployment_status = mavlink.PARACHUTE_DEPLOYMENT_TRIGGER_DRONE
+                else:
+                    result = mavlink.MAV_RESULT_TEMPORARILY_REJECTED
+            else:
+                result = mavlink.MAV_RESULT_FAILED
+        self.mavlink_udp_connection.udpout.mav.command_ack_send(msg.command, result=result)
+
+    def handle_command_set_parachute_arm(self, msg: mavlink.MAVLink_command_long_message) -> None:
+        """Handle MAVLink set parachute arm command"""
+        arm_flags = int(msg.param1)
+        bitmask = int(msg.param2)
+        valid_parachute_trigger_flags = ((mavlink.PARACHUTE_TRIGGER_FLAGS_ENUM_END - 1) << 1) - 1
+        if arm_flags & ~valid_parachute_trigger_flags or bitmask & ~valid_parachute_trigger_flags:
+            self.mavlink_udp_connection.udpout.mav.command_ack_send(
+                msg.command, result=mavlink.MAV_RESULT_DENIED
+            )
+        else:
+            new_arm_status = self.status.arm_status & ~bitmask
+            new_arm_status |= arm_flags & bitmask
+            self.status.arm_status = new_arm_status
+            self.mavlink_udp_connection.udpout.mav.command_ack_send(
+                msg.command, result=mavlink.MAV_RESULT_ACCEPTED
+            )
+
+
+if __name__ == "__main__":
+    # Create parachute object and start listening for commands
+    parachute = Parachute(
+        dialect="common",
+        time_manufacture_s=int(time.time()),
+        vendor_name=b"Vendor",
+        model_name=b"Model",
+        software_version=b"1.2.3",
+        hardware_version=b"4.5.6",
+        serial_number=b"123456",
+        parachute_packed_date=datetime.date.today().strftime("%Y-%m-%d").encode(),
+    )
+    parachute.listen_for_commands()