diff --git a/mavctl/connect/conn.py b/mavctl/connect/conn.py index 66ab8f1..44b80c7 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,22 +13,17 @@ 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.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): 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, @@ -43,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) @@ -62,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 new file mode 100644 index 0000000..0ad863b --- /dev/null +++ b/mavctl/connect/heartbeat.py @@ -0,0 +1,121 @@ +"""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. +""" +# pylint: disable=too-many-instance-attributes + +import threading +import time +from typing import Callable +from pymavlink import mavutil + +class HeartbeatManager: + """Monitor MAVLink heartbeat messages and detect connection loss. + + 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, master, 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 (default 1.0). + max_missed_heartbeats: Number of consecutive heartbeats that can be + missed before connection is considered lost (default 2). + """ + self.master = master + 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._send_thread = None + self._callbacks = { + 'on_connection_lost': None, + 'on_connection_established': 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 called when connection is lost. + on_connection_established: Callback function called when connection + is 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) + 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.""" + while not self._stop_event.is_set(): + msg = self.master.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 + callback = self._callbacks['on_connection_established'] + if callback is not None: + callback() + else: + self.missed_heartbeats += 1 + if (self.missed_heartbeats >= self.max_missed_heartbeats + and self.is_connected): + self.is_connected = False + callback = self._callbacks['on_connection_lost'] + if callback is not None: + callback() + + def _send_heartbeat(self): + """Send heartbeat messages""" + while not self._stop_event.is_set(): + 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(self.heartbeat_timeout) + + 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 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()