Publish heartbeat message every second

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:

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

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", []),
         },
     )
 

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
 

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