From 42b53b61a3ab51ef6f0d7b990efd55a42d3e32e4 Mon Sep 17 00:00:00 2001 From: HameedFawwaz Date: Fri, 12 Dec 2025 16:41:17 -0700 Subject: [PATCH 1/9] Refactored Navigator to initialize a connection object when Navigator is intialized --- mavctl/connect/conn.py | 1 - mavctl/messages/navigator.py | 11 +++++++++-- mavctl/tests/flight_tests/first_flight.py | 4 +--- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/mavctl/connect/conn.py b/mavctl/connect/conn.py index 44b80c7..47fd35a 100644 --- a/mavctl/connect/conn.py +++ b/mavctl/connect/conn.py @@ -4,7 +4,6 @@ import threading from pymavlink import mavutil from mavctl.connect.heartbeat import HeartbeatManager -from mavctl.messages.navigator import Navigator class Connect: diff --git a/mavctl/messages/navigator.py b/mavctl/messages/navigator.py index 08b984e..c5889ae 100644 --- a/mavctl/messages/navigator.py +++ b/mavctl/messages/navigator.py @@ -2,13 +2,20 @@ import time from mavctl.messages import util from mavctl.messages.location import LocationGlobal, LocationGlobalRelative, LocationLocal, Velocity +from mavctl.connect.conn import Connect from math import sqrt class Navigator: - def __init__(self, master): - self.master = master + def __init__(self, + ip: str = "udp:127.0.0.1:14551", + baud: int = 57600, + heartbeat_timeout = None): + + self.master = Connect(ip = ip, baud = baud, heartbeat_timeout = heartbeat_timeout).master + + def DO_NOT_USE_ARM(self): diff --git a/mavctl/tests/flight_tests/first_flight.py b/mavctl/tests/flight_tests/first_flight.py index 3c697c5..24a24ba 100644 --- a/mavctl/tests/flight_tests/first_flight.py +++ b/mavctl/tests/flight_tests/first_flight.py @@ -1,4 +1,3 @@ -from mavctl.connect.conn import Connect from pymavlink import mavutil from mavctl.messages.navigator import Navigator from mavctl.messages import advanced @@ -6,8 +5,7 @@ CONN_STR = "udp:127.0.0.1:14551" -connect = Connect(ip = CONN_STR) -drone = Navigator(connect.master) +drone = Navigator(ip = CONN_STR) while not drone.wait_for_mode_and_arm(): pass From 7d083f2edfce63cca1cc6b1fa5564350755bc404 Mon Sep 17 00:00:00 2001 From: HameedFawwaz Date: Fri, 12 Dec 2025 17:14:31 -0700 Subject: [PATCH 2/9] Updated requirements.txt --- requirements.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/requirements.txt b/requirements.txt index 1ceb0ed..2e983ca 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1 +1,3 @@ pymavlink==2.4.49 +pylint +mypy-extensions From e31cbe7db2bfd6a82a0ce9ec9fc41856b27f55f1 Mon Sep 17 00:00:00 2001 From: HameedFawwaz Date: Fri, 12 Dec 2025 21:56:06 -0700 Subject: [PATCH 3/9] Major Refactoring, created logging infrastructure and made everything pass linter checks --- .pylintrc | 32 + {mavctl/bin => bin}/run-nav.sh | 0 mavctl/__init__.py | 7 + mavctl/_internal/logger.py | 18 + mavctl/_internal/util.py | 81 ++ mavctl/connect/conn.py | 123 +-- mavctl/connect/heartbeat.py | 4 +- mavctl/exceptions.py | 9 + mavctl/messages/advanced.py | 26 - mavctl/messages/location.py | 83 +- mavctl/messages/messenger.py | 57 +- mavctl/messages/navigator.py | 739 +++++++----------- mavctl/messages/util.py | 77 -- {mavctl/tests => tests}/flight_tests/air.py | 0 {mavctl/tests => tests}/flight_tests/auto.py | 0 .../flight_tests/first_flight.py | 5 +- {mavctl/tests => tests}/flight_tests/plane.py | 0 {mavctl/tests => tests}/flight_tests/quad.py | 0 .../flight_tests/swarm/copter1.py | 0 .../flight_tests/swarm/copter2.py | 0 .../tests => tests}/flight_tests/waypoint.py | 0 {mavctl/tests => tests}/msg_test.py | 0 22 files changed, 595 insertions(+), 666 deletions(-) create mode 100644 .pylintrc rename {mavctl/bin => bin}/run-nav.sh (100%) create mode 100644 mavctl/__init__.py create mode 100644 mavctl/_internal/logger.py create mode 100644 mavctl/_internal/util.py create mode 100644 mavctl/exceptions.py delete mode 100644 mavctl/messages/advanced.py delete mode 100644 mavctl/messages/util.py rename {mavctl/tests => tests}/flight_tests/air.py (100%) rename {mavctl/tests => tests}/flight_tests/auto.py (100%) rename {mavctl/tests => tests}/flight_tests/first_flight.py (59%) rename {mavctl/tests => tests}/flight_tests/plane.py (100%) rename {mavctl/tests => tests}/flight_tests/quad.py (100%) rename {mavctl/tests => tests}/flight_tests/swarm/copter1.py (100%) rename {mavctl/tests => tests}/flight_tests/swarm/copter2.py (100%) rename {mavctl/tests => tests}/flight_tests/waypoint.py (100%) rename {mavctl/tests => tests}/msg_test.py (100%) diff --git a/.pylintrc b/.pylintrc new file mode 100644 index 0000000..c411285 --- /dev/null +++ b/.pylintrc @@ -0,0 +1,32 @@ +[MASTER] +ignore=tests,bin + +[MESSAGES CONTROL] +# Disable warnings that are common in pymavlink / high-level API +disable= + missing-module-docstring, + missing-function-docstring, + missing-class-docstring, + invalid-name, + broad-except, + attribute-defined-outside-init, + protected-access, + R0902 + +[TYPECHECK] +ignored-modules= + pymavlink + pymavlink.* + +[BASIC] +good-names=i,j,k, + x,y,z, + lat,lon,alt, + msg,mav,seq + +[DESIGN] +max-args=10 +max-locals=20 +max-branches=15 +max-statements=60 +min-public-methods=0 diff --git a/mavctl/bin/run-nav.sh b/bin/run-nav.sh similarity index 100% rename from mavctl/bin/run-nav.sh rename to bin/run-nav.sh diff --git a/mavctl/__init__.py b/mavctl/__init__.py new file mode 100644 index 0000000..6fda89f --- /dev/null +++ b/mavctl/__init__.py @@ -0,0 +1,7 @@ +from .messages.navigator import Navigator + +__all__ = [ + "Navigator" +] + +__version__ = "0.0.1" diff --git a/mavctl/_internal/logger.py b/mavctl/_internal/logger.py new file mode 100644 index 0000000..2290b47 --- /dev/null +++ b/mavctl/_internal/logger.py @@ -0,0 +1,18 @@ +import logging + +def get_logger(name: str, level=logging.DEBUG) -> logging.Logger: + """Returns a configured logger instance.""" + logger = logging.getLogger(name) + logger.setLevel(level) + + if not logger.handlers: + ch = logging.StreamHandler() + ch.setLevel(level) + formatter = logging.Formatter( + fmt="%(asctime)s [%(levelname)s] %(name)s: %(message)s", + datefmt="%Y-%m-%d %H:%M:%S" + ) + ch.setFormatter(formatter) + logger.addHandler(ch) + + return logger diff --git a/mavctl/_internal/util.py b/mavctl/_internal/util.py new file mode 100644 index 0000000..4afffd7 --- /dev/null +++ b/mavctl/_internal/util.py @@ -0,0 +1,81 @@ +from math import radians, degrees, cos, sin, atan2, sqrt +from mavctl.messages.location import LocationLocal + + +def distance_to_target(target1, target2) -> LocationLocal: + """ + Returns the distance vector to the target. + + Args: + target1: Current Position of Vehicle (LocationLocal) + target2: Target Position (LocationLocal) + """ + dx = target2.north - target1.north + dy = target2.east - target1.east + dz = target2.down - target1.down + + return LocationLocal(dx, dy, dz) + + +def check_target_reached(target1, target2, tolerance: float = 0.5) -> bool: + """ + Checks if the vehicle has reached the target within a certain tolerance. + + Args: + target1: Current Position of the vehicle (LocationLocal) + target2: Target Position (LocationLocal) + tolerance: Distance tolerance in meters + """ + delta = distance_to_target(target1, target2) + delta_length = sqrt(delta.north ** 2 + delta.east ** 2 + delta.down ** 2) + + return delta_length < tolerance + + +def LatLon_to_Distance(point, origin) -> float: + """ + Computes the distance between two global coordinates using Haversine formula. + + Args: + point: Target LocationGlobal + origin: Origin LocationGlobal + + Returns: + Distance in meters + """ + R = 6378137 # Earth radius in meters + + origin_lat_rad = radians(origin.lat) + origin_lon_rad = radians(origin.lon) + point_lat_rad = radians(point.lat) + point_lon_rad = radians(point.lon) + + delta_lat = point_lat_rad - origin_lat_rad + delta_lon = point_lon_rad - origin_lon_rad + + a = sin(delta_lat / 2) ** 2 + cos(origin_lat_rad) * cos(point_lat_rad) * sin(delta_lon / 2) ** 2 + c = 2 * atan2(sqrt(a), sqrt(1 - a)) + + return R * c + + +def Heading(point1, point2) -> float: + """ + Computes the heading from point1 to point2 in degrees (0-360). + + Args: + point1: Start LocationGlobal + point2: End LocationGlobal + + Returns: + Heading in degrees + """ + lat1_rad = radians(point1.lat) + lat2_rad = radians(point2.lat) + delta_lon = radians(point2.lon - point1.lon) + + x = sin(delta_lon) * cos(lat2_rad) + y = cos(lat1_rad) * sin(lat2_rad) - sin(lat1_rad) * cos(lat2_rad) * cos(delta_lon) + + angle = atan2(x, y) + return (degrees(angle) + 360) % 360 diff --git a/mavctl/connect/conn.py b/mavctl/connect/conn.py index 47fd35a..d97d25d 100644 --- a/mavctl/connect/conn.py +++ b/mavctl/connect/conn.py @@ -1,89 +1,94 @@ - -from pymavlink import mavutil import time -import threading +from typing import Optional + from pymavlink import mavutil + from mavctl.connect.heartbeat import HeartbeatManager +from mavctl.exceptions import ConnError +from mavctl._internal.logger import get_logger + +LOGGER = get_logger(__name__) class Connect: - - def __init__(self, + """Handles MAVLink connections and heartbeat monitors""" + + def __init__(self, ip: str = "udp:127.0.0.1:14552", baud: int = 57600, - heartbeat_timeout = None): + heartbeat_timeout: Optional[int] = None): + """ + Initialize a MAVLink connection and run the heartbeat manager on the connection + + Args: + ip: Connection String + baud: Baud rate + heartbeat_timeout: Optional timeout in seconds for waiting for heartbeats + """ self.master = self.connect(ip = ip, baud = baud, heartbeat_timeout = heartbeat_timeout) - self._heartbeat_manager = HeartbeatManager(self.master) self._heartbeat_manager.start() - - def send_heartbeat(self, interval: int = 0.25): + self.ip = ip - try: - while True: - self.master.mav.heartbeat_send( - type=mavutil.mavlink.MAV_TYPE_GCS, - autopilot=mavutil.mavlink.MAV_AUTOPILOT_INVALID, - base_mode=0, - custom_mode=0, - system_status=mavutil.mavlink.MAV_STATE_ACTIVE - ) - time.sleep(interval) + def connect(self, ip: str = "udp:127.0.0.1:14552", + baud: int = 57600, + heartbeat_timeout: Optional[int] = None) -> mavutil.mavlink_connection: + """ + Function to connect to the MAVLink device - except Exception as e: - raise Exception("MAVCTL ERROR: Failed to send heartbeat!: ", e) - - def recv_heartbeat(self, interval: int = 1): - - try: - while True: - msg_recv = self.master.recv_match(type="HEARTBEAT", blocking=False) + Args: + ip: Connection String + baud: Baud rate + heartbeat_timeout: Optional timeout in seconds for waiting for heartbeats - # Create disconnect handling here... - time.sleep(interval) + Returns: + A mavutil.mavlink_connection instance - except Exception as e: - raise Exception("MAVCTL ERROR: Failed to send heartbeat!: ", e) - - def heartbeat_start(self): - self.send_heartbeat_thread.start() - self.recv_heartbeat_thread.start() - - def heartbeat_kill(self): - self._stop_event.set() - self.send_heartbeat_thread.join(timeout=2) - self.recv_heartbeat_thread.join(timeout=2) - - def disconnect(self): - self.heartbeat_kill() - self.master.close() - print("MAVCTL: Disconnecting!") - - # Function to connect to mavlink - def connect(self, ip: str = "udp:127.0.0.1:14552", # Default IP and port for the UAARG Autopilot Simulator - baud: int = 57600, # Default Baud Rate for the PixHawk, Add support for this later - #source_system = 255, # Add support for this later - #source_component = 0, # Add support for this later - heartbeat_timeout = None # Automatically assume that there is no timeout - ): + Raises: + ConnError: If heartbeat or the connection fails + """ try: - # NOTE: Using default parameters built into pymavlink, look into how this can be customized master = mavutil.mavlink_connection(ip, baud) msg_recv = master.recv_match(type="HEARTBEAT", blocking=False) while not msg_recv: i = 0 - if heartbeat_timeout != None: - if heartbeat_timeout > 0: + if heartbeat_timeout is not None: + if heartbeat_timeout > 0: i += 1 if heartbeat_timeout % 5: print("MAVCTL: Waiting for heartbeat ...") elif heartbeat_timeout == 0: - raise Exception("MAVCTL ERROR: Heartbeat not found!") + raise ConnError("MAVCTL ERROR: Heartbeat not found!") print("MAVCTL: Waiting for heartbeat ...") msg_recv = master.recv_match(type="HEARTBEAT", blocking=False) time.sleep(1) - print("MAVCTL: Connected at: ", ip) + LOGGER.info("MAVCTL: Connected at %s", ip) return master + + except Exception as e: + raise ConnError("MAVCTL ERROR: Failed to receive heartbeat!: ", e) from e + + def disconnect(self, timeout: float = 5, device: Optional[str] = None) -> bool: + """Graceful Disconnect from the MAVLink Device + + Returns: + Bool saying whether or not the disconnect was successful + + Raises ConnErorr if disconnect fails""" + + LOGGER.info("MAVCTL: Disconnecting ...") + try: + + self._heartbeat_manager.stop() + start_time = time.monotonic() + while self._heartbeat_manager.get_connection_status(): + if (time.monotonic() - start_time) > timeout: + LOGGER.warning("MAVCTL: Heartbeat manager did not stop within timeout") + return False + self.master.close() + LOGGER.info("MAVCTL: Successfully Disconnected from %s", device) + return True + except Exception as e: - raise Exception("MAVCTL ERROR: Failed to receive heartbeat!: ", e) + raise ConnError("MAVCTL ERROR: Disconnect failed", e) from e diff --git a/mavctl/connect/heartbeat.py b/mavctl/connect/heartbeat.py index 0ad863b..4d770c6 100644 --- a/mavctl/connect/heartbeat.py +++ b/mavctl/connect/heartbeat.py @@ -105,8 +105,8 @@ def _send_heartbeat(self): base_mode=0, custom_mode=0, system_status=mavutil.mavlink.MAV_STATE_ACTIVE - ) - time.sleep(self.heartbeat_timeout) + ) + time.sleep(self.heartbeat_timeout) def get_connection_status(self) -> bool: """Get the current connection status.""" diff --git a/mavctl/exceptions.py b/mavctl/exceptions.py new file mode 100644 index 0000000..2d40a4a --- /dev/null +++ b/mavctl/exceptions.py @@ -0,0 +1,9 @@ +""" +Used to create specific exceptions to help with debugging +""" + +class MAVCTLError(Exception): + """Base exception for all mavctl errors""" + +class ConnError(MAVCTLError): + """Exception to handle Connection Errors""" diff --git a/mavctl/messages/advanced.py b/mavctl/messages/advanced.py deleted file mode 100644 index 5db7f54..0000000 --- a/mavctl/messages/advanced.py +++ /dev/null @@ -1,26 +0,0 @@ -import time -from mavctl.messages import util -from mavctl.messages.location import LocationGlobal, LocationGlobalRelative, LocationLocal -from math import radians, atan -from mavctl.messages.util import Heading, LatLon_to_Distance - -# This file is meant for implementing more advanced features in mavctl - -# This tests the wait target reached and has been verified to work - -def simple_goto_local(master, x, y, z): - - # Mimics simple_goto from dronekit, includes the right yaw angle so that the drone faces where it needs to go. - - type_mask = master.generate_typemask([0, 1, 2, 9]) - angle = atan(y/x) - master.set_position_local_ned(type_mask = type_mask, x = x, y = y, z = -z, yaw = angle) - master.wait_target_reached(LocationLocal(x, y, -z)) - -def simple_goto_global(master, lat, lon, alt): - type_mask = master.generate_typemask([0, 1, 2, 9]) - start_point = master.get_global_position() - heading = Heading(start_point, LocationGlobal(lat, lon, alt)) - master.set_position_global(type_mask = type_mask, lon = lon, lat = lat, alt = alt, yaw = 0) - origin = master.get_global_origin() - master.wait_target_reached_global(LocationGlobal(lat, lon, alt)) diff --git a/mavctl/messages/location.py b/mavctl/messages/location.py index 36f2aaf..b020cf7 100644 --- a/mavctl/messages/location.py +++ b/mavctl/messages/location.py @@ -1,51 +1,42 @@ -from pymavlink import mavutil +from dataclasses import dataclass from math import sqrt - - -''' -Classes adapted from here: -https://github.com/dronekit/dronekit-python/blob/master/dronekit/__init__.py - -''' - -class LocationGlobal(object): - def __init__(self, lat, lon, alt=None): - self.lat = lat - self.lon = lon - self.alt = alt - -class LocationGlobalRelative(object): - def __init__(self, lat, lon, alt=None): - self.lat = lat - self.lon = lon - self.alt = alt - -class LocationLocal(object): - def __init__(self, north, east, down): - self.north = north - self.east = east - self.down = down - - -class Battery(object): - - def __init__(self, voltage, current, level): +from typing import Optional + +@dataclass +class LocationGlobal: + lat: float + lon: float + alt: Optional[float] = None + +@dataclass +class LocationGlobalRelative: + lat: float + lon: float + alt: Optional[float] = None + +@dataclass +class LocationLocal: + north: float + east: float + down: float + +@dataclass +class Battery: + voltage: float # in Volts + current: Optional[float] # in Amps + level: Optional[int] # battery level % + + def __init__(self, voltage: float, current: int, level: int): self.voltage = voltage / 1000.0 - if current == -1: - self.current = None - else: - self.current = current / 100.0 - - if level == -1: - self.level = None - else: - self.level = level + self.current = None if current == -1 else current / 100.0 + self.level = None if level == -1 else level -class Velocity(object): - def __init__(self, north, east, down): - self.north = north - self.east = east - self.down = down +@dataclass +class Velocity: + north: float + east: float + down: float - def magnitude(self): + def magnitude(self) -> float: + """Compute the 3D magnitude of the velocity vector.""" return sqrt(self.north ** 2 + self.east ** 2 + self.down ** 2) diff --git a/mavctl/messages/messenger.py b/mavctl/messages/messenger.py index 4ce21af..01c1377 100644 --- a/mavctl/messages/messenger.py +++ b/mavctl/messages/messenger.py @@ -1,31 +1,54 @@ -from pymavlink import mavutil +import logging import pymavlink.dialects.v20.all as dialect +from mavctl.connect.conn import Connect + +LOGGER = logging.getLogger(__name__) +LOGGER.setLevel(logging.DEBUG) + +if not LOGGER.handlers: + ch = logging.StreamHandler() + ch.setLevel(logging.DEBUG) + formatter = logging.Formatter( + fmt="%(asctime)s [%(levelname)s] %(name)s: %(message)s", + datefmt="%Y-%m-%d %H:%M:%S" + ) + ch.setFormatter(formatter) + LOGGER.addHandler(ch) class Messenger: """ Messenger class for sending messages from the autopilot to the ground station. - Source: - https://github.com/mustafa-gokce/ardupilot-software-development/blob/main/pymavlink/send-status-text.py + Maintains a separate MAVLink connection for sending status text messages. """ - def __init__(self, port): - self.__master = mavutil.mavlink_connection( - device=f"udp:127.0.0.1:{port}", - source_system=1, - source_component=1) + def __init__(self, ip: str = "udp:127.0.0.1:14553") -> None: + """ + Initialize a MAVLink connection. + + Args: + ip: Connection string to connect to, must be different than the drone connection. + """ + LOGGER.info("MAVCTL: Messenger connecting to %s", ip) + # Explicitly define baud and heartbeat_timeout here + self.master = Connect(ip=ip, baud=57600, heartbeat_timeout=5).master + LOGGER.info("MAVCTL: Messenger connected to %s", ip) - def send(self, message, prefix="SHEPARD"): + def send(self, message: str, prefix: str = "MAVCTL") -> None: """ Sends a message to the ground station. - :param message: The message to send. - :param prefix: The prefix to add to the message. - :return: None + Args: + message: The message to be sent. + prefix: The device it's coming from (the name of the drone or the software). """ - - message = f"{prefix}: {message}" - mav_message = dialect.MAVLink_statustext_message( - severity=dialect.MAV_SEVERITY_INFO, text=message.encode("utf-8")) - self.__master.mav.send(mav_message) + try: + full_message = f"{prefix}: {message}" + mav_message = dialect.MAVLink_statustext_message( + severity=dialect.MAV_SEVERITY_INFO, + text=full_message.encode("utf-8") + ) + self.master.mav.send(mav_message) + except Exception as e: + LOGGER.error("Messenger failed to send message: %s", e) diff --git a/mavctl/messages/navigator.py b/mavctl/messages/navigator.py index c5889ae..2d49081 100644 --- a/mavctl/messages/navigator.py +++ b/mavctl/messages/navigator.py @@ -1,488 +1,355 @@ -from pymavlink import mavutil import time -from mavctl.messages import util -from mavctl.messages.location import LocationGlobal, LocationGlobalRelative, LocationLocal, Velocity -from mavctl.connect.conn import Connect -from math import sqrt - +from math import atan +from dataclasses import dataclass +from typing import Optional -class Navigator: +from pymavlink import mavutil - def __init__(self, - ip: str = "udp:127.0.0.1:14551", +from mavctl._internal import util +from mavctl.messages.location import LocationGlobal, LocationLocal, Velocity +from mavctl.connect.conn import Connect +from mavctl._internal.logger import get_logger + +# Initialize the Logger +LOGGER = get_logger(__name__) + +# ---------------------- +# Dataclasses for setpoints +# ---------------------- +@dataclass +class PositionSetpointLocal: + x: float = 0 + y: float = 0 + z: float = 0 + vx: float = 0 + vy: float = 0 + vz: float = 0 + afx: float = 0 + afy: float = 0 + afz: float = 0 + yaw: float = 0 + yaw_rate: float = 0 + + +@dataclass +class PositionSetpointGlobal: + lat: float = 0 + lon: float = 0 + alt: float = 0 + vx: float = 0 + vy: float = 0 + vz: float = 0 + afx: float = 0 + afy: float = 0 + afz: float = 0 + yaw: float = 0 + yaw_rate: float = 0 + + +# ---------------------- +# Navigator Class +# ---------------------- +class Navigator: # pylint: disable=too-many-public-methods,too-many-instance-attributes + """Navigator class for MAVLink drones with logging and helper functions.""" + + def __init__(self, ip: str = "udp:127.0.0.1:14551", baud: int = 57600, - heartbeat_timeout = None): - - self.master = Connect(ip = ip, baud = baud, heartbeat_timeout = heartbeat_timeout).master - - - - - def DO_NOT_USE_ARM(self): + heartbeat_timeout: Optional[int] = None): """ - Arms the drone. - NOTE: UNDER NO CIRCUMSTANCE SHOULD THIS BE USED IN A REAL FLIGHT SCRIPT - NOTE: ONLY FOR USE IN SIMULATED SCRIPTS - """ - - self.master.mav.command_long_send( - self.master.target_system, - self.master.target_component, - mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 0, - 1, - 0, - 0, - 0, - 0, - 0, - 0 - ) + Initialize a MAVLink connection. - print("MAVCTL: Waiting for vehicle to arm") - self.master.motors_armed_wait() - print("MAVCTL: Armed!") - - def wait_vehicle_armed(self): - """ - Waits for the vehicle to be armed. See samples directory for examples + Args: + ip: Connection string (UDP/serial). + baud: Serial baud rate (ignored for UDP). + heartbeat_timeout: Optional heartbeat timeout. """ - print("MAVCTL: Waiting for vehicle to arm") + self.master = Connect(ip=ip, baud=baud, heartbeat_timeout=heartbeat_timeout).master + LOGGER.info("Navigator connected to %s", ip) + + # ---------------------- + # Arm / Disarm + # ---------------------- + def arm(self) -> None: + """Arms the vehicle (simulation only).""" + self._send_command_arm_disarm(True) + LOGGER.info("Waiting for vehicle to arm...") self.master.motors_armed_wait() - print("Armed!") - - def wait_for_mode_and_arm(self, mode="GUIDED", timeout=None) -> bool: - """Wait for the vehicle to enter ``mode`` and to be armed""" - mode_ready = self.set_mode_wait(mode=mode, timeout=timeout) - if not mode_ready: - return False - while not self.wait_vehicle_armed(): - return True - pass - - def DO_NOT_USE_DISARM(self): - """ - Disarms the drone. - NOTE: UNDER NO CIRCUMSTANCE SHOULD THIS BE USED IN A REAL FLIGHT SCRIPT - NOTE: ONLY FOR USE IN SIMULATED SCRIPTS - """ + LOGGER.info("Vehicle armed.") + + def disarm(self) -> None: + """Disarms the vehicle (simulation only).""" + self._send_command_arm_disarm(False) + LOGGER.info("Waiting for vehicle to disarm...") + self.master.motors_disarmed_wait() + LOGGER.info("Vehicle disarmed.") + def _send_command_arm_disarm(self, arm: bool) -> None: + """Helper to arm/disarm vehicle.""" self.master.mav.command_long_send( self.master.target_system, self.master.target_component, mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0 - ) - - print("MAVCTL: Waiting for vehicle to disarm") - self.master.motors_disarmed_wait() - print("MAVCTL: Disarmed!") - - def DO_NOT_USE_SET_MODE(self, mode = "GUIDED"): - """ - Sets the mode of the drone + int(arm), + 0, 0, 0, 0, 0, 0 + ) - mode: Mode to be set to + # ---------------------- + # Mode management + # ---------------------- + def set_mode_wait(self, mode: str = "GUIDED", timeout: Optional[int] = None) -> bool: """ - mode_mapping = self.master.mode_mapping() - if mode not in mode_mapping: - raise ValueError("MAVCTL Error: Mode " + mode + "not recognized") + Wait until the vehicle is in the specified mode. - mode_id = mode_mapping[mode] + Args: + mode: Target mode (e.g., "GUIDED"). + timeout: Maximum wait time in seconds. - self.master.mav.command_long_send( - self.master.target_system, - self.master.target_component, - mavutil.mavlink.MAV_CMD_DO_SET_MODE, - 0, - 1, - mode_id, - 0, - 0, - 0, - 0, - 0) - - - def set_mode_wait(self, mode = "GUIDED", timeout = None) -> bool: + Returns: + True if mode is set, False if timed out. """ - Waits for the specified mode to be selected. - Timesout after specified timeout - - mode: Mode you want to wait for, leave blank for the most part - timeout: Timeout in seconds, leave blank to have no timeout - """ start_time = time.time() - mode_mapping = self.master.mode_mapping() if mode not in mode_mapping: - raise ValueError("MAVCTL Error: Mode " + mode + "not recognized") - + raise ValueError(f"Mode {mode} not recognized") mode_id = mode_mapping[mode] - print("MAVCTL: Waiting for " + mode + " mode") + LOGGER.info("Waiting for mode %s", mode) + while True: - if timeout: - if time.time() - start_time >= timeout: - print("MAVCTL: Timeout waiting for " + mode + " mode") - return False + if timeout and (time.time() - start_time) >= timeout: + LOGGER.warning("Timeout waiting for mode %s", mode) + return False self.master.mav.request_data_stream_send( - self.master.target_system, - self.master.target_component, - mavutil.mavlink.MAV_DATA_STREAM_ALL, - 1, - 1) - - msg = self.master.recv_match(type = "HEARTBEAT", blocking = True, timeout = 0.25) - - if msg: - current_mode_id = msg.custom_mode - if current_mode_id == mode_id: - print("MAVCTL: Set to " + mode + " mode") - return True - - def get_global_position(self): - """ - Gets Global Position of drone. - returns lat, lon, and altitude with respect to GLOBAL coordinates - Altitude is a value which is measured from sea level and is positive - - """ - msg = self.master.recv_match(type='GLOBAL_POSITION_INT', blocking=True) - if msg: - lat = msg.lat / 1e7 - lon = msg.lon / 1e7 - alt = msg.alt / 1000 - - return LocationGlobal(lat, lon, alt) - - def get_local_position(self): - """ - Gets Local Position of drone. - returns x, y, z in NED coordinates with respect to local origin - DOWN is positive - """ - - msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=True) + self.master.target_system, + self.master.target_component, + mavutil.mavlink.MAV_DATA_STREAM_ALL, + 1, + 1 + ) + msg = self.master.recv_match(type="HEARTBEAT", blocking=True, timeout=0.25) + if msg and getattr(msg, "custom_mode", None) == mode_id: + LOGGER.info("Mode set to %s", mode) + return True + + # ---------------------- + # Position / Velocity access + # ---------------------- + def get_global_position(self) -> LocationGlobal: + """Returns the current global position.""" + msg = self.master.recv_match(type="GLOBAL_POSITION_INT", blocking=True) if msg: - north = msg.x - east = msg.y - down = msg.z + return LocationGlobal(msg.lat / 1e7, msg.lon / 1e7, msg.alt / 1000) + return LocationGlobal(0, 0, 0) - return LocationLocal(north, east, down) - - def get_global_origin(self): - """ - Gets the local origin of the drone (local position [0, 0, 0]) in terms of lat, lon and alt - """ - self.master.mav.command_long_send( - self.master.target_system, - self.master.target_component, - mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE, - 0, - 242, - 0, - 0, - 0, - 0, - 0, - 0) - - msg = self.master.recv_match(type='HOME_POSITION', blocking=True) - print(msg) + def get_local_position(self) -> LocationLocal: + """Returns the current local NED position.""" + msg = self.master.recv_match(type="LOCAL_POSITION_NED", blocking=True) if msg: - lat = msg.latitude / 1e7 - lon = msg.longitude / 1e7 - alt = msg.altitude / 1000 + return LocationLocal(msg.x, msg.y, msg.z) + return LocationLocal(0, 0, 0) - return LocationGlobal(lat, lon, alt) - - def get_velocity(self): - """ - Gets velocity of drone in NED coordinates - """ - msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=True) + def get_velocity(self) -> Velocity: + """Returns the current local velocity in NED.""" + msg = self.master.recv_match(type="LOCAL_POSITION_NED", blocking=True) if msg: - north = msg.vx - east = msg.vy - down = msg.vz - - return Velocity(north, east, down) - - - - def takeoff(self, altitude, pitch=15): - """ - Initiates a takeoff the drone to target altiude + return Velocity(msg.vx, msg.vy, msg.vz) + return Velocity(0, 0, 0) + + # ---------------------- + # Takeoff + # ---------------------- + def takeoff(self, altitude: float, pitch: float = 15) -> None: + """Initiates a takeoff to the target altitude.""" + LOGGER.info("Takeoff to %.2f m", altitude) + self.master.mav.command_long_send( + self.master.target_system, + self.master.target_component, + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + 0, + pitch, + 0, 0, 0, 0, 0, + altitude + ) + self.wait_target_reached(PositionSetpointLocal(z=-altitude)) + + # ---------------------- + # Setpoints + # ---------------------- + def set_position_local_ned(self, setpoint: PositionSetpointLocal, + coordinate_frame: int = mavutil.mavlink.MAV_FRAME_LOCAL_NED, + type_mask: int = 0x07FF) -> None: + """Send a local NED setpoint.""" + self.master.mav.set_position_target_local_ned_send( + 0, + self.master.target_system, + self.master.target_component, + coordinate_frame, + type_mask, + setpoint.x, setpoint.y, setpoint.z, + setpoint.vx, setpoint.vy, setpoint.vz, + setpoint.afx, setpoint.afy, setpoint.afz, + setpoint.yaw, setpoint.yaw_rate + ) + LOGGER.debug("Local setpoint sent: %s", setpoint) - Altitude: Altituide to take off to in meters + def set_position_global(self, setpoint: PositionSetpointGlobal, + coordinate_frame: int = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + type_mask: int = 0x07FF) -> None: + """Send a global setpoint.""" + self.master.mav.set_position_target_global_int_send( + 0, + self.master.target_system, + self.master.target_component, + coordinate_frame, + type_mask, + int(setpoint.lat * 1e7), + int(setpoint.lon * 1e7), + setpoint.alt, + setpoint.vx, setpoint.vy, setpoint.vz, + setpoint.afx, setpoint.afy, setpoint.afz, + setpoint.yaw, setpoint.yaw_rate + ) + LOGGER.debug("Global setpoint sent: %s", setpoint) + + # ---------------------- + # Target reaching + # ---------------------- + def wait_target_reached(self, target: Optional[PositionSetpointLocal] = None, + tolerance: float = 0.05, timeout: int = 30) -> bool: + """Wait until the local target is reached within tolerance.""" + if target is None: + target = PositionSetpointLocal() + start_time = time.time() + while True: + current_pos = self.get_local_position() + if util.check_target_reached(current_pos, target, tolerance): + LOGGER.info("Target reached locally: %s", target) + return True + if (time.time() - start_time) > timeout: + LOGGER.warning("Timeout: Failed to reach local target: %s", target) + return False - NOTE: Positive is upwards - """ + def wait_target_reached_global(self, target: Optional[PositionSetpointGlobal] = None, + timeout: int = 30) -> bool: + """Wait until the global target is reached within tolerance.""" + if target is None: + target = PositionSetpointGlobal() + start_time = time.time() + while True: + current_pos = self.get_global_position() + distance = util.LatLon_to_Distance(current_pos, target) + if distance < 5: + LOGGER.info("Target reached globally: %s", target) + return True + if (time.time() - start_time) > timeout: + LOGGER.warning("Timeout: Failed to reach global target: %s", target) + return False - print("MAVCTL: Taking Off to: " + str(altitude) + "m") + # ---------------------- + # Simple goto commands + # ---------------------- + def simple_goto_local(self, x: float, y: float, z: float) -> None: + """Move drone to local NED coordinates.""" + type_mask = self.master.generate_typemask([0, 1, 2, 9]) + yaw_angle = atan(y / x) if x != 0 else 0 + setpoint = PositionSetpointLocal(x=x, y=y, z=-z, yaw=yaw_angle) + self.set_position_local_ned(setpoint, type_mask=type_mask) + self.wait_target_reached(setpoint) + + def simple_goto_global(self, lat: float, lon: float, alt: float) -> None: + """Move drone to global coordinates.""" + type_mask = self.master.generate_typemask([0, 1, 2, 9]) + start_point = self.get_global_position() + yaw = util.Heading(start_point, LocationGlobal(lat, lon, alt)) + setpoint = PositionSetpointGlobal(lat=lat, lon=lon, alt=alt, yaw=yaw) + self.set_position_global(setpoint, type_mask=type_mask) + self.wait_target_reached_global(setpoint) + + # ---------------------- + # VTOL transitions + # ---------------------- + def transition_mc(self) -> None: + """Transition from fixed-wing to multi-copter mode.""" self.master.mav.command_long_send( - self.master.target_system, - self.master.target_component, - mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, - 0, - pitch, - 0, - 0, - 0, - 0, - 0, - altitude) - - self.wait_target_reached(LocationLocal(0, 0, -altitude)) - - def vtol_takeoff(self, altitude, pitch=15): - """ - Initiates a takeoff the drone to target altiude - - Altitude: Altituide to take off to in meters - - NOTE: Positive is upwards - """ + self.master.target_system, + self.master.target_component, + mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, + 0, + mavutil.mavlink.MAV_VTOL_STATE_MC, + 0, 0, 0, 0, 0, 0 + ) + LOGGER.info("VTOL transition: multi-copter") - print("MAVCTL: Taking Off to: " + str(altitude) + "m") + def transition_fw(self) -> None: + """Transition from multi-copter to fixed-wing (forward flight) mode.""" self.master.mav.command_long_send( - self.master.target_system, - self.master.target_component, - mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, - 0, - pitch, - 0, - 0, - 0, - 0, - 0, - altitude) - - self.wait_target_reached(LocationLocal(0, 0, -altitude)) - - - def return_to_launch(self): - print("MAVCTL: RTL") + self.master.target_system, + self.master.target_component, + mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, + 0, + mavutil.mavlink.MAV_VTOL_STATE_FW, + 0, 0, 0, 0, 0, 0 + ) + LOGGER.info("VTOL transition: forward flight") + + # ---------------------- + # Return to Launch + # ---------------------- + def return_to_launch(self) -> None: + """Command the vehicle to return to its launch location.""" self.master.mav.command_long_send( self.master.target_system, self.master.target_component, mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0 + 0, 0, 0, 0, 0, 0, 0 ) + LOGGER.info("Return to Launch (RTL) command sent") - - - - - - def generate_typemask(self, keeps): - # Generates typemask based on which values to be included - mask = 0 - for bit in keeps: - mask |= (0 << bit) - return mask - - - def set_position_local_ned(self, - coordinate_frame = mavutil.mavlink.MAV_FRAME_LOCAL_NED, - type_mask=0x07FF, - x=0, - y=0, - z=0, - vx=0, - vy=0, - vz=0, - afx=0, - afy=0, - afz=0, - yaw=0, - yaw_rate=0): + # ---------------------- + # Repositioning + # ---------------------- + def do_reposition(self, radius: float, lat: float, lon: float, alt: float) -> None: """ - Set position in local/relative coordinates. Can also set velocity/acceleration setpoints - x, y, z is in meters in NED coordinates (down is positive) - """ - - self.master.mav.set_position_target_local_ned_send( - 0, - self.master.target_system, - self.master.target_component, - coordinate_frame, - type_mask, - x, - y, - z, - vx, - vy, - vz, - afx, - afy, - afz, - yaw, - yaw_rate) - - - def set_speed(self, speed): - - - #Adjusts the global speed parameter. - #(WIP) DO NOT USE IN REAL FLIGHT, THIS METHOD HAS NOT BEEN VERIFIED YET. - + Reposition vehicle for plane frame types (altitude relative to ground). - self.master.mav.param_set_send( - self.master.target_system, - self.master.target_component, - b'WPNAV_SPEED', - speed*100, # Speed in m/s is converted to cm/s - mavutil.mavlink.MAV_PARAM_TYPE_REAL32) - - - def set_position_global(self, - coordinate_frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, - type_mask=0x07FF, - lat=0, - lon=0, - alt=0, - vx=0, - vy=0, - vz=0, - afx=0, - afy=0, - afz=0, - yaw=0, - yaw_rate=0): - """ - Set position in global coordinates. Can also set velocity/acceleration setpoints - lat, lon, and altitude. Altitude depends on the coordinate frame chosen - """ - - - self.master.mav.set_position_target_global_int_send(0, - self.master.target_system, - self.master.target_component, - coordinate_frame, - type_mask, - int(lat * 10000000), - int(lon * 10000000), - alt, - vx, - vy, - vz, - afx, - afy, - afz, - yaw, - yaw_rate) - - - def do_reposition(self, - radius, - lat, - lon, - alt): - """ - Similar to set_position_global but is intended for use with plane frame types. - Alt is set to be relative altitude (ground is 0m) + Args: + radius: Desired radius around target location. + lat: Latitude of target. + lon: Longitude of target. + alt: Relative altitude at target. """ - self.master.mav.command_int_send( - self.master.target_system, - self.master.target_component, - mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, - mavutil.mavlink.MAV_CMD_DO_REPOSITION, - 0, - 1, - -1, - 0, - radius, - 0, - int(lat * 1e7), - int(lon * 1e7), - alt) - - - def transition_mc(self): - # A method to transition from fixed wing to multi-copter - # Normal Transition is default, force immediate is not recommended as it can cause damage to the vehicle - - self.master.mav.command_long_send( - self.master.target_system, - self.master.target_component, - mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, - 0, - mavutil.mavlink.MAV_VTOL_STATE_MC, - 0, # Normal Transition and not a force immediate - 0, - 0, - 0, - 0, - 0) - - - - def transition_fw(self): - # A method to transition from multi-copter to forward flight - # Normal Transition is default, force immediate is not recommended as it can cause damage to the vehicle + self.master.target_system, + self.master.target_component, + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + mavutil.mavlink.MAV_CMD_DO_REPOSITION, + 0, + 1, -1, 0, + radius, 0, + int(lat * 1e7), + int(lon * 1e7), + alt + ) + LOGGER.info("Reposition command: lat=%.7f lon=%.7f alt=%.2f radius=%.2f", + lat, lon, alt, radius) - self.master.mav.command_long_send( - self.master.target_system, - self.master.target_component, - mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION, - 0, - mavutil.mavlink.MAV_VTOL_STATE_MC, - 0, - 0, - 0, - 0, - 0, - 0) - - - def wait_target_reached(self, target, tolerance=0.05, timeout = 30) -> bool: - """ - Waits for the drone to reach specified target + # ---------------------- + # Speed control + # ---------------------- + def set_speed(self, speed: float) -> None: """ + Sets the global speed parameter (simulation/WIP). - current_pos = self.get_local_position() - - check_target = util.check_target_reached(current_pos, target, tolerance) - start_time = time.time() - while not check_target: - current_pos = self.get_local_position() - check_target = util.check_target_reached(current_pos, target, tolerance) - if time.time() - start_time > timeout: - print("MAVCTL Timeout: Failed to reach target within tolerance. Continuing execution") - return False - - return True - - def wait_target_reached_global(self, target, timeout = 30): - """ - Waits for the drone to reach specified target (in global coordinates) + Args: + speed: Desired speed in m/s (converted to cm/s for MAVLink). """ - current_pos = self.get_global_position() - distance = util.LatLon_to_Distance(current_pos, target) - start_time = time.time() - - while not distance < 5: - current_pos = self.get_global_position() - distance = util.LatLon_to_Distance(current_pos, target) - if time.time() - start_time > timeout: - print("MAVCTL Timeout: Failed to reach target within tolerance. Continuing execution") - return False - - return True + self.master.mav.param_set_send( + self.master.target_system, + self.master.target_component, + b'WPNAV_SPEED', + speed * 100, # Convert m/s -> cm/s + mavutil.mavlink.MAV_PARAM_TYPE_REAL32 + ) + LOGGER.info("Global speed set to %.2f m/s", speed) diff --git a/mavctl/messages/util.py b/mavctl/messages/util.py deleted file mode 100644 index 87edbe7..0000000 --- a/mavctl/messages/util.py +++ /dev/null @@ -1,77 +0,0 @@ -import math -from math import radians, degrees, cos, sin, atan2, sqrt -from pymavlink import mavutil -from mavctl.messages.location import LocationGlobal, LocationGlobalRelative, LocationLocal - - -def distance_to_target(target1, target2): - """ - Returns the distance vector to the target - Target 1: Current Position of Vehicle - Target 2: Target Position - - """ - - dx = target2.north - target1.north - dy = target2.east - target1.east - dz = target2.down - target1.down - - return LocationLocal(dx, dy, dz) - -def check_target_reached(target1, target2, tolerance) -> bool: - """ - Checks if the vehicle has reached the target to a certian level of precision - target1: Current Position of the vehicle (tuple) - target2: Target Position (tuple) - tolerance: Tolerance coefficent - """ - delta = distance_to_target(target1, target2) - - target_length = math.sqrt(target1.north ** 2 + target1.east ** 2 + target1.down ** 2) - - delta_length = math.sqrt(delta.north ** 2 + delta.east ** 2 + delta.down ** 2) - if delta_length < 0.5: - return True - else: - return False - -def LatLon_to_Distance(point, origin): - - R = 6378137 # Radius of the Earth in meters - - origin_lat = origin.lat - origin_lon = origin.lon - point_lat = point.lat - point_lon = point.lon - - origin_lon_rad = radians(origin_lon) - origin_lat_rad = radians(origin_lat) - point_lon_rad = radians(point_lon) - point_lat_rad = radians(point_lat) - - delta_lon = point_lon_rad - origin_lon_rad - delta_lat = point_lat_rad - origin_lat_rad - - alt = point.alt - origin.alt - - a = sin(delta_lat / 2) ** 2 + cos(origin_lat) * cos(point_lat) * sin(delta_lon / 2) ** 2 - c = 2 * atan2(sqrt(a), sqrt(1-a)) - l = R * c - - return l - -def Heading(point1, point2): - - lat1_rad = radians(point1.lat) - lat2_rad = radians(point2.lat) - delta_lon = radians(point2.lon - point1.lon) - - x = sin(delta_lon) * cos(lat2_rad) - y = cos(lat1_rad) * sin(lat2_rad) - sin(lat1_rad) * cos(lat2_rad) * cos(delta_lon) - - angle = atan2(x, y) - heading = (degrees(angle) + 360) % 360 - - return heading - - diff --git a/mavctl/tests/flight_tests/air.py b/tests/flight_tests/air.py similarity index 100% rename from mavctl/tests/flight_tests/air.py rename to tests/flight_tests/air.py diff --git a/mavctl/tests/flight_tests/auto.py b/tests/flight_tests/auto.py similarity index 100% rename from mavctl/tests/flight_tests/auto.py rename to tests/flight_tests/auto.py diff --git a/mavctl/tests/flight_tests/first_flight.py b/tests/flight_tests/first_flight.py similarity index 59% rename from mavctl/tests/flight_tests/first_flight.py rename to tests/flight_tests/first_flight.py index 24a24ba..31fd3fb 100644 --- a/mavctl/tests/flight_tests/first_flight.py +++ b/tests/flight_tests/first_flight.py @@ -1,6 +1,5 @@ from pymavlink import mavutil -from mavctl.messages.navigator import Navigator -from mavctl.messages import advanced +from mavctl import Navigator import time CONN_STR = "udp:127.0.0.1:14551" @@ -12,6 +11,6 @@ drone.takeoff(10) time.sleep(5) -advanced.simple_goto_global(drone, 53.496970, -113.545194, 20) +drone.simple_goto_global(53.496970, -113.545194, 20) drone.return_to_launch() diff --git a/mavctl/tests/flight_tests/plane.py b/tests/flight_tests/plane.py similarity index 100% rename from mavctl/tests/flight_tests/plane.py rename to tests/flight_tests/plane.py diff --git a/mavctl/tests/flight_tests/quad.py b/tests/flight_tests/quad.py similarity index 100% rename from mavctl/tests/flight_tests/quad.py rename to tests/flight_tests/quad.py diff --git a/mavctl/tests/flight_tests/swarm/copter1.py b/tests/flight_tests/swarm/copter1.py similarity index 100% rename from mavctl/tests/flight_tests/swarm/copter1.py rename to tests/flight_tests/swarm/copter1.py diff --git a/mavctl/tests/flight_tests/swarm/copter2.py b/tests/flight_tests/swarm/copter2.py similarity index 100% rename from mavctl/tests/flight_tests/swarm/copter2.py rename to tests/flight_tests/swarm/copter2.py diff --git a/mavctl/tests/flight_tests/waypoint.py b/tests/flight_tests/waypoint.py similarity index 100% rename from mavctl/tests/flight_tests/waypoint.py rename to tests/flight_tests/waypoint.py diff --git a/mavctl/tests/msg_test.py b/tests/msg_test.py similarity index 100% rename from mavctl/tests/msg_test.py rename to tests/msg_test.py From b5c1d63345ed58f03e359bdf9e53ccded08cfa18 Mon Sep 17 00:00:00 2001 From: HameedFawwaz Date: Fri, 12 Dec 2025 21:58:48 -0700 Subject: [PATCH 4/9] Added .pylintrc to pylint.yml --- .github/workflows/pylint.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/pylint.yml b/.github/workflows/pylint.yml index c73e032..5bcad5e 100644 --- a/.github/workflows/pylint.yml +++ b/.github/workflows/pylint.yml @@ -20,4 +20,4 @@ jobs: pip install pylint - name: Analysing the code with pylint run: | - pylint $(git ls-files '*.py') + pylint --rcfile=.pylintrc $(git ls-files '*.py') From 9720cc2aaa4aa748a96e3ce4c6093ff8a5760ec7 Mon Sep 17 00:00:00 2001 From: HameedFawwaz Date: Fri, 12 Dec 2025 22:01:10 -0700 Subject: [PATCH 5/9] Updated pylint.yml --- .github/workflows/pylint.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/pylint.yml b/.github/workflows/pylint.yml index 5bcad5e..9976c57 100644 --- a/.github/workflows/pylint.yml +++ b/.github/workflows/pylint.yml @@ -7,7 +7,7 @@ jobs: runs-on: ubuntu-latest strategy: matrix: - python-version: ["3.8", "3.9", "3.10"] + python-version: ["3.10"] steps: - uses: actions/checkout@v4 - name: Set up Python ${{ matrix.python-version }} From 0fdd9c71d8daa92a11f6f27d4cf3738322c8200d Mon Sep 17 00:00:00 2001 From: HameedFawwaz Date: Fri, 12 Dec 2025 22:01:58 -0700 Subject: [PATCH 6/9] Updated pylint.yml to look at requirements.txt --- .github/workflows/pylint.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/pylint.yml b/.github/workflows/pylint.yml index 9976c57..3c898f4 100644 --- a/.github/workflows/pylint.yml +++ b/.github/workflows/pylint.yml @@ -18,6 +18,7 @@ jobs: run: | python -m pip install --upgrade pip pip install pylint + pip install -r requirements.txt - name: Analysing the code with pylint run: | pylint --rcfile=.pylintrc $(git ls-files '*.py') From 2b7540607a28f648487c7d1857be08291445b8da Mon Sep 17 00:00:00 2001 From: HameedFawwaz Date: Fri, 12 Dec 2025 22:04:32 -0700 Subject: [PATCH 7/9] Updated pylint.yml to look at all files --- .github/workflows/pylint.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/pylint.yml b/.github/workflows/pylint.yml index 3c898f4..bf686fa 100644 --- a/.github/workflows/pylint.yml +++ b/.github/workflows/pylint.yml @@ -21,4 +21,4 @@ jobs: pip install -r requirements.txt - name: Analysing the code with pylint run: | - pylint --rcfile=.pylintrc $(git ls-files '*.py') + pylint --rcfile=.pylintrc mavctl From 1fe6365af6cb685043702ff282a3b8aad6f68d9c Mon Sep 17 00:00:00 2001 From: HameedFawwaz Date: Fri, 12 Dec 2025 22:06:44 -0700 Subject: [PATCH 8/9] Updated pylint.yml --- .github/workflows/pylint.yml | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/.github/workflows/pylint.yml b/.github/workflows/pylint.yml index 2b79412..c48ddef 100644 --- a/.github/workflows/pylint.yml +++ b/.github/workflows/pylint.yml @@ -22,10 +22,8 @@ jobs: - name: Install dependencies run: | python -m pip install --upgrade pip - pip install pylint - # If you have requirements: - # pip install -r requirements.txt + pip install -r requirements.txt - name: Run pylint run: | - pylint $(git ls-files '*.py') + pylint --rcfile=.pylintrc mavctl From 92665d39f3c86ed8f4bf2ab913b124e3cd6e5234 Mon Sep 17 00:00:00 2001 From: HameedFawwaz Date: Fri, 12 Dec 2025 22:29:01 -0700 Subject: [PATCH 9/9] Making flight test script pass.... --- .github/workflows/pylint.yml | 2 +- mavctl/_internal/util.py | 6 ++--- mavctl/messages/navigator.py | 43 ++++++++++++++++++++++++++++++++++-- 3 files changed, 45 insertions(+), 6 deletions(-) diff --git a/.github/workflows/pylint.yml b/.github/workflows/pylint.yml index c48ddef..4fced63 100644 --- a/.github/workflows/pylint.yml +++ b/.github/workflows/pylint.yml @@ -26,4 +26,4 @@ jobs: - name: Run pylint run: | - pylint --rcfile=.pylintrc mavctl + pylint --rcfile=.pylintrc $(find mavctl -name "*.py") diff --git a/mavctl/_internal/util.py b/mavctl/_internal/util.py index 4afffd7..c2e1546 100644 --- a/mavctl/_internal/util.py +++ b/mavctl/_internal/util.py @@ -10,9 +10,9 @@ def distance_to_target(target1, target2) -> LocationLocal: target1: Current Position of Vehicle (LocationLocal) target2: Target Position (LocationLocal) """ - dx = target2.north - target1.north - dy = target2.east - target1.east - dz = target2.down - target1.down + dx = target2.x - target1.north + dy = target2.y - target1.east + dz = target2.z - target1.down return LocationLocal(dx, dy, dz) diff --git a/mavctl/messages/navigator.py b/mavctl/messages/navigator.py index 2d49081..07aa4ee 100644 --- a/mavctl/messages/navigator.py +++ b/mavctl/messages/navigator.py @@ -1,7 +1,7 @@ import time from math import atan from dataclasses import dataclass -from typing import Optional +from typing import Optional, Iterable from pymavlink import mavutil @@ -132,6 +132,24 @@ def set_mode_wait(self, mode: str = "GUIDED", timeout: Optional[int] = None) -> LOGGER.info("Mode set to %s", mode) return True + def wait_for_mode_and_arm(self, mode="GUIDED", timeout=None) -> bool: + """Wait for the vehicle to enter ``mode`` and to be armed""" + mode_ready = self.set_mode_wait(mode=mode, timeout=timeout) + if not mode_ready: + return False + while not self.wait_vehicle_armed(): + return True + LOGGER.warning("MAVCTL: Failed to wait for vehicle arm") + return False + + def wait_vehicle_armed(self): + """ + Waits for the vehicle to be armed. See samples directory for examples + """ + LOGGER.info("MAVCTL: Waiting for vehicle to arm") + self.master.motors_armed_wait() + LOGGER.info("MAVCTL: Armed!") + # ---------------------- # Position / Velocity access # ---------------------- @@ -259,7 +277,7 @@ def simple_goto_local(self, x: float, y: float, z: float) -> None: def simple_goto_global(self, lat: float, lon: float, alt: float) -> None: """Move drone to global coordinates.""" - type_mask = self.master.generate_typemask([0, 1, 2, 9]) + type_mask = self.generate_typemask([0, 1, 2, 9]) start_point = self.get_global_position() yaw = util.Heading(start_point, LocationGlobal(lat, lon, alt)) setpoint = PositionSetpointGlobal(lat=lat, lon=lon, alt=alt, yaw=yaw) @@ -353,3 +371,24 @@ def set_speed(self, speed: float) -> None: mavutil.mavlink.MAV_PARAM_TYPE_REAL32 ) LOGGER.info("Global speed set to %.2f m/s", speed) + + def generate_typemask(self, keeps: Iterable[int]) -> int: + """ + Generate a MAVLink type mask based on the bits to keep (enable). + + Each bit position in `keeps` will be set to 1 in the resulting mask. + + Args: + keeps (Iterable[int]): Bit positions to enable in the mask. + + Returns: + int: Generated type mask. + """ + mask = 0 + + for bit in keeps: + if bit < 0: + raise ValueError(f"Bit positions must be non-negative, got {bit}") + mask |= 1 << bit + + return mask