mrtautopilot 8__tar.gz → 10__tar.gz

{mrtautopilot-8 → mrtautopilot-10}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: mrtautopilot
-Version: 8
+Version: 10
 Summary: Communicate with the MRT Autopilot Mavlink API
 Maintainer-email: Nathan Knotts <nathan@magothyrt.com>
 Project-URL: Homepage, https://www.magothyrt.com/

{mrtautopilot-8 → mrtautopilot-10}/mrtautopilot/__init__.py

@@ -1,4 +1,4 @@
-__version__ = "8"
+__version__ = "10"
 
 from .autopilot import *
 from .mission import *

{mrtautopilot-8 → mrtautopilot-10}/mrtautopilot/autopilot.py

@@ -3,12 +3,13 @@ import threading
 import multiprocessing
 import socket
 import asyncio
+import datetime
 import logging
 import struct
 import uuid
 from dataclasses import dataclass
 from enum import Enum
-from typing import List, Union, Tuple
+from typing import List, Union, Tuple, Callable
 
 import mrtmavlink
 from . import mission
@@ -267,6 +268,187 @@ class LowBandwidth:
     fault_response: Union[HealthResponse, None]
 
 
+class GpsFixType(Enum):
+    NoGps = 0
+    NoFix = 1
+    Fix2d = 2
+    Fix3d = 3
+    Dgps = 4
+    RtkFloat = 5
+    RtkFixed = 6
+    Static = 7
+    Ppp = 8
+
+
+@dataclass
+class Gps2Raw:
+    def __init__(self, msg: mrtmavlink.MAVLink_gps2_raw_message):
+        self.timestamp = datetime.datetime.fromtimestamp(
+            msg.time_usec / 1e6, tz=datetime.timezone.utc
+        )
+        self.fix_type = GpsFixType(msg.fix_type)
+
+        if msg.lat != 0x7FFFFFFF:
+            self.latitude_deg = msg.lat / 1e7
+        else:
+            self.latitude_deg = None
+
+        if msg.lon != 0x7FFFFFFF:
+            self.longitude_deg = msg.lon / 1e7
+        else:
+            self.longitude_deg = None
+
+        if msg.alt != 0x7FFFFFF:
+            self.altitude_m = msg.alt / 1000
+        else:
+            self.altitude_m = None
+
+        if msg.eph != 0xFFFF:
+            self.hdop = msg.eph / 100
+        else:
+            self.hdop = None
+
+        if msg.epv != 0xFFFF:
+            self.vdop = msg.epv / 100
+        else:
+            self.vdop = None
+
+        if msg.vel != 0xFFFF:
+            self.velocity_mps = msg.vel / 100
+        else:
+            self.velocity_mps = None
+
+        if msg.cog != 0xFFFF:
+            self.course_deg = msg.cog / 100
+        else:
+            self.course_deg = None
+
+        if msg.satellites_visible != 0xFF:
+            self.num_set = msg.satellites_visible
+        else:
+            self.num_set = None
+
+    timestamp: datetime.datetime
+    fix_type: GpsFixType
+    latitude_deg: Union[float, None]
+    longitude_deg: Union[float, None]
+    altitude_m: Union[float, None]
+    hdop: Union[float, None]
+    vdop: Union[float, None]
+    velocity_mps: Union[float, None]
+    course_deg: Union[float, None]
+    num_set: Union[int, None]
+
+
+@dataclass
+class ParamUint8:
+    value: int
+
+
+@dataclass
+class ParamInt8:
+    value: int
+
+
+@dataclass
+class ParamUint16:
+    value: int
+
+
+@dataclass
+class ParamInt16:
+    value: int
+
+
+@dataclass
+class ParamUint32:
+    value: int
+
+
+@dataclass
+class ParamInt32:
+    value: int
+
+
+@dataclass
+class ParamReal32:
+    value: float
+
+
+ParamValueType = Union[
+    ParamUint8,
+    ParamInt8,
+    ParamUint16,
+    ParamInt16,
+    ParamUint32,
+    ParamInt32,
+    ParamReal32,
+]
+
+
+@dataclass
+class ParamValue:
+    @staticmethod
+    def from_mavlink(msg: mrtmavlink.MAVLink_param_value_message):
+        buffer = struct.pack("<f", msg.param_value)
+        if msg.param_type == mrtmavlink.MAV_PARAM_TYPE_UINT8:
+            value = ParamUint8(int(struct.unpack("BBBB", buffer)[0]))
+        elif msg.param_type == mrtmavlink.MAV_PARAM_TYPE_INT8:
+            value = ParamInt8(int(struct.unpack("bBBB", buffer)[0]))
+        elif msg.param_type == mrtmavlink.MAV_PARAM_TYPE_UINT16:
+            value = ParamUint16(int(struct.unpack("<HBB", buffer)[0]))
+        elif msg.param_type == mrtmavlink.MAV_PARAM_TYPE_INT16:
+            value = ParamInt16(int(struct.unpack("<hBB", buffer)[0]))
+        elif msg.param_type == mrtmavlink.MAV_PARAM_TYPE_UINT32:
+            value = ParamUint32(int(struct.unpack("<I", buffer)[0]))
+        elif msg.param_type == mrtmavlink.MAV_PARAM_TYPE_INT32:
+            value = ParamInt32(int(struct.unpack("<i", buffer)[0]))
+        elif msg.param_type == mrtmavlink.MAV_PARAM_TYPE_REAL32:
+            value = ParamReal32(float(struct.unpack("<f", buffer)[0]))
+        else:
+            raise ValueError(f"Unsupported parameter type: {msg.param_type}")
+
+        return ParamValue(msg.param_id, value, msg.param_index, msg.param_count)
+
+    def to_mavlink(self) -> mrtmavlink.MAVLink_param_value_message:
+        if isinstance(self.value, ParamUint8):
+            typ = mrtmavlink.MAV_PARAM_TYPE_UINT8
+            value = struct.pack("BBBB", self.value.value, 0, 0, 0)
+        elif isinstance(self.value, ParamInt8):
+            typ = mrtmavlink.MAV_PARAM_TYPE_INT8
+            value = struct.pack("bBBB", self.value.value, 0, 0, 0)
+        elif isinstance(self.value, ParamUint16):
+            typ = mrtmavlink.MAV_PARAM_TYPE_UINT16
+            value = struct.pack("<HBB", self.value.value, 0, 0)
+        elif isinstance(self.value, ParamInt16):
+            typ = mrtmavlink.MAV_PARAM_TYPE_INT16
+            value = struct.pack("<hBB", self.value.value, 0, 0)
+        elif isinstance(self.value, ParamUint32):
+            typ = mrtmavlink.MAV_PARAM_TYPE_UINT32
+            value = struct.pack("<I", self.value.value)
+        elif isinstance(self.value, ParamInt32):
+            typ = mrtmavlink.MAV_PARAM_TYPE_INT32
+            value = struct.pack("<i", self.value.value)
+        elif isinstance(self.value, ParamReal32):
+            typ = mrtmavlink.MAV_PARAM_TYPE_REAL32
+            value = struct.pack("<f", self.value.value)
+        else:
+            raise ValueError(f"Unsupported parameter type: {type(self.value)}")
+
+        return mrtmavlink.MAVLink_param_value_message(
+            param_id=self.name.encode("utf-8"),
+            param_value=struct.unpack("f", value)[0],
+            param_type=typ,
+            param_index=self.param_idx,
+            param_count=self.num_param,
+        )
+
+    name: str
+    value: ParamValueType
+    param_idx: int
+    num_param: int
+
+
 class MavlinkThread:
     def __init__(
         self,
@@ -278,6 +460,7 @@ class MavlinkThread:
     ):
         self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
         self.sock.setblocking(False)
+        self.msg_callback: Callable[[mrtmavlink.MAVLink_message], None] = lambda x: None
 
         if multicast_group:
             self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
@@ -297,9 +480,12 @@ class MavlinkThread:
         self.remote_port = remote_port
         self.remote_address_list = [remote_address]
         if remote_address == "127.0.0.1":
-            self.remote_address_list.extend(
-                socket.gethostbyname_ex(socket.gethostname())[2]
-            )
+            try:
+                self.remote_address_list.extend(
+                    socket.gethostbyname_ex(socket.gethostname())[2]
+                )
+            except socket.gaierror:
+                logging.warning("Failed to get local IP address")
 
         self.is_started = False
         self.is_started_cv = threading.Condition()
@@ -324,6 +510,9 @@ class MavlinkThread:
         self.expected_cmd_uuid = 0
         self.ack_result: Union[int, None] = None
 
+        self.is_requesting_parameters = False
+        self.param_value: Union[ParamValue, None] = None
+
     def write(self, data: bytes):
         self.sock.sendto(data, (self.remote_address, self.remote_port))
 
@@ -363,15 +552,25 @@ class MavlinkThread:
             for msg in messages:
                 self._msg_callback(msg)
 
+    async def _notify_param_result(self, result: ParamValue):
+        async with self.param_cond:
+            self.param_result = result
+            self.param_cond.notify()
+
     async def _notify_ack_result(self, result: int):
         async with self.ack_cond:
             self.ack_result = result
             self.ack_cond.notify()
 
-    def _msg_callback(self, msg: mrtmavlink.MAVLink_message):
-        logging.debug(f"Received: {msg.msgname}, addr")
+    def register_message_callback(
+        self, callback: Callable[[mrtmavlink.MAVLink_message], None]
+    ):
+        self.msg_callback = callback
 
+    def _msg_callback(self, msg: mrtmavlink.MAVLink_message):
+        self.msg_callback(msg)
         sys_id = msg.get_srcSystem()
+        logging.debug(f"Received: {msg.msgname}, sys_id={sys_id}, {msg}")
         if self.system_id != sys_id:
             self.system_id = msg.get_srcSystem()
             logging.info(f"Setting System ID to {sys_id}")
@@ -411,6 +610,10 @@ class MavlinkThread:
             ):
                 logging.debug(f"Received Command Ack: {msg}")
                 self.loop.create_task(self._notify_ack_result(msg.result))
+        elif type(msg) is mrtmavlink.MAVLink_param_value_message:
+            param = ParamValue.from_mavlink(msg)
+            logging.debug(f"Received Param Value: {param}")
+            self.loop.create_task(self._notify_param_result(param))
 
     def send_heartbeat(self):
         self.loop.call_soon_threadsafe(lambda: self._send_heartbeat())
@@ -421,55 +624,150 @@ class MavlinkThread:
             mrtmavlink.MAV_TYPE_GCS, mrtmavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0
         )
 
+    def get_parameter(
+        self,
+        id: str,
+        callback: Callable[[Union[ParamValue, None]], None],
+    ):
+        self.loop.create_task(self._get_parameter(id, callback))
+
+    async def _get_parameter(
+        self,
+        id: str,
+        callback: Callable[[Union[ParamValue, None]], None],
+    ):
+        assert len(id) <= 16
+        if self.system_id is None:
+            logging.info("Failed to get parameter, system_id not set")
+            return
+
+        if self.is_requesting_parameters:
+            logging.warning("Already requesting parameters")
+            callback(None)
+            return
+
+        RETRY_PERIOD_S = 0.25
+        try:
+            self.is_requesting_parameters = True
+
+            logging.info(f"Getting Param {id}")
+
+            self.param_result = None
+            self.conn.param_request_read_send(
+                self.system_id,
+                mrtmavlink.MAV_COMP_ID_AUTOPILOT1,
+                id.encode("utf-8"),
+                -1,
+            )
+            try:
+                async with self.param_cond:
+                    await asyncio.wait_for(self.param_cond.wait(), RETRY_PERIOD_S)
+            except asyncio.TimeoutError:
+                logging.error("Timeout waiting for parameter ack")
+                callback(None)
+                return
+
+            if self.param_result is not None:
+                if self.param_result.name == id:
+                    callback(self.param_result)
+                else:
+                    logging.warning(
+                        f"Set parameter {id} but got {self.param_result.name}"
+                    )
+                    callback(None)
+            else:
+                logging.warning(f"Set parameter {id} but got None")
+                callback(None)
+        finally:
+            self.is_requesting_parameters = False
+
     def set_parameter(
         self,
         id: str,
-        value: float,
-        typ: int,
-        component: int = mrtmavlink.MAV_COMP_ID_AUTOPILOT1,
+        value: ParamValueType,
+        callback: Callable[[Union[ParamValue, None]], None] = lambda x: None,
     ):
-        self.loop.call_soon_threadsafe(
-            lambda: self._set_parameter(id, value, typ, component)
-        )
+        self.loop.create_task(self._set_parameter(id, value, callback))
 
-    def _set_parameter(self, id: str, value: float, typ: int, component: int):
+    async def _set_parameter(
+        self,
+        id: str,
+        value: ParamValueType,
+        callback: Callable[[Union[ParamValue, None]], None],
+    ):
         assert len(id) <= 16
-        if not self.system_id:
+        if self.system_id is None:
             logging.info("Failed to set parameter, system_id not set")
             return
 
-        logging.info(f"Setting Param {id} to {value}")
-
-        if typ == mrtmavlink.MAV_PARAM_TYPE_UINT8:
-            val_bytes = struct.pack("BBBB", int(value), 0, 0, 0)
-        elif typ == mrtmavlink.MAV_PARAM_TYPE_INT8:
-            val_bytes = struct.pack("bBBB", int(value), 0, 0, 0)
-        elif typ == mrtmavlink.MAV_PARAM_TYPE_UINT16:
-            val_bytes = struct.pack("<HBB", int(value), 0, 0)
-        elif typ == mrtmavlink.MAV_PARAM_TYPE_INT16:
-            val_bytes = struct.pack("<hBB", int(value), 0, 0)
-        elif typ == mrtmavlink.MAV_PARAM_TYPE_UINT32:
-            val_bytes = struct.pack("<I", int(value))
-        elif typ == mrtmavlink.MAV_PARAM_TYPE_INT32:
-            val_bytes = struct.pack("<i", int(value))
-        elif typ == mrtmavlink.MAV_PARAM_TYPE_REAL32:
-            val_bytes = struct.pack("<f", value)
-        else:
-            assert False, "unsupported type"
-
-        self.conn.param_set_send(
-            self.system_id,
-            component,
-            id.encode("utf-8"),
-            struct.unpack("f", val_bytes)[0],
-            typ,
-        )
+        if self.is_requesting_parameters:
+            logging.warning("Already requesting parameters")
+            callback(None)
+            return
+
+        RETRY_PERIOD_S = 0.25
+        try:
+            self.is_requesting_parameters = True
+
+            logging.info(f"Setting Param {id} to {value}")
+
+            if isinstance(value, ParamUint8):
+                val_bytes = struct.pack("BBBB", int(value.value), 0, 0, 0)
+                typ = mrtmavlink.MAV_PARAM_TYPE_UINT8
+            elif isinstance(value, ParamInt8):
+                val_bytes = struct.pack("bBBB", int(value.value), 0, 0, 0)
+                typ = mrtmavlink.MAV_PARAM_TYPE_INT8
+            elif isinstance(value, ParamUint16):
+                val_bytes = struct.pack("<HBB", int(value.value), 0, 0)
+                typ = mrtmavlink.MAV_PARAM_TYPE_UINT16
+            elif isinstance(value, ParamInt16):
+                val_bytes = struct.pack("<hBB", int(value.value), 0, 0)
+                typ = mrtmavlink.MAV_PARAM_TYPE_INT16
+            elif isinstance(value, ParamUint32):
+                val_bytes = struct.pack("<I", int(value.value))
+                typ = mrtmavlink.MAV_PARAM_TYPE_UINT32
+            elif isinstance(value, ParamInt32):
+                val_bytes = struct.pack("<i", int(value.value))
+                typ = mrtmavlink.MAV_PARAM_TYPE_INT32
+            elif isinstance(value, ParamReal32):
+                val_bytes = struct.pack("<f", value.value)
+                typ = mrtmavlink.MAV_PARAM_TYPE_REAL32
+            else:
+                assert False, "unsupported type"
+
+            self.param_result = None
+            self.conn.param_set_send(
+                self.system_id,
+                mrtmavlink.MAV_COMP_ID_AUTOPILOT1,
+                id.encode("utf-8"),
+                struct.unpack("f", val_bytes)[0],
+                typ,
+            )
+            try:
+                async with self.param_cond:
+                    await asyncio.wait_for(self.param_cond.wait(), RETRY_PERIOD_S)
+            except asyncio.TimeoutError:
+                logging.error("Timeout waiting for parameter ack")
+                callback(None)
+                return
+
+            if self.param_result is not None:
+                if self.param_result.name == id:
+                    callback(self.param_result)
+                else:
+                    logging.warning(
+                        f"Set parameter {id} but got {self.param_result.name}"
+                    )
+                    callback(None)
+            else:
+                logging.warning(f"Set parameter {id} but got None")
+                callback(None)
+        finally:
+            self.is_requesting_parameters = False
 
     def set_motor_enablement(self, enable: bool):
         logging.info(f"Setting Motor Enablement to {enable}")
-        self.set_parameter(
-            "MOTOR_ENABLE", 1 if enable else 0, mrtmavlink.MAV_PARAM_TYPE_UINT8
-        )
+        self.set_parameter("MOTOR_ENABLE", ParamUint8(1 if enable else 0))
 
     async def _run(self):
         with self.is_started_cv:
@@ -478,6 +776,7 @@ class MavlinkThread:
 
         self.loop = asyncio.get_running_loop()
         self.ack_cond = asyncio.Condition()
+        self.param_cond = asyncio.Condition()
         await self.loop.create_datagram_endpoint(lambda: self, sock=self.sock)  # type: ignore
 
         while not self.is_done:
@@ -491,11 +790,57 @@ class MavlinkThread:
     def _send_system_time(self):
         self.conn.system_time_send(int(time.time() * 1e6), 0)
 
+    def _send_param_request_list(self):
+        self.conn.param_request_list_send(
+            self.system_id,  # target_system
+            mrtmavlink.MAV_COMP_ID_AUTOPILOT1,  # target_component
+        )
+
+    def request_parameters(self, callback: Callable[[List[ParamValue]], None]):
+        self.loop.create_task(self._request_parameters(callback))
+
+    async def _request_parameters(self, callback: Callable[[List[ParamValue]], None]):
+        if self.system_id is None:
+            logging.warning("System ID not set, not requesting parameters")
+            return
+
+        if self.is_requesting_parameters:
+            logging.warning("Already requesting parameters")
+            return
+
+        RETRY_PERIOD_S = 0.25
+        params: List[ParamValue] = []
+        try:
+            self.is_requesting_parameters = True
+            self._send_param_request_list()
+            while True:
+                try:
+                    self.param_result = None
+                    async with self.param_cond:
+                        await asyncio.wait_for(self.param_cond.wait(), RETRY_PERIOD_S)
+
+                    if self.param_result is not None:
+                        params.append(self.param_result)
+                        if (
+                            self.param_result.param_idx
+                            == self.param_result.num_param - 1
+                        ):
+                            break
+                except asyncio.TimeoutError:
+                    logging.error("Timeout waiting for parameter")
+                    break
+
+        finally:
+            self.is_requesting_parameters = False
+            self.param_result = None
+
+        callback(params)
+
     def send_autopilot_stop(self):
         self.loop.call_soon_threadsafe(lambda: self._send_autopilot_stop())
 
     def _send_autopilot_stop(self):
-        if not self.system_id:
+        if self.system_id is None:
             logging.warning("System ID not set, not sending STOP")
             return
         logging.info("Sending STOP")
@@ -532,7 +877,7 @@ class MavlinkThread:
         )
 
     def _send_waypoint(self, lat_deg: float, lon_deg: float, speed_mps: float):
-        if not self.system_id:
+        if self.system_id is None:
             logging.warning("System ID not set, not sending waypoint")
             return
         logging.info(
@@ -559,7 +904,7 @@ class MavlinkThread:
         )
 
     def _send_loiter(self, speed_mps: float, radius_m: float, duration_s: float):
-        if not self.system_id:
+        if self.system_id is None:
             logging.warning("System ID not set, not sending loiter")
             return
         logging.info(
@@ -707,7 +1052,7 @@ class MavlinkThread:
             )
 
             for retry_num in range(max_retries):
-                logging.debug(f"Set mission index: ({retry_num+1}/{max_retries})")
+                logging.debug(f"Set mission index: ({retry_num + 1}/{max_retries})")
 
                 self.ack_result = None
                 self.conn.send(cmd)
@@ -732,3 +1077,42 @@ class MavlinkThread:
 
         finally:
             self.is_sending_mission = False
+
+    def set_attitude(
+        self,
+        roll_rad: float = 0,
+        pitch_rad: float = 0,
+        yaw_rad: float = 0,
+        roll_rate_rps: float = 0,
+        pitch_rate_rps: float = 0,
+        yaw_rate_rps: float = 0,
+    ):
+        self.loop.call_soon_threadsafe(
+            lambda: self._set_attitude(
+                roll_rad,
+                pitch_rad,
+                yaw_rad,
+                roll_rate_rps,
+                pitch_rate_rps,
+                yaw_rate_rps,
+            )
+        )
+
+    def _set_attitude(
+        self,
+        roll_rad: float,
+        pitch_rad: float,
+        yaw_rad: float,
+        roll_rate_rps: float,
+        pitch_rate_rps: float,
+        yaw_rate_rps: float,
+    ):
+        self.conn.attitude_send(
+            0,
+            roll_rad,
+            pitch_rad,
+            yaw_rad,
+            roll_rate_rps,
+            pitch_rate_rps,
+            yaw_rate_rps,
+        )

{mrtautopilot-8 → mrtautopilot-10}/mrtautopilot.egg-info/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: mrtautopilot
-Version: 8
+Version: 10
 Summary: Communicate with the MRT Autopilot Mavlink API
 Maintainer-email: Nathan Knotts <nathan@magothyrt.com>
 Project-URL: Homepage, https://www.magothyrt.com/