diff --git a/radio/app/drone.py b/radio/app/drone.py index 8a8fb8826..f03453ecf 100644 --- a/radio/app/drone.py +++ b/radio/app/drone.py @@ -4,9 +4,9 @@ import traceback from logging import Logger, getLogger from pathlib import Path -from queue import Queue +from queue import Empty, Queue from secrets import token_hex -from threading import Lock, Thread +from threading import Event, Lock, Thread from typing import Callable, Dict, List, Optional import serial @@ -165,7 +165,8 @@ def __init__( self.sendConnectionStatusUpdate("Cleaned temp logs") - self.is_active = True + self.is_active = Event() + self.is_active.set() self.is_listening = False # To ensure that only one command is sent at a time and we wait for a @@ -434,7 +435,7 @@ def removeMessageListener(self, message_id: str) -> bool: def checkForMessages(self) -> None: """Check for messages from the drone and add them to the message queue.""" - while self.is_active: + while self.is_active.is_set(): if not self.is_listening: time.sleep(0.05) # Sleep a bit to not clog up processing usage continue @@ -455,8 +456,7 @@ def checkForMessages(self) -> None: self.logger.error(e, exc_info=True) if self.droneDisconnectCb: self.droneDisconnectCb() - self.is_listening = False - self.is_active = False + self.close() break except Exception as e: # Log any other unexpected exception @@ -498,10 +498,12 @@ def checkForMessages(self) -> None: def executeMessages(self) -> None: """Executes message listeners based on messages from the message queue.""" - while self.is_active: + while self.is_active.is_set(): try: - q = self.message_queue.get() + q = self.message_queue.get(timeout=1) self.message_listeners[q[0]](q[1]) + except Empty: + continue except KeyError as e: self.logger.error( f"Could not execute message (likely due to backend abruptly stopping): {e}" @@ -511,45 +513,52 @@ def logMessages(self) -> None: """A thread to log messages into a temp FTLog file from the log queue.""" current_line_number = 0 - while self.is_active: - log_msg = self.log_message_queue.get() - if log_msg: - # Check if a temp log file has been created yet, if not create one - if self.current_log_file is None: - self.current_log_file = self.log_directory.joinpath( - f"tmp_first_{token_hex(8)}.ftlog" - ) - self.log_file_names.append(self.current_log_file) - with open(self.current_log_file, "w") as f: - f.write( - f"==START_TIME=={self.__getCurrentDateTimeStr()}==END==\n" + while self.is_active.is_set(): + try: + log_msg = self.log_message_queue.get(timeout=1) + if log_msg: + # Check if a temp log file has been created yet, if not create one + if self.current_log_file is None: + self.current_log_file = self.log_directory.joinpath( + f"tmp_first_{token_hex(8)}.ftlog" ) + self.log_file_names.append(self.current_log_file) + with open(self.current_log_file, "w") as f: + f.write( + f"==START_TIME=={self.__getCurrentDateTimeStr()}==END==\n" + ) - # Write the incoming telemetry message to the temp log file - if current_line_number < LOG_LINE_LIMIT: - with open(self.current_log_file, "a") as current_log_file_handler: - current_log_file_handler.write(log_msg + "\n") - current_line_number += 1 - else: - # If the current log file has reached the line limit, create a new temp log file - next_log_file_name = self.log_directory.joinpath( - f"tmp_{token_hex(8)}.ftlog" - ) - with open(self.current_log_file, "a") as current_log_file_handler: - # Write the next file name to the current log file - current_log_file_handler.write( - f"==NEXT_FILE=={str(next_log_file_name)}==END==\n" + # Write the incoming telemetry message to the temp log file + if current_line_number < LOG_LINE_LIMIT: + with open( + self.current_log_file, "a" + ) as current_log_file_handler: + current_log_file_handler.write(log_msg + "\n") + current_line_number += 1 + else: + # If the current log file has reached the line limit, create a new temp log file + next_log_file_name = self.log_directory.joinpath( + f"tmp_{token_hex(8)}.ftlog" ) + with open( + self.current_log_file, "a" + ) as current_log_file_handler: + # Write the next file name to the current log file + current_log_file_handler.write( + f"==NEXT_FILE=={str(next_log_file_name)}==END==\n" + ) - self.current_log_file = next_log_file_name - self.log_file_names.append(self.current_log_file) + self.current_log_file = next_log_file_name + self.log_file_names.append(self.current_log_file) - with open(self.current_log_file, "w") as f: - f.write( - f"==START_TIME=={self.__getCurrentDateTimeStr()}==END==\n" - ) - f.write(log_msg + "\n") - current_line_number = 1 + with open(self.current_log_file, "w") as f: + f.write( + f"==START_TIME=={self.__getCurrentDateTimeStr()}==END==\n" + ) + f.write(log_msg + "\n") + current_line_number = 1 + except Empty: + continue def getLinkDebugData(self) -> None: """While active, get link debug data""" @@ -573,7 +582,7 @@ def getLinkDebugData(self) -> None: "uptime": 0, } - while self.is_active: + while self.is_active.is_set(): if self.linkDebugStatsCb: try: link_stats = { @@ -630,16 +639,44 @@ def getLinkDebugData(self) -> None: time.sleep(1 / refresh_rate_hz) + def sendHeartbeatMessage(self) -> None: + """Sends a heartbeat message to the drone every second.""" + while self.is_active.is_set(): + try: + self.master.mav.heartbeat_send( + mavutil.mavlink.MAV_TYPE_GCS, + mavutil.mavlink.MAV_AUTOPILOT_INVALID, + 0, + 0, + 0, + ) + except Exception as e: + self.logger.error(f"Failed to send heartbeat: {e}", exc_info=True) + time.sleep(1) + def startThread(self) -> None: """Starts the listener and sender threads.""" self.listener_thread = Thread(target=self.checkForMessages, daemon=True) self.sender_thread = Thread(target=self.executeMessages, daemon=True) self.log_thread = Thread(target=self.logMessages, daemon=True) self.link_debug_data_thread = Thread(target=self.getLinkDebugData, daemon=True) + self.heartbeat_thread = Thread(target=self.sendHeartbeatMessage, daemon=True) self.listener_thread.start() self.sender_thread.start() self.log_thread.start() self.link_debug_data_thread.start() + self.heartbeat_thread.start() + + def stopAllThreads(self) -> None: + """Stops all threads.""" + self.is_active.clear() + self.is_listening = False + + self.listener_thread.join(timeout=3) + self.sender_thread.join(timeout=3) + self.log_thread.join(timeout=3) + self.link_debug_data_thread.join(timeout=3) + self.heartbeat_thread.join(timeout=3) def rebootAutopilot(self) -> None: """Reboot the autopilot.""" @@ -648,7 +685,7 @@ def rebootAutopilot(self) -> None: self.sendCommand( mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, - param1=1, # Autpilot + param1=1, # Autopilot param2=0, # Companion param3=0, # Component action param4=0, # Component ID @@ -815,7 +852,7 @@ def close(self) -> None: self.removeMessageListener(message_id) self.stopAllDataStreams() - self.is_active = False + self.stopAllThreads() self.master.close() if len(self.log_file_names) == 0: diff --git a/radio/app/endpoints/autopilot.py b/radio/app/endpoints/autopilot.py index 8f574a443..9b4bd9a24 100644 --- a/radio/app/endpoints/autopilot.py +++ b/radio/app/endpoints/autopilot.py @@ -23,7 +23,7 @@ def rebootAutopilot() -> None: socketio.emit("disconnected_from_drone") droneStatus.drone.rebootAutopilot() - while droneStatus.drone is not None and droneStatus.drone.is_active: + while droneStatus.drone is not None and droneStatus.drone.is_active.is_set(): time.sleep(0.05) tries = 0 diff --git a/radio/app/endpoints/mission.py b/radio/app/endpoints/mission.py index ab9c982ef..4949a70f2 100644 --- a/radio/app/endpoints/mission.py +++ b/radio/app/endpoints/mission.py @@ -1,3 +1,5 @@ +from typing import Any + from typing_extensions import TypedDict import app.droneStatus as droneStatus @@ -105,12 +107,16 @@ def getCurrentMissionAll() -> None: result = droneStatus.drone.missionController.getCurrentMissionAll() + data: dict[str, Any] = result.get("data", {}) + if not isinstance(data, dict): + data = {} + socketio.emit( "current_mission_all", { - "mission_items": result.get("data", {}).get("mission_items", []), - "fence_items": result.get("data", {}).get("fence_items", []), - "rally_items": result.get("data", {}).get("rally_items", []), + "mission_items": data.get("mission_items", []), + "fence_items": data.get("fence_items", []), + "rally_items": data.get("rally_items", []), }, ) diff --git a/radio/tests/test_comPorts.py b/radio/tests/test_comPorts.py index 8fd8e7e9d..e925e756e 100644 --- a/radio/tests/test_comPorts.py +++ b/radio/tests/test_comPorts.py @@ -1,17 +1,15 @@ import sys -import pytest -from typing import Union - -from serial.tools import list_ports -from serial.tools.list_ports_common import ListPortInfo +import pytest from app import droneStatus from app.drone import Drone +from serial.tools import list_ports + from . import socketio_client from .conftest import setupDrone from .helpers import send_and_recieve -VALID_DRONE_PORT: Union[str, ListPortInfo] +VALID_DRONE_PORT: str @pytest.fixture(scope="module", autouse=True) @@ -23,9 +21,6 @@ def run_once_after_all_tests(): global VALID_DRONE_PORT VALID_DRONE_PORT = droneStatus.drone.port - # Get the connection string - if isinstance(VALID_DRONE_PORT, ListPortInfo): - VALID_DRONE_PORT = VALID_DRONE_PORT.device droneStatus.drone.logger.info(f"Found drone running on port {VALID_DRONE_PORT}") yield diff --git a/radio/tests/test_connections.py b/radio/tests/test_connections.py index 5db228747..b308d69b2 100644 --- a/radio/tests/test_connections.py +++ b/radio/tests/test_connections.py @@ -1,6 +1,5 @@ import pytest from flask_socketio.test_client import SocketIOTestClient -from serial.tools.list_ports_common import ListPortInfo from . import falcon_test @@ -17,10 +16,6 @@ def run_once_after_all_tests(): assert droneStatus.drone is not None VALID_DRONE_PORT = droneStatus.drone.port - # Get the connection string - if isinstance(VALID_DRONE_PORT, ListPortInfo): - VALID_DRONE_PORT = VALID_DRONE_PORT.device - droneStatus.drone.logger.info(f"Found drone running on port {VALID_DRONE_PORT}") yield