From 229b19e10efec82f6d6499aa02af04a7ecb60e5f Mon Sep 17 00:00:00 2001 From: Crusific Date: Fri, 14 Nov 2025 15:41:40 -0700 Subject: [PATCH 1/7] Add HeartbeatManager for connection monitoring Implements a heartbeat manager to monitor connection status between ground station and drone, handling reconnection attempts. --- mavctl/connect/heartbeat.py | 82 +++++++++++++++++++++++++++++++++++++ 1 file changed, 82 insertions(+) create mode 100644 mavctl/connect/heartbeat.py diff --git a/mavctl/connect/heartbeat.py b/mavctl/connect/heartbeat.py new file mode 100644 index 0000000..fafeb5c --- /dev/null +++ b/mavctl/connect/heartbeat.py @@ -0,0 +1,82 @@ +import threading +import time +from typing import Callable +from pymavlink import mavutil + +class HeartbeatManager: + """ + Manages the heartbeat system between the ground station and the drone. + Monitors connection status and handles reconnection attempts. + """ + def __init__(self, mav, heartbeat_timeout: float = 1.0, max_missed_heartbeats: int = 2): + """ + Initialize the heartbeat manager. + + Args: + mav: The MAVLink connection object + heartbeat_timeout: Time in seconds to wait for a heartbeat before considering it missed + max_missed_heartbeats: Number of consecutive heartbeats that can be missed before connection is considered lost + """ + self.mav = mav + self.heartbeat_timeout = heartbeat_timeout + self.max_missed_heartbeats = max_missed_heartbeats + self.last_heartbeat = 0 + self.missed_heartbeats = 0 + self.is_connected = False + self._stop_event = threading.Event() + self._monitor_thread = None + self._on_connection_lost_callback = None + self._on_connection_established_callback = None + + def start(self, on_connection_lost: Callable = None, on_connection_established: Callable = None): + """ + Start the heartbeat monitoring system. + + Args: + on_connection_lost: Callback function to be called when connection is lost + on_connection_established: Callback function to be called when connection is established + """ + self._on_connection_lost_callback = on_connection_lost + self._on_connection_established_callback = on_connection_established + self._stop_event.clear() + self._monitor_thread = threading.Thread(target=self._monitor_heartbeat) + self._monitor_thread.daemon = True + self._monitor_thread.start() + + def stop(self): + """Stop the heartbeat monitoring system.""" + if self._monitor_thread: + self._stop_event.set() + self._monitor_thread.join() + self._monitor_thread = None + + def _monitor_heartbeat(self): + """Monitor heartbeat messages and manage connection status.""" + while not self._stop_event.is_set(): + msg = self.mav.recv_match(type="HEARTBEAT", blocking=True, timeout=self.heartbeat_timeout) + + if msg is not None: + self.last_heartbeat = time.time() + self.missed_heartbeats = 0 + if not self.is_connected: + self.is_connected = True + if self._on_connection_established_callback: + self._on_connection_established_callback() + else: + self.missed_heartbeats += 1 + if self.missed_heartbeats >= self.max_missed_heartbeats and self.is_connected: + self.is_connected = False + if self._on_connection_lost_callback: + self._on_connection_lost_callback() + + def get_connection_status(self) -> bool: + """Get the current connection status.""" + return self.is_connected + + def get_last_heartbeat_time(self) -> float: + """Get the timestamp of the last received heartbeat.""" + return self.last_heartbeat + + def get_missed_heartbeats(self) -> int: + """Get the number of consecutively missed heartbeats.""" + return self.missed_heartbeats From ffe6f10131effa1d4deb327eb562ea8e87512eb9 Mon Sep 17 00:00:00 2001 From: Crusific Date: Fri, 14 Nov 2025 15:47:10 -0700 Subject: [PATCH 2/7] Refactor HeartbeatManager for type hints and clarity --- mavctl/connect/heartbeat.py | 90 ++++++++++++++++++++++++------------- 1 file changed, 60 insertions(+), 30 deletions(-) diff --git a/mavctl/connect/heartbeat.py b/mavctl/connect/heartbeat.py index fafeb5c..7e4be9f 100644 --- a/mavctl/connect/heartbeat.py +++ b/mavctl/connect/heartbeat.py @@ -1,82 +1,112 @@ import threading import time -from typing import Callable +from typing import Callable, Optional from pymavlink import mavutil + class HeartbeatManager: """ Manages the heartbeat system between the ground station and the drone. Monitors connection status and handles reconnection attempts. """ - def __init__(self, mav, heartbeat_timeout: float = 1.0, max_missed_heartbeats: int = 2): + + def __init__( + self, + mav: mavutil.mavlink.MAVLink_connection, + heartbeat_timeout: float = 1.0, + max_missed_heartbeats: int = 2, + ) -> None: """ Initialize the heartbeat manager. - + Args: - mav: The MAVLink connection object - heartbeat_timeout: Time in seconds to wait for a heartbeat before considering it missed - max_missed_heartbeats: Number of consecutive heartbeats that can be missed before connection is considered lost + mav: The MAVLink connection object. + heartbeat_timeout: Seconds to wait before considering a heartbeat missed. + max_missed_heartbeats: Number of consecutive missed heartbeats allowed. """ self.mav = mav self.heartbeat_timeout = heartbeat_timeout self.max_missed_heartbeats = max_missed_heartbeats - self.last_heartbeat = 0 - self.missed_heartbeats = 0 - self.is_connected = False + + self.last_heartbeat: float = 0.0 + self.missed_heartbeats: int = 0 + self.is_connected: bool = False + self._stop_event = threading.Event() - self._monitor_thread = None - self._on_connection_lost_callback = None - self._on_connection_established_callback = None + self._monitor_thread: Optional[threading.Thread] = None + + self._on_connection_lost_callback: Optional[Callable[[], None]] = None + self._on_connection_established_callback: Optional[Callable[[], None]] = None - def start(self, on_connection_lost: Callable = None, on_connection_established: Callable = None): + def start( + self, + on_connection_lost: Optional[Callable[[], None]] = None, + on_connection_established: Optional[Callable[[], None]] = None, + ) -> None: """ - Start the heartbeat monitoring system. - + Start the heartbeat monitoring thread. + Args: - on_connection_lost: Callback function to be called when connection is lost - on_connection_established: Callback function to be called when connection is established + on_connection_lost: Callback when connection is lost. + on_connection_established: Callback when connection becomes established. """ self._on_connection_lost_callback = on_connection_lost self._on_connection_established_callback = on_connection_established + self._stop_event.clear() - self._monitor_thread = threading.Thread(target=self._monitor_heartbeat) - self._monitor_thread.daemon = True + self._monitor_thread = threading.Thread( + target=self._monitor_heartbeat, name="HeartbeatMonitor", daemon=True + ) self._monitor_thread.start() - def stop(self): - """Stop the heartbeat monitoring system.""" - if self._monitor_thread: + def stop(self) -> None: + """Stop the heartbeat monitoring thread.""" + if self._monitor_thread is not None: self._stop_event.set() self._monitor_thread.join() self._monitor_thread = None - def _monitor_heartbeat(self): - """Monitor heartbeat messages and manage connection status.""" + def _monitor_heartbeat(self) -> None: + """ + Monitor incoming heartbeat messages and update connection status. + This runs in a background thread. + """ while not self._stop_event.is_set(): - msg = self.mav.recv_match(type="HEARTBEAT", blocking=True, timeout=self.heartbeat_timeout) - + msg = self.mav.recv_match( + type="HEARTBEAT", + blocking=True, + timeout=self.heartbeat_timeout, + ) + if msg is not None: + # Heartbeat received self.last_heartbeat = time.time() self.missed_heartbeats = 0 + if not self.is_connected: self.is_connected = True if self._on_connection_established_callback: self._on_connection_established_callback() else: + # Missed heartbeat self.missed_heartbeats += 1 - if self.missed_heartbeats >= self.max_missed_heartbeats and self.is_connected: + + if ( + self.missed_heartbeats >= self.max_missed_heartbeats + and self.is_connected + ): self.is_connected = False if self._on_connection_lost_callback: self._on_connection_lost_callback() def get_connection_status(self) -> bool: - """Get the current connection status.""" + """Return whether the connection is currently active.""" return self.is_connected def get_last_heartbeat_time(self) -> float: - """Get the timestamp of the last received heartbeat.""" + """Return the timestamp of the last received heartbeat.""" return self.last_heartbeat def get_missed_heartbeats(self) -> int: - """Get the number of consecutively missed heartbeats.""" + """Return the count of consecutively missed heartbeats.""" return self.missed_heartbeats From 211166a5d4ad50482383432f133c0742d3feaa94 Mon Sep 17 00:00:00 2001 From: Crusific Date: Fri, 14 Nov 2025 15:54:04 -0700 Subject: [PATCH 3/7] Refactor HeartbeatManager for improved clarity --- mavctl/connect/heartbeat.py | 122 +++++++++++++++++------------------- 1 file changed, 57 insertions(+), 65 deletions(-) diff --git a/mavctl/connect/heartbeat.py b/mavctl/connect/heartbeat.py index 7e4be9f..6af27f7 100644 --- a/mavctl/connect/heartbeat.py +++ b/mavctl/connect/heartbeat.py @@ -1,112 +1,104 @@ +"""Heartbeat monitoring system for drone-ground station connection management. + +This module provides the HeartbeatManager class, which monitors MAVLink heartbeat +messages from the drone and automatically detects connection loss based on missed +heartbeats. It supports callback-based notifications for connection state changes. +""" + import threading import time -from typing import Callable, Optional -from pymavlink import mavutil +from typing import Callable + +from pymavlink import mavutil # pylint: disable=import-error class HeartbeatManager: - """ - Manages the heartbeat system between the ground station and the drone. - Monitors connection status and handles reconnection attempts. - """ + """Monitor MAVLink heartbeat messages and detect connection loss. - def __init__( - self, - mav: mavutil.mavlink.MAVLink_connection, - heartbeat_timeout: float = 1.0, - max_missed_heartbeats: int = 2, - ) -> None: - """ - Initialize the heartbeat manager. + Runs a background thread that listens for HEARTBEAT messages from the drone. + Triggers callbacks when connection is established or lost after N missed + heartbeats. + """ + def __init__(self, mav, heartbeat_timeout: float = 1.0, max_missed_heartbeats: int = 2): + """Initialize the heartbeat manager. Args: - mav: The MAVLink connection object. - heartbeat_timeout: Seconds to wait before considering a heartbeat missed. - max_missed_heartbeats: Number of consecutive missed heartbeats allowed. + mav: The MAVLink connection object + heartbeat_timeout: Time in seconds to wait for a heartbeat before + considering it missed (default 1.0). + max_missed_heartbeats: Number of consecutive heartbeats that can be + missed before connection is considered lost (default 2). """ self.mav = mav self.heartbeat_timeout = heartbeat_timeout self.max_missed_heartbeats = max_missed_heartbeats - - self.last_heartbeat: float = 0.0 - self.missed_heartbeats: int = 0 - self.is_connected: bool = False - + self.last_heartbeat = 0 + self.missed_heartbeats = 0 + self.is_connected = False self._stop_event = threading.Event() - self._monitor_thread: Optional[threading.Thread] = None - - self._on_connection_lost_callback: Optional[Callable[[], None]] = None - self._on_connection_established_callback: Optional[Callable[[], None]] = None + self._monitor_thread = None + self._callbacks = { + 'on_connection_lost': None, + 'on_connection_established': None, + } - def start( - self, - on_connection_lost: Optional[Callable[[], None]] = None, - on_connection_established: Optional[Callable[[], None]] = None, - ) -> None: - """ - Start the heartbeat monitoring thread. + def start(self, on_connection_lost: Callable = None, + on_connection_established: Callable = None): + """Start the heartbeat monitoring system. Args: - on_connection_lost: Callback when connection is lost. - on_connection_established: Callback when connection becomes established. + on_connection_lost: Callback function called when connection is lost. + on_connection_established: Callback function called when connection + is established. """ - self._on_connection_lost_callback = on_connection_lost - self._on_connection_established_callback = on_connection_established - + self._callbacks['on_connection_lost'] = on_connection_lost + self._callbacks['on_connection_established'] = on_connection_established self._stop_event.clear() - self._monitor_thread = threading.Thread( - target=self._monitor_heartbeat, name="HeartbeatMonitor", daemon=True - ) + self._monitor_thread = threading.Thread(target=self._monitor_heartbeat) + self._monitor_thread.daemon = True self._monitor_thread.start() - def stop(self) -> None: - """Stop the heartbeat monitoring thread.""" - if self._monitor_thread is not None: + def stop(self): + """Stop the heartbeat monitoring system.""" + if self._monitor_thread: self._stop_event.set() self._monitor_thread.join() self._monitor_thread = None - def _monitor_heartbeat(self) -> None: - """ - Monitor incoming heartbeat messages and update connection status. - This runs in a background thread. - """ + def _monitor_heartbeat(self): + """Monitor heartbeat messages and manage connection status.""" while not self._stop_event.is_set(): msg = self.mav.recv_match( type="HEARTBEAT", blocking=True, - timeout=self.heartbeat_timeout, + timeout=self.heartbeat_timeout ) if msg is not None: - # Heartbeat received self.last_heartbeat = time.time() self.missed_heartbeats = 0 - if not self.is_connected: self.is_connected = True - if self._on_connection_established_callback: - self._on_connection_established_callback() + callback = self._callbacks['on_connection_established'] + if callback: + callback() else: - # Missed heartbeat self.missed_heartbeats += 1 - - if ( - self.missed_heartbeats >= self.max_missed_heartbeats - and self.is_connected - ): + if (self.missed_heartbeats >= self.max_missed_heartbeats + and self.is_connected): self.is_connected = False - if self._on_connection_lost_callback: - self._on_connection_lost_callback() + callback = self._callbacks['on_connection_lost'] + if callback: + callback() def get_connection_status(self) -> bool: - """Return whether the connection is currently active.""" + """Get the current connection status.""" return self.is_connected def get_last_heartbeat_time(self) -> float: - """Return the timestamp of the last received heartbeat.""" + """Get the timestamp of the last received heartbeat.""" return self.last_heartbeat def get_missed_heartbeats(self) -> int: - """Return the count of consecutively missed heartbeats.""" + """Get the number of consecutively missed heartbeats.""" return self.missed_heartbeats From b91b7a2c80c421ad5103e25399806255bb93e49b Mon Sep 17 00:00:00 2001 From: Crusific Date: Fri, 14 Nov 2025 15:57:44 -0700 Subject: [PATCH 4/7] Remove unused import statement for pymavlink --- mavctl/connect/heartbeat.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/mavctl/connect/heartbeat.py b/mavctl/connect/heartbeat.py index 6af27f7..225b21a 100644 --- a/mavctl/connect/heartbeat.py +++ b/mavctl/connect/heartbeat.py @@ -4,13 +4,12 @@ messages from the drone and automatically detects connection loss based on missed heartbeats. It supports callback-based notifications for connection state changes. """ +# pylint: disable=too-many-instance-attributes import threading import time from typing import Callable -from pymavlink import mavutil # pylint: disable=import-error - class HeartbeatManager: """Monitor MAVLink heartbeat messages and detect connection loss. From 890406a5e4eea70eb6c72775529de714821b1aae Mon Sep 17 00:00:00 2001 From: Crusific Date: Fri, 14 Nov 2025 16:19:47 -0700 Subject: [PATCH 5/7] Update callback checks to use None comparison --- mavctl/connect/heartbeat.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mavctl/connect/heartbeat.py b/mavctl/connect/heartbeat.py index 225b21a..850fe1b 100644 --- a/mavctl/connect/heartbeat.py +++ b/mavctl/connect/heartbeat.py @@ -79,7 +79,7 @@ def _monitor_heartbeat(self): if not self.is_connected: self.is_connected = True callback = self._callbacks['on_connection_established'] - if callback: + if callback is not None: callback() else: self.missed_heartbeats += 1 @@ -87,7 +87,7 @@ def _monitor_heartbeat(self): and self.is_connected): self.is_connected = False callback = self._callbacks['on_connection_lost'] - if callback: + if callback is not None: callback() def get_connection_status(self) -> bool: From afd4b28b3ce2b050a01cd9a472348e034f2489cb Mon Sep 17 00:00:00 2001 From: HameedFawwaz Date: Fri, 14 Nov 2025 20:35:49 -0700 Subject: [PATCH 6/7] Created send heartbeat infrastructure and integrated with conn.py --- mavctl/connect/conn.py | 14 +++++--------- mavctl/connect/heartbeat.py | 20 +++++++++++++++++++- 2 files changed, 24 insertions(+), 10 deletions(-) diff --git a/mavctl/connect/conn.py b/mavctl/connect/conn.py index 66ab8f1..c6d9e01 100644 --- a/mavctl/connect/conn.py +++ b/mavctl/connect/conn.py @@ -3,6 +3,7 @@ import time import threading from pymavlink import mavutil +from mavctl.connect.heartbeat import HeartbeatManager from mavctl.messages.navigator import Navigator class Connect: @@ -12,16 +13,11 @@ def __init__(self, baud: int = 57600, heartbeat_timeout = None): - self._stop_event = threading.Event() - - - - self.send_heartbeat_thread = threading.Thread(target = self.send_heartbeat, daemon = True) - self.recv_heartbeat_thread = threading.Thread(target = self.recv_heartbeat, daemon = True) - self.mav = self.connect(ip = ip, baud = baud, heartbeat_timeout = heartbeat_timeout) - self.heartbeat_start() - + + self._heartbeat_manager = HeartbeatManager(self.mav) + self._heartbeat_manager.start() + def send_heartbeat(self, interval: int = 0.25): diff --git a/mavctl/connect/heartbeat.py b/mavctl/connect/heartbeat.py index 850fe1b..39e83e2 100644 --- a/mavctl/connect/heartbeat.py +++ b/mavctl/connect/heartbeat.py @@ -9,7 +9,7 @@ import threading import time from typing import Callable - +from pymavlink import mavutil class HeartbeatManager: """Monitor MAVLink heartbeat messages and detect connection loss. @@ -36,6 +36,7 @@ def __init__(self, mav, heartbeat_timeout: float = 1.0, max_missed_heartbeats: i self.is_connected = False self._stop_event = threading.Event() self._monitor_thread = None + self._send_thread = None self._callbacks = { 'on_connection_lost': None, 'on_connection_established': None, @@ -55,14 +56,19 @@ def start(self, on_connection_lost: Callable = None, self._stop_event.clear() self._monitor_thread = threading.Thread(target=self._monitor_heartbeat) self._monitor_thread.daemon = True + self._send_thread = threading.Thread(target=self._send_heartbeat) + self._send_thread.daemon = True self._monitor_thread.start() + self._send_thread.start() def stop(self): """Stop the heartbeat monitoring system.""" if self._monitor_thread: self._stop_event.set() self._monitor_thread.join() + self._send_thread.join() self._monitor_thread = None + self._send_thread = None def _monitor_heartbeat(self): """Monitor heartbeat messages and manage connection status.""" @@ -90,6 +96,18 @@ def _monitor_heartbeat(self): if callback is not None: callback() + def _send_heartbeat(self): + """Send heartbeat messages""" + while not self._stop_event.is_set(): + self.mav.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(self.heartbeat_timeout) + def get_connection_status(self) -> bool: """Get the current connection status.""" return self.is_connected From 2d19af13e10e5e036db5d6325426c8ba5a0aa5b1 Mon Sep 17 00:00:00 2001 From: HameedFawwaz Date: Fri, 28 Nov 2025 15:04:33 -0700 Subject: [PATCH 7/7] Added some minor modifications to structure to help with readibility --- mavctl/connect/conn.py | 10 +-- mavctl/connect/heartbeat.py | 8 +- mavctl/messages/advanced.py | 9 +- mavctl/messages/navigator.py | 102 +++++++++++----------- mavctl/tests/flight_tests/first_flight.py | 23 +++-- 5 files changed, 73 insertions(+), 79 deletions(-) diff --git a/mavctl/connect/conn.py b/mavctl/connect/conn.py index c6d9e01..44b80c7 100644 --- a/mavctl/connect/conn.py +++ b/mavctl/connect/conn.py @@ -13,9 +13,9 @@ def __init__(self, baud: int = 57600, heartbeat_timeout = None): - self.mav = self.connect(ip = ip, baud = baud, heartbeat_timeout = heartbeat_timeout) + self.master = self.connect(ip = ip, baud = baud, heartbeat_timeout = heartbeat_timeout) - self._heartbeat_manager = HeartbeatManager(self.mav) + self._heartbeat_manager = HeartbeatManager(self.master) self._heartbeat_manager.start() @@ -23,7 +23,7 @@ def send_heartbeat(self, interval: int = 0.25): try: while True: - self.mav.mav.heartbeat_send( + self.master.mav.heartbeat_send( type=mavutil.mavlink.MAV_TYPE_GCS, autopilot=mavutil.mavlink.MAV_AUTOPILOT_INVALID, base_mode=0, @@ -39,7 +39,7 @@ def recv_heartbeat(self, interval: int = 1): try: while True: - msg_recv = self.mav.recv_match(type="HEARTBEAT", blocking=False) + msg_recv = self.master.recv_match(type="HEARTBEAT", blocking=False) # Create disconnect handling here... time.sleep(interval) @@ -58,7 +58,7 @@ def heartbeat_kill(self): def disconnect(self): self.heartbeat_kill() - self.mav.close() + self.master.close() print("MAVCTL: Disconnecting!") # Function to connect to mavlink diff --git a/mavctl/connect/heartbeat.py b/mavctl/connect/heartbeat.py index 39e83e2..0ad863b 100644 --- a/mavctl/connect/heartbeat.py +++ b/mavctl/connect/heartbeat.py @@ -18,7 +18,7 @@ class HeartbeatManager: Triggers callbacks when connection is established or lost after N missed heartbeats. """ - def __init__(self, mav, heartbeat_timeout: float = 1.0, max_missed_heartbeats: int = 2): + def __init__(self, master, heartbeat_timeout: float = 1.0, max_missed_heartbeats: int = 2): """Initialize the heartbeat manager. Args: @@ -28,7 +28,7 @@ def __init__(self, mav, heartbeat_timeout: float = 1.0, max_missed_heartbeats: i max_missed_heartbeats: Number of consecutive heartbeats that can be missed before connection is considered lost (default 2). """ - self.mav = mav + self.master = master self.heartbeat_timeout = heartbeat_timeout self.max_missed_heartbeats = max_missed_heartbeats self.last_heartbeat = 0 @@ -73,7 +73,7 @@ def stop(self): def _monitor_heartbeat(self): """Monitor heartbeat messages and manage connection status.""" while not self._stop_event.is_set(): - msg = self.mav.recv_match( + msg = self.master.recv_match( type="HEARTBEAT", blocking=True, timeout=self.heartbeat_timeout @@ -99,7 +99,7 @@ def _monitor_heartbeat(self): def _send_heartbeat(self): """Send heartbeat messages""" while not self._stop_event.is_set(): - self.mav.mav.heartbeat_send( + self.master.mav.heartbeat_send( type=mavutil.mavlink.MAV_TYPE_GCS, autopilot=mavutil.mavlink.MAV_AUTOPILOT_INVALID, base_mode=0, diff --git a/mavctl/messages/advanced.py b/mavctl/messages/advanced.py index f81a675..5db7f54 100644 --- a/mavctl/messages/advanced.py +++ b/mavctl/messages/advanced.py @@ -1,8 +1,8 @@ import time -from messages import util -from messages.location import LocationGlobal, LocationGlobalRelative, LocationLocal +from mavctl.messages import util +from mavctl.messages.location import LocationGlobal, LocationGlobalRelative, LocationLocal from math import radians, atan -from messages.util import Heading, LatLon_to_Distance +from mavctl.messages.util import Heading, LatLon_to_Distance # This file is meant for implementing more advanced features in mavctl @@ -22,8 +22,5 @@ def simple_goto_global(master, lat, lon, alt): 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) - print("Sent set position message") origin = master.get_global_origin() - print("Waiting for drone to reach target") master.wait_target_reached_global(LocationGlobal(lat, lon, alt)) - print("reached target") diff --git a/mavctl/messages/navigator.py b/mavctl/messages/navigator.py index 2c92f33..9fbc34a 100644 --- a/mavctl/messages/navigator.py +++ b/mavctl/messages/navigator.py @@ -7,8 +7,8 @@ class Navigator: - def __init__(self, mav): - self.mav = mav + def __init__(self, master): + self.master = master def arm(self): @@ -18,9 +18,9 @@ def arm(self): NOTE: ONLY FOR USE IN SIMULATED SCRIPTS """ - self.mav.mav.command_long_send( - self.mav.target_system, - self.mav.target_component, + self.master.mav.command_long_send( + self.master.target_system, + self.master.target_component, mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, 1, @@ -33,7 +33,7 @@ def arm(self): ) print("MAVCTL: Waiting for vehicle to arm") - self.mav.motors_armed_wait() + self.master.motors_armed_wait() print("MAVCTL: Armed!") def wait_vehicle_armed(self): @@ -41,7 +41,7 @@ def wait_vehicle_armed(self): Waits for the vehicle to be armed. See samples directory for examples """ print("MAVCTL: Waiting for vehicle to arm") - self.mav.motors_armed_wait() + self.master.motors_armed_wait() print("Armed!") def disarm(self): @@ -51,9 +51,9 @@ def disarm(self): NOTE: ONLY FOR USE IN SIMULATED SCRIPTS """ - self.mav.mav.command_long_send( - self.mav.target_system, - self.mav.target_component, + self.master.mav.command_long_send( + self.master.target_system, + self.master.target_component, mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, 0, @@ -66,7 +66,7 @@ def disarm(self): ) print("MAVCTL: Waiting for vehicle to disarm") - self.mav.motors_disarmed_wait() + self.master.motors_disarmed_wait() print("MAVCTL: Disarmed!") def set_mode_wait(self, mode = "GUIDED", timeout = None) -> bool: @@ -79,8 +79,7 @@ def set_mode_wait(self, mode = "GUIDED", timeout = None) -> bool: """ start_time = time.time() - mode_mapping = self.mav.mode_mapping() - print(mode_mapping) + mode_mapping = self.master.mode_mapping() if mode not in mode_mapping: raise ValueError("MAVCTL Error: Mode " + mode + "not recognized") @@ -92,18 +91,17 @@ def set_mode_wait(self, mode = "GUIDED", timeout = None) -> bool: print("MAVCTL: Timeout waiting for " + mode + " mode") return False - self.mav.mav.request_data_stream_send( - self.mav.target_system, - self.mav.target_component, + 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.mav.recv_match(type = "HEARTBEAT", blocking = True, timeout = 0.25) + msg = self.master.recv_match(type = "HEARTBEAT", blocking = True, timeout = 0.25) if msg: current_mode_id = msg.custom_mode - print(current_mode_id, mode_id) if current_mode_id == mode_id: print("MAVCTL: Set to " + mode + " mode") return True @@ -115,7 +113,7 @@ def get_global_position(self): Altitude is a value which is measured from sea level and is positive """ - msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) + msg = self.master.recv_match(type='GLOBAL_POSITION_INT', blocking=True) if msg: lat = msg.lat / 1e7 lon = msg.lon / 1e7 @@ -130,7 +128,7 @@ def get_local_position(self): DOWN is positive """ - msg = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True) + msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=True) if msg: north = msg.x east = msg.y @@ -142,9 +140,9 @@ 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.mav.mav.command_long_send( - self.mav.target_system, - self.mav.target_component, + self.master.mav.command_long_send( + self.master.target_system, + self.master.target_component, mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE, 0, 242, @@ -155,7 +153,7 @@ def get_global_origin(self): 0, 0) - msg = self.mav.recv_match(type='HOME_POSITION', blocking=True) + msg = self.master.recv_match(type='HOME_POSITION', blocking=True) print(msg) if msg: lat = msg.latitude / 1e7 @@ -168,7 +166,7 @@ def get_velocity(self): """ Gets velocity of drone in NED coordinates """ - msg = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True) + msg = self.master.recv_match(type='LOCAL_POSITION_NED', blocking=True) if msg: north = msg.vx east = msg.vy @@ -188,9 +186,9 @@ def takeoff(self, altitude, pitch=15): """ print("MAVCTL: Taking Off to: " + str(altitude) + "m") - self.mav.mav.command_long_send( - self.mav.target_system, - self.mav.target_component, + self.master.mav.command_long_send( + self.master.target_system, + self.master.target_component, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, pitch, @@ -213,9 +211,9 @@ def vtol_takeoff(self, altitude, pitch=15): """ print("MAVCTL: Taking Off to: " + str(altitude) + "m") - self.mav.mav.command_long_send( - self.mav.target_system, - self.mav.target_component, + self.master.mav.command_long_send( + self.master.target_system, + self.master.target_component, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, pitch, @@ -231,9 +229,9 @@ def vtol_takeoff(self, altitude, pitch=15): def return_to_launch(self): print("MAVCTL: RTL") - self.mav.mav.command_long_send( - self.mav.target_system, - self.mav.target_component, + self.master.mav.command_long_send( + self.master.target_system, + self.master.target_component, mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, @@ -277,10 +275,10 @@ def set_position_local_ned(self, x, y, z is in meters in NED coordinates (down is positive) """ - self.mav.mav.set_position_target_local_ned_send( + self.master.mav.set_position_target_local_ned_send( 0, - self.mav.target_system, - self.mav.target_component, + self.master.target_system, + self.master.target_component, coordinate_frame, type_mask, x, @@ -303,9 +301,9 @@ def set_speed(self, speed): #(WIP) DO NOT USE IN REAL FLIGHT, THIS METHOD HAS NOT BEEN VERIFIED YET. - self.mav.mav.param_set_send( - self.mav.target_system, - self.mav.target_component, + 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) @@ -331,9 +329,9 @@ def set_position_global(self, """ - self.mav.mav.set_position_target_global_int_send(0, - self.mav.target_system, - self.mav.target_component, + 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), @@ -359,9 +357,9 @@ def do_reposition(self, Alt is set to be relative altitude (ground is 0m) """ - self.mav.mav.command_int_send( - self.mav.target_system, - self.mav.target_component, + 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, @@ -379,9 +377,9 @@ 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.mav.mav.command_long_send( - self.mav.target_system, - self.mav.target_component, + 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, @@ -398,9 +396,9 @@ 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.mav.mav.command_long_send( - self.mav.target_system, - self.mav.target_component, + 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, diff --git a/mavctl/tests/flight_tests/first_flight.py b/mavctl/tests/flight_tests/first_flight.py index 1933777..0da3ffe 100644 --- a/mavctl/tests/flight_tests/first_flight.py +++ b/mavctl/tests/flight_tests/first_flight.py @@ -1,24 +1,23 @@ -from connect import conn +from mavctl.connect.conn import Connect from pymavlink import mavutil -from messages.Navigator import Navigator -from messages.messenger import Messenger -from messages import advanced +from mavctl.messages.navigator import Navigator +from mavctl.messages.messenger import Messenger +from mavctl.messages import advanced import time CONN_STR = "udp:127.0.0.1:14551" -mav = conn.connect() -master = Navigator(mav) -master.send_status_message("MAVCTL: Online") +connect = Connect(ip = CONN_STR) +drone = Navigator(connect.master) -while master.wait_vehicle_armed(): +while drone.wait_vehicle_armed(): pass -while not master.set_mode_wait(): +while not drone.set_mode_wait(): pass -master.takeoff(10) +drone.takeoff(10) time.sleep(5) -advanced.simple_goto_global(master, 53.496970, -113.545194, 20) +advanced.simple_goto_global(drone, 53.496970, -113.545194, 20) -master.return_to_launch() +drone.return_to_launch()