From 5a567b1994202c095ad3741cae0c520854b64381 Mon Sep 17 00:00:00 2001 From: Kush Makkapati Date: Thu, 30 Oct 2025 21:54:47 +0000 Subject: [PATCH 01/12] Refactor message handling in controllers to use reservation system, reset dashboard statustext messages when connecting to an aircraft --- gcs/src/redux/middleware/emitters.js | 6 +- gcs/src/redux/middleware/socketMiddleware.js | 3 +- radio/app/controllers/armController.py | 69 +++-- .../app/controllers/flightModesController.py | 19 +- radio/app/controllers/gripperController.py | 42 ++- radio/app/controllers/missionController.py | 290 ++++++++++-------- radio/app/controllers/motorTestController.py | 174 ++++++----- radio/app/controllers/navController.py | 147 +++++---- radio/app/controllers/paramsController.py | 182 ++++++----- radio/app/drone.py | 211 +++++++++---- 10 files changed, 703 insertions(+), 440 deletions(-) diff --git a/gcs/src/redux/middleware/emitters.js b/gcs/src/redux/middleware/emitters.js index ac05ec2f2..39171e17d 100644 --- a/gcs/src/redux/middleware/emitters.js +++ b/gcs/src/redux/middleware/emitters.js @@ -50,6 +50,7 @@ import { emitRefreshParams, emitSetMultipleParams, } from "../slices/paramsSlice" +import { resetMessages } from "../slices/statusTextSlice" export function handleEmitters(socket, store, action) { if (!socket) return @@ -73,7 +74,10 @@ export function handleEmitters(socket, store, action) { }, { emitter: emitConnectToDrone, - callback: () => socket.socket.emit("connect_to_drone", action.payload), + callback: () => { + socket.socket.emit("connect_to_drone", action.payload) + store.dispatch(resetMessages()) + }, }, { emitter: emitStartForwarding, diff --git a/gcs/src/redux/middleware/socketMiddleware.js b/gcs/src/redux/middleware/socketMiddleware.js index 666b43be1..2de0dcc65 100644 --- a/gcs/src/redux/middleware/socketMiddleware.js +++ b/gcs/src/redux/middleware/socketMiddleware.js @@ -98,7 +98,7 @@ import { setShownParams, updateParamValue, } from "../slices/paramsSlice.js" -import { pushMessage, resetMessages } from "../slices/statusTextSlice.js" +import { pushMessage } from "../slices/statusTextSlice.js" import { handleEmitters } from "./emitters.js" const SocketEvents = Object.freeze({ @@ -354,7 +354,6 @@ const socketMiddleware = (store) => { store.dispatch(setAutoPilotRebootModalOpen(false)) store.dispatch(setShouldFetchAllMissionsOnDashboard(true)) store.dispatch(setShowMotorTestWarningModal(true)) - store.dispatch(resetMessages()) }) // Link stats diff --git a/radio/app/controllers/armController.py b/radio/app/controllers/armController.py index b01f71e9a..6b040c611 100644 --- a/radio/app/controllers/armController.py +++ b/radio/app/controllers/armController.py @@ -1,6 +1,7 @@ from __future__ import annotations import time +from threading import current_thread from typing import TYPE_CHECKING from app.customTypes import Response @@ -19,6 +20,7 @@ def __init__(self, drone: Drone) -> None: Args: drone (Drone): The main drone object """ + self.controller_id = f"arm_{current_thread().ident}" self.drone = drone @sendingCommandLock @@ -35,17 +37,24 @@ def arm(self, force: bool = False) -> Response: if self.drone.armed: return {"success": False, "message": "Already armed"} - self.drone.is_listening = False - - self.drone.sendCommand( - mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - param1=1, # 0=disarm, 1=arm - param2=2989 if force else 0, # force arm/disarm - ) + if not self.drone.reserve_message_type("COMMAND_ACK", self.controller_id): + return { + "success": False, + "message": "Could not reserve COMMAND_ACK messages", + } try: - response = self.drone.master.recv_match(type="COMMAND_ACK", blocking=True) - self.drone.is_listening = True + self.drone.sendCommand( + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + param1=1, # 0=disarm, 1=arm + param2=2989 if force else 0, # force arm/disarm + ) + + response = self.drone.wait_for_message( + "COMMAND_ACK", + self.controller_id, + timeout=3, + ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM): # Wait for the drone to be armed fully after the command has been accepted @@ -56,13 +65,18 @@ def arm(self, force: bool = False) -> Response: return {"success": True, "message": "Armed successfully"} else: self.drone.logger.debug("Arming failed") + return { + "success": False, + "message": "Could not arm, command not accepted", + } + except Exception as e: - self.drone.is_listening = True self.drone.logger.error(e, exc_info=True) if self.drone.droneErrorCb: self.drone.droneErrorCb(str(e)) return {"success": False, "message": "Could not arm, serial exception"} - return {"success": False, "message": "Could not arm, command not accepted"} + finally: + self.drone.release_message_type("COMMAND_ACK", self.controller_id) @sendingCommandLock def disarm(self, force: bool = False) -> Response: @@ -78,17 +92,24 @@ def disarm(self, force: bool = False) -> Response: if not self.drone.armed: return {"success": False, "message": "Already disarmed"} - self.drone.is_listening = False - - self.drone.sendCommand( - mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - param1=0, # 0=disarm, 1=arm - param2=2989 if force else 0, # force arm/disarm - ) + if not self.drone.reserve_message_type("COMMAND_ACK", self.controller_id): + return { + "success": False, + "message": "Could not reserve COMMAND_ACK messages", + } try: - response = self.drone.master.recv_match(type="COMMAND_ACK", blocking=True) - self.drone.is_listening = True + self.drone.sendCommand( + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + param1=0, # 0=disarm, 1=arm + param2=2989 if force else 0, # force arm/disarm + ) + + response = self.drone.wait_for_message( + "COMMAND_ACK", + self.controller_id, + timeout=3, + ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM): # Wait for the drone to be disarmed fully after the command has been accepted @@ -99,11 +120,17 @@ def disarm(self, force: bool = False) -> Response: return {"success": True, "message": "Disarmed successfully"} else: self.drone.logger.debug("Could not disarm, command not accepted") + return { + "success": False, + "message": "Could not disarm, command not accepted", + } + except Exception as e: - self.drone.is_listening = True self.drone.logger.error(e, exc_info=True) if self.drone.droneErrorCb: self.drone.droneErrorCb(str(e)) return {"success": False, "message": "Could not disarm, serial exception"} + finally: + self.drone.release_message_type("COMMAND_ACK", self.controller_id) return {"success": False, "message": "Could not disarm, command not accepted"} diff --git a/radio/app/controllers/flightModesController.py b/radio/app/controllers/flightModesController.py index 9dd6b7a82..874c17f06 100644 --- a/radio/app/controllers/flightModesController.py +++ b/radio/app/controllers/flightModesController.py @@ -1,6 +1,7 @@ from __future__ import annotations import time +from threading import current_thread from typing import TYPE_CHECKING, List, Union import serial @@ -30,6 +31,7 @@ def __init__(self, drone: Drone) -> None: Args: drone (Drone): The main drone object """ + self.controller_id = f"flightmodes_{current_thread().ident}" self.drone = drone @@ -138,7 +140,11 @@ def setCurrentFlightMode(self, flightMode: int) -> Response: Returns: A message to show if the drone recieved the message and succesfully set the new mode """ - self.drone.is_listening = False + if not self.drone.reserve_message_type("COMMAND_ACK", self.controller_id): + return { + "success": False, + "message": "Could not reserve COMMAND_ACK messages", + } time.sleep(0.3) self.drone.sendCommand( message=mavutil.mavlink.MAV_CMD_DO_SET_MODE, @@ -152,26 +158,27 @@ def setCurrentFlightMode(self, flightMode: int) -> Response: ) try: - response = self.drone.master.recv_match( - type="COMMAND_ACK", blocking=True, timeout=3 + response = self.drone.wait_for_message( + "COMMAND_ACK", + self.controller_id, + timeout=3, ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_SET_MODE): - self.drone.is_listening = True self.drone.logger.info("Flight mode set successfully") return {"success": True, "message": "Flight mode set successfully"} else: - self.drone.is_listening = True return { "success": False, "message": "Could not set flight mode", } except serial.serialutil.SerialException: - self.drone.is_listening = True return { "success": False, "message": "Could not set flight mode, serial exception", } + finally: + self.drone.release_message_type("COMMAND_ACK", self.controller_id) def setGuidedMode(self) -> Response: """ diff --git a/radio/app/controllers/gripperController.py b/radio/app/controllers/gripperController.py index a6d11520b..1cae88b50 100644 --- a/radio/app/controllers/gripperController.py +++ b/radio/app/controllers/gripperController.py @@ -1,5 +1,6 @@ from __future__ import annotations +from threading import current_thread from typing import TYPE_CHECKING import serial @@ -29,6 +30,7 @@ def __init__(self, drone: Drone) -> None: Args: drone (Drone): The main drone object """ + self.controller_id = f"gripper_{current_thread().ident}" self.drone = drone self.params: dict = {} @@ -109,24 +111,32 @@ def setGripper(self, action: str) -> Response: "message": 'Gripper action must be either "release" or "grab"', } - self.drone.is_listening = False - self.drone.sending_command_lock.acquire() + if not self.drone.reserve_message_type("COMMAND_ACK", self.controller_id): + return { + "success": False, + "message": "Could not reserve COMMAND_ACK messages", + } - self.drone.sendCommand( - mavutil.mavlink.MAV_CMD_DO_GRIPPER, - 0, - 0 if action == "release" else 1, - 0, - 0, - 0, - 0, - 0, - ) + self.drone.sending_command_lock.acquire() try: - response = self.drone.master.recv_match(type="COMMAND_ACK", blocking=True) + self.drone.sendCommand( + mavutil.mavlink.MAV_CMD_DO_GRIPPER, + 0, + 0 if action == "release" else 1, + 0, + 0, + 0, + 0, + 0, + ) + + response = self.drone.wait_for_message( + "COMMAND_ACK", + self.controller_id, + timeout=3, + ) - self.drone.is_listening = True self.drone.sending_command_lock.release() if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_GRIPPER): @@ -139,13 +149,15 @@ def setGripper(self, action: str) -> Response: "success": False, "message": "Setting gripper failed", } + except serial.serialutil.SerialException: - self.drone.is_listening = True self.drone.sending_command_lock.release() return { "success": False, "message": "Setting gripper failed, serial exception", } + finally: + self.drone.release_message_type("COMMAND_ACK", self.controller_id) def getConfig(self) -> dict: """ diff --git a/radio/app/controllers/missionController.py b/radio/app/controllers/missionController.py index cca6f642b..81a480351 100644 --- a/radio/app/controllers/missionController.py +++ b/radio/app/controllers/missionController.py @@ -1,6 +1,7 @@ from __future__ import annotations import os +from threading import current_thread from typing import TYPE_CHECKING, Any, Callable, List, Optional import serial @@ -27,6 +28,7 @@ def __init__(self, drone: Drone) -> None: """ self.drone = drone + self.controller_id = f"MissionController_{current_thread().ident}" # Loaders are only used to manage the mission items that are currently loaded in the drone. # Importing and exporting mission items to/from files do not use loaders as these waypoints @@ -188,35 +190,36 @@ def getMissionItems( else: loader = self.rallyLoader - self.drone.is_listening = False - - try: - self.drone.master.mav.mission_request_list_send( - self.drone.target_system, - mavutil.mavlink.MAV_COMP_ID_AUTOPILOT1, - mission_type=mission_type, - ) - except TypeError: - self.drone.is_listening = True - # TypeError is raised if mavlink V1 is used where the mission_request_list_send - # function does not have a mission_type parameter - self.drone.logger.error( - "Failed to request mission list from autopilot, got type error. Trying again without mission type." - ) + if not self.drone.reserve_message_type("MISSION_COUNT", self.controller_id): return { "success": False, - "message": "Failed to request mission list from autopilot, got type error. Try reconnecting to the drone.", + "message": "Could not reserve MISSION_COUNT messages", } try: - response = self.drone.master.recv_match( - type=[ - "MISSION_COUNT", - ], - blocking=True, + try: + self.drone.master.mav.mission_request_list_send( + self.drone.target_system, + mavutil.mavlink.MAV_COMP_ID_AUTOPILOT1, + mission_type=mission_type, + ) + except TypeError: + # TypeError is raised if mavlink V1 is used where the mission_request_list_send + # function does not have a mission_type parameter + self.drone.logger.error( + "Failed to request mission list from autopilot, got type error. Trying again without mission type." + ) + return { + "success": False, + "message": "Failed to request mission list from autopilot, got type error. Try reconnecting to the drone.", + } + + response = self.drone.wait_for_message( + "MISSION_COUNT", + self.controller_id, timeout=2, + condition_func=lambda msg: msg.mission_type == mission_type, ) - self.drone.is_listening = True if response: if response.mission_type != mission_type: @@ -284,11 +287,12 @@ def getMissionItems( "message": failure_message, } except serial.serialutil.SerialException: - self.drone.is_listening = True return { "success": False, "message": f"{failure_message}, serial exception", } + finally: + self.drone.release_message_type("MISSION_COUNT", self.controller_id) def getItemDetails( self, item_number: int, mission_type: int, mission_count: int @@ -310,24 +314,28 @@ def getItemDetails( failure_message = f"Failed to get mission item {item_number + 1}/{mission_count} for mission type {mission_type}" - self.drone.is_listening = False - # The sending_command_lock is held by the caller function getMissionItems - - self.drone.master.mav.mission_request_int_send( - self.drone.target_system, - mavutil.mavlink.MAV_COMP_ID_AUTOPILOT1, - item_number, - mission_type=mission_type, - ) + if not self.drone.reserve_message_type("MISSION_ITEM_INT", self.controller_id): + return { + "success": False, + "message": "Could not reserve MISSION_ITEM_INT messages", + } try: - response = self.drone.master.recv_match( - type="MISSION_ITEM_INT", - blocking=True, - timeout=1.5, + # The sending_command_lock is held by the caller function getMissionItems + self.drone.master.mav.mission_request_int_send( + self.drone.target_system, + mavutil.mavlink.MAV_COMP_ID_AUTOPILOT1, + item_number, + mission_type=mission_type, ) - self.drone.is_listening = True + response = self.drone.wait_for_message( + "MISSION_ITEM_INT", + self.controller_id, + timeout=1.5, + condition_func=lambda msg: msg.mission_type == mission_type + and msg.seq == item_number, + ) if response: self.drone.logger.debug( @@ -348,7 +356,6 @@ def getItemDetails( } except serial.serialutil.SerialException: - self.drone.is_listening = True self.drone.logger.error( f"Got no response for mission item {item_number + 1}/{mission_count}, serial exception" ) @@ -356,6 +363,8 @@ def getItemDetails( "success": False, "message": f"{failure_message}, serial exception", } + finally: + self.drone.release_message_type("MISSION_ITEM_INT", self.controller_id) @sendingCommandLock def startMission(self) -> Response: @@ -365,16 +374,22 @@ def startMission(self) -> Response: Returns: Dict: The response of the mission start request """ - self.drone.is_listening = False - - self.drone.sendCommand( - mavutil.mavlink.MAV_CMD_MISSION_START, - ) + if not self.drone.reserve_message_type("COMMAND_ACK", self.controller_id): + return { + "success": False, + "message": "Could not reserve COMMAND_ACK messages", + } try: - response = self.drone.master.recv_match(type="COMMAND_ACK", blocking=True) + self.drone.sendCommand( + mavutil.mavlink.MAV_CMD_MISSION_START, + ) - self.drone.is_listening = True + response = self.drone.wait_for_message( + "COMMAND_ACK", + self.controller_id, + timeout=3, + ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_MISSION_START): return { @@ -387,11 +402,12 @@ def startMission(self) -> Response: "message": "Failed to start mission", } except serial.serialutil.SerialException: - self.drone.is_listening = True return { "success": False, "message": "Failed to start mission, serial exception", } + finally: + self.drone.release_message_type("COMMAND_ACK", self.controller_id) @sendingCommandLock def restartMission(self) -> Response: @@ -401,14 +417,22 @@ def restartMission(self) -> Response: Returns: Dict: The response of the mission restart request """ - self.drone.is_listening = False - - self.drone.sendCommand(mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT, param2=1) + if not self.drone.reserve_message_type("COMMAND_ACK", self.controller_id): + return { + "success": False, + "message": "Could not reserve COMMAND_ACK messages", + } try: - response = self.drone.master.recv_match(type="COMMAND_ACK", blocking=True) + self.drone.sendCommand( + mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT, param2=1 + ) - self.drone.is_listening = True + response = self.drone.wait_for_message( + "COMMAND_ACK", + self.controller_id, + timeout=3, + ) if commandAccepted( response, mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT @@ -423,11 +447,12 @@ def restartMission(self) -> Response: "message": "Failed to restart mission", } except serial.serialutil.SerialException: - self.drone.is_listening = True return { "success": False, "message": "Failed to restart mission, serial exception", } + finally: + self.drone.release_message_type("COMMAND_ACK", self.controller_id) @sendingCommandLock def clearMission(self, mission_type: int) -> Response: @@ -441,29 +466,33 @@ def clearMission(self, mission_type: int) -> Response: if not mission_type_check.get("success"): return mission_type_check - self.drone.is_listening = False + if not self.drone.reserve_message_type("MISSION_ACK", self.controller_id): + return { + "success": False, + "message": "Could not reserve MISSION_ACK messages", + } - self.drone.master.mav.mission_clear_all_send( - self.drone.target_system, - self.drone.target_component, - mission_type=mission_type, - ) try: + self.drone.master.mav.mission_clear_all_send( + self.drone.target_system, + self.drone.target_component, + mission_type=mission_type, + ) + while True: - response = self.drone.master.recv_match( - type=[ - "MISSION_ACK", - ], - blocking=True, + response = self.drone.wait_for_message( + "MISSION_ACK", + self.controller_id, timeout=2, + condition_func=lambda msg: msg.mission_type == mission_type, ) + if not response: - break - elif response.mission_type != mission_type: - continue + return { + "success": False, + "message": "Could not clear mission", + } elif response.type == 0: - self.drone.is_listening = True - return { "success": True, "message": "Mission cleared successfully", @@ -472,20 +501,18 @@ def clearMission(self, mission_type: int) -> Response: self.drone.logger.error( f"Error clearing mission, mission ack response: {response.type}" ) - break - - self.drone.is_listening = True - return { - "success": False, - "message": "Could not clear mission", - } + return { + "success": False, + "message": "Could not clear mission", + } except serial.serialutil.SerialException: - self.drone.is_listening = True return { "success": False, "message": "Could not clear mission, serial exception", } + finally: + self.drone.release_message_type("MISSION_ACK", self.controller_id) def _parseWaypointsListIntoLoader( self, waypoints: List[dict], mission_type: int @@ -579,44 +606,64 @@ def uploadMission( "message": f"Cleared mission type {mission_type}, no waypoints to upload", } - self.drone.is_listening = False - self.drone.sending_command_lock.acquire() - - self.drone.master.mav.mission_count_send( - self.drone.target_system, - self.drone.target_component, - new_loader.count(), - mission_type=mission_type, - ) + if not self.drone.reserve_message_type("MISSION_REQUEST", self.controller_id): + return { + "success": False, + "message": "Could not reserve MISSION_REQUEST messages", + } + if not self.drone.reserve_message_type("MISSION_ACK", self.controller_id): + self.drone.release_message_type("MISSION_REQUEST", self.controller_id) + return { + "success": False, + "message": "Could not reserve MISSION_ACK messages", + } try: + self.drone.sending_command_lock.acquire() + + self.drone.master.mav.mission_count_send( + self.drone.target_system, + self.drone.target_component, + new_loader.count(), + mission_type=mission_type, + ) + while True: - response = self.drone.master.recv_match( - type=["MISSION_REQUEST", "MISSION_ACK"], - blocking=True, + # Wait for either MISSION_REQUEST or MISSION_ACK + response = None + mission_request = self.drone.wait_for_message( + "MISSION_REQUEST", + self.controller_id, timeout=2, + condition_func=lambda msg: msg.mission_type == mission_type, ) - if not response: - self.drone.is_listening = True - self.drone.sending_command_lock.release() - - return { - "success": False, - "message": "Could not upload mission, mission request not received", - } - elif response.msgname == "MISSION_ACK" and response.type != 0: - self.drone.logger.error( - f"Error uploading mission, mission ack response: {response.type}" + if not mission_request: + mission_ack = self.drone.wait_for_message( + "MISSION_ACK", + self.controller_id, + timeout=2, + condition_func=lambda msg: msg.mission_type == mission_type, ) - self.drone.is_listening = True - self.drone.sending_command_lock.release() - return { - "success": False, - "message": "Could not upload mission, received mission acknowledgement error", - } - elif response.msgname == "MISSION_ACK" and response.type == 0: - continue # Continue to next iteration if we received a MISSION_ACK - elif response.mission_type == mission_type: + if mission_ack: + if mission_ack.type != 0: + self.drone.logger.error( + f"Error uploading mission, mission ack response: {mission_ack.type}" + ) + return { + "success": False, + "message": "Could not upload mission, received mission acknowledgement error", + } + else: + continue # Continue if we received a successful MISSION_ACK + else: + return { + "success": False, + "message": "Could not upload mission, mission request not received", + } + else: + response = mission_request + + if response: self.drone.logger.debug( f"Sending mission item {response.seq + 1} out of {new_loader.count()}" ) @@ -629,21 +676,14 @@ def uploadMission( ) if response.seq == new_loader.count() - 1: - mission_ack_response = self.drone.master.recv_match( - type=[ - "MISSION_ACK", - ], - blocking=True, + mission_ack_response = self.drone.wait_for_message( + "MISSION_ACK", + self.controller_id, timeout=2, + condition_func=lambda msg: msg.mission_type == mission_type, ) - self.drone.is_listening = True - self.drone.sending_command_lock.release() - - if ( - mission_ack_response - and mission_ack_response.type == 0 - and mission_ack_response.mission_type == mission_type - ): + + if mission_ack_response and mission_ack_response.type == 0: self.drone.logger.info("Uploaded mission successfully") if mission_type == TYPE_MISSION: self.missionLoader = new_loader @@ -658,19 +698,21 @@ def uploadMission( } else: self.drone.logger.error( - f"Error uploading mission, mission ack response: {mission_ack_response.type}" + f"Error uploading mission, mission ack response: {mission_ack_response.type if mission_ack_response else 'None'}" ) return { "success": False, "message": "Could not upload mission, not received mission acknowledgement", } except serial.serialutil.SerialException: - self.drone.is_listening = True - self.drone.sending_command_lock.release() return { "success": False, "message": "Could not upload mission, serial exception", } + finally: + self.drone.sending_command_lock.release() + self.drone.release_message_type("MISSION_REQUEST", self.controller_id) + self.drone.release_message_type("MISSION_ACK", self.controller_id) def importMissionFromFile(self, mission_type: int, file_path: str) -> Response: """ diff --git a/radio/app/controllers/motorTestController.py b/radio/app/controllers/motorTestController.py index 2652ae0d8..11e7adb35 100644 --- a/radio/app/controllers/motorTestController.py +++ b/radio/app/controllers/motorTestController.py @@ -1,5 +1,6 @@ from __future__ import annotations +from threading import current_thread from typing import TYPE_CHECKING, Optional import serial @@ -24,6 +25,7 @@ def __init__(self, drone: Drone) -> None: Args: drone (Drone): The main drone object """ + self.controller_id = f"motortest_{current_thread().ident}" self.drone = drone def checkMotorTestValues( @@ -66,8 +68,6 @@ def testOneMotor(self, data: MotorTestAllValues) -> Response: Returns: Response: The response from the motor test """ - self.drone.is_listening = False - throttle, duration, err = self.checkMotorTestValues(data) if err: return {"success": False, "message": err} @@ -80,22 +80,30 @@ def testOneMotor(self, data: MotorTestAllValues) -> Response: ) return {"success": False, "message": "Invalid value for motorInstance"} - self.drone.sendCommand( - mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, - param1=motor_instance, # ID of the motor to be tested - param2=0, # throttle type (PWM,% etc) - param3=throttle, # value of the throttle - 0 to 100% - param4=duration, # duration of the test in seconds - param5=0, # number of motors to test in a sequence - param6=0, # test order - ) - - motor_letter = chr(64 + motor_instance) + if not self.drone.reserve_message_type("COMMAND_ACK", self.controller_id): + return { + "success": False, + "message": "Could not reserve COMMAND_ACK messages", + } try: - response = self.drone.master.recv_match(type="COMMAND_ACK", blocking=True) + self.drone.sendCommand( + mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, + param1=motor_instance, # ID of the motor to be tested + param2=0, # throttle type (PWM,% etc) + param3=throttle, # value of the throttle - 0 to 100% + param4=duration, # duration of the test in seconds + param5=0, # number of motors to test in a sequence + param6=0, # test order + ) - self.drone.is_listening = True + motor_letter = chr(64 + motor_instance) + + response = self.drone.wait_for_message( + "COMMAND_ACK", + self.controller_id, + timeout=3, + ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST): self.drone.logger.info(f"Motor test started for motor {motor_instance}") @@ -111,9 +119,8 @@ def testOneMotor(self, data: MotorTestAllValues) -> Response: "success": False, "message": f"Motor test for motor {motor_letter} not started", } - except serial.serialutil.SerialException: - self.drone.is_listening = True + except serial.serialutil.SerialException: self.drone.logger.error( f"Motor test for motor {motor_instance} not started, serial exception" ) @@ -121,6 +128,8 @@ def testOneMotor(self, data: MotorTestAllValues) -> Response: "success": False, "message": f"Motor test for motor {motor_letter} not started, serial exception", } + finally: + self.drone.release_message_type("COMMAND_ACK", self.controller_id) @sendingCommandLock def testMotorSequence(self, data: MotorTestThrottleDurationAndNumber) -> Response: @@ -133,8 +142,6 @@ def testMotorSequence(self, data: MotorTestThrottleDurationAndNumber) -> Respons Returns: Response: The response from the motor test """ - self.drone.is_listening = False - throttle, duration, err = self.checkMotorTestValues(data) if err: return {"success": False, "message": err} @@ -146,20 +153,28 @@ def testMotorSequence(self, data: MotorTestThrottleDurationAndNumber) -> Respons ) return {"success": False, "message": "Invalid value for number_of_motors"} - self.drone.sendCommand( - mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, - param1=0, # ID of the motor to be tested - param2=0, # throttle type (PWM,% etc) - param3=throttle, # value of the throttle - 0 to 100% - param4=duration, # delay between tests in seconds - param5=num_motors, # number of motors to test in a sequence - param6=0, # test order - ) + if not self.drone.reserve_message_type("COMMAND_ACK", self.controller_id): + return { + "success": False, + "message": "Could not reserve COMMAND_ACK messages", + } try: - response = self.drone.master.recv_match(type="COMMAND_ACK", blocking=True) + self.drone.sendCommand( + mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, + param1=0, # ID of the motor to be tested + param2=0, # throttle type (PWM,% etc) + param3=throttle, # value of the throttle - 0 to 100% + param4=duration, # delay between tests in seconds + param5=num_motors, # number of motors to test in a sequence + param6=0, # test order + ) - self.drone.is_listening = True + response = self.drone.wait_for_message( + "COMMAND_ACK", + self.controller_id, + timeout=3, + ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST): self.drone.logger.info("Motor sequence test started") @@ -167,14 +182,15 @@ def testMotorSequence(self, data: MotorTestThrottleDurationAndNumber) -> Respons else: self.drone.logger.error("Motor sequence test not started") return {"success": False, "message": "Motor sequence test not started"} - except serial.serialutil.SerialException: - self.drone.is_listening = True + except serial.serialutil.SerialException: self.drone.logger.error("Motor sequence test not started, serial exception") return { "success": False, "message": "Motor sequence test not started, serial exception", } + finally: + self.drone.release_message_type("COMMAND_ACK", self.controller_id) @sendingCommandLock def testAllMotors(self, data: MotorTestThrottleDurationAndNumber) -> Response: @@ -190,8 +206,6 @@ def testAllMotors(self, data: MotorTestThrottleDurationAndNumber) -> Response: # Timeout after 3 seconds waiting for the motor test confirmation RESPONSE_TIMEOUT = 3 - self.drone.is_listening = False - throttle, duration, err = self.checkMotorTestValues(data) if err: return {"success": False, "message": err} @@ -204,56 +218,66 @@ def testAllMotors(self, data: MotorTestThrottleDurationAndNumber) -> Response: ) return {"success": False, "message": "Invalid value for number_of_motors"} - # Send all commands - for idx in range(1, num_motors + 1): - self.drone.sendCommand( - mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, - param1=idx, # ID of the motor to be tested - param2=0, # throttle type (PWM,% etc) - param3=throttle, # value of the throttle - 0 to 100% - param4=duration, # duration of the test in seconds - param5=0, # number of motors to test in a sequence - param6=0, # test order - ) + if not self.drone.reserve_message_type("COMMAND_ACK", self.controller_id): + return { + "success": False, + "message": "Could not reserve COMMAND_ACK messages", + } - successful_responses = 0 try: - # Attempt to gather all the command acknowledegements + # Send all commands + for idx in range(1, num_motors + 1): + self.drone.sendCommand( + mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, + param1=idx, # ID of the motor to be tested + param2=0, # throttle type (PWM,% etc) + param3=throttle, # value of the throttle - 0 to 100% + param4=duration, # duration of the test in seconds + param5=0, # number of motors to test in a sequence + param6=0, # test order + ) + + successful_responses = 0 + # Attempt to gather all the command acknowledgements using the new system for _ in range(num_motors): - response = self.drone.master.recv_match( - type="COMMAND_ACK", blocking=True, timeout=RESPONSE_TIMEOUT + response = self.drone.wait_for_message( + "COMMAND_ACK", + self.controller_id, + timeout=RESPONSE_TIMEOUT, ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST): successful_responses += 1 - except serial.serialutil.SerialException: - self.drone.is_listening = True + # Return data based on the number of successful command acknowledgements + if successful_responses == num_motors: + self.drone.logger.info("All motor test started successfully") + return { + "success": True, + "message": "All motor test started successfully", + } + elif successful_responses < num_motors: + self.drone.logger.warning( + f"Number of successful responses ({successful_responses}) was less than number of motors ({num_motors})" + ) + return { + "success": False, + "message": f"All motor test successfully started {successful_responses} / {num_motors} motors", + } + else: # pragma: no cover + # We should never reach this (since we should only ever have successful_responses <= num_motors) + self.drone.logger.info( + f"All motor test potentially started, but received {successful_responses} responses with {num_motors} motors" + ) + return { + "success": True, + "message": "All motor test potentially started", + } + + except serial.serialutil.SerialException: self.drone.logger.error("All motor test not started, serial exception") return { "success": False, "message": "All motor test not started, serial exception", } - - self.drone.is_listening = True - - # Return data based on the number of successful command acknowledgements - if successful_responses == num_motors: - self.drone.logger.info("All motor test started successfully") - return {"success": True, "message": "All motor test started successfully"} - elif successful_responses < num_motors: - self.drone.logger.warning( - f"Number of successful responses ({successful_responses}) was less than number of motors ({num_motors})" - ) - return { - "success": False, - "message": f"All motor test successfully started {successful_responses} / {num_motors} motors", - } - else: # pragma: no cover - # We should never reach this (since we should only ever have successful_responses <= num_motors) - self.drone.logger.info( - f"All motor test potentially started, but received {successful_responses} responses with {num_motors} motors" - ) - return { - "success": True, - "message": "All motor test potentially started", - } + finally: + self.drone.release_message_type("COMMAND_ACK", self.controller_id) diff --git a/radio/app/controllers/navController.py b/radio/app/controllers/navController.py index 20120d3ed..1d89bfc08 100644 --- a/radio/app/controllers/navController.py +++ b/radio/app/controllers/navController.py @@ -1,5 +1,6 @@ from __future__ import annotations +from threading import current_thread from typing import TYPE_CHECKING import serial @@ -19,6 +20,7 @@ def __init__(self, drone: Drone) -> None: Args: drone (Drone): The main drone object """ + self.controller_id = f"nav_{current_thread().ident}" self.drone = drone self.loiter_radius_param_type = mavutil.mavlink.MAV_PARAM_TYPE_INT16 @@ -33,17 +35,21 @@ def getHomePosition(self) -> Response: """ Request the current home position from the drone. """ - self.drone.is_listening = False - self.drone.sendCommand( - mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE, - param1=mavutil.mavlink.MAVLINK_MSG_ID_HOME_POSITION, - ) + if not self.drone.reserve_message_type("HOME_POSITION", self.controller_id): + return { + "success": False, + "message": "Could not reserve HOME_POSITION messages", + } try: - response = self.drone.master.recv_match( - type="HOME_POSITION", blocking=True, timeout=1.5 + self.drone.sendCommand( + mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE, + param1=mavutil.mavlink.MAVLINK_MSG_ID_HOME_POSITION, + ) + + response = self.drone.wait_for_message( + "HOME_POSITION", self.controller_id, timeout=1.5 ) - self.drone.is_listening = True if response: self.drone.logger.info("Home position received") @@ -65,13 +71,15 @@ def getHomePosition(self) -> Response: "success": False, "message": "Could not get home position", } + except serial.serialutil.SerialException: - self.drone.is_listening = True self.drone.logger.warning("Could not get home position, serial exception") return { "success": False, "message": "Could not get home position, serial exception", } + finally: + self.drone.release_message_type("HOME_POSITION", self.controller_id) @sendingCommandLock def setHomePosition(self, lat: float, lon: float, alt: float) -> Response: @@ -83,28 +91,28 @@ def setHomePosition(self, lat: float, lon: float, alt: float) -> Response: lon (float): The longitude of the home point alt (float): The altitude of the home point """ - - self.drone.is_listening = False - self.drone.sendCommandInt( - mavutil.mavlink.MAV_CMD_DO_SET_HOME, x=lat, y=lon, z=alt - ) + if not self.drone.reserve_message_type("COMMAND_ACK", self.controller_id): + return { + "success": False, + "message": "Could not reserve COMMAND_ACK messages", + } try: - response = self.drone.master.recv_match( - type=[ - "COMMAND_ACK", - ], - blocking=True, + self.drone.sendCommandInt( + mavutil.mavlink.MAV_CMD_DO_SET_HOME, x=lat, y=lon, z=alt + ) + + response = self.drone.wait_for_message( + "COMMAND_ACK", + self.controller_id, timeout=2, ) - self.drone.is_listening = True if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_SET_HOME): return { "success": True, "message": "Home point set successfully", } - else: return { "success": False, @@ -112,11 +120,12 @@ def setHomePosition(self, lat: float, lon: float, alt: float) -> Response: } except serial.serialutil.SerialException: - self.drone.is_listening = True return { "success": False, "message": "Could not set home point, serial exception", } + finally: + self.drone.release_message_type("COMMAND_ACK", self.controller_id) def takeoff(self, alt: float) -> Response: """ @@ -138,32 +147,42 @@ def takeoff(self, alt: float) -> Response: if not guidedModeSetResult["success"]: return guidedModeSetResult - self.drone.is_listening = False - self.drone.sending_command_lock.acquire() + if not self.drone.reserve_message_type("COMMAND_ACK", self.controller_id): + return { + "success": False, + "message": "Could not reserve COMMAND_ACK messages", + } - self.drone.sendCommand(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, param7=alt) + self.drone.sending_command_lock.acquire() try: - response = self.drone.master.recv_match(type="COMMAND_ACK", blocking=True) - self.drone.is_listening = True + self.drone.sendCommand(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, param7=alt) + + response = self.drone.wait_for_message( + "COMMAND_ACK", + self.controller_id, + timeout=3, + ) + self.drone.sending_command_lock.release() if commandAccepted(response, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF): - self.drone.is_listening = True self.drone.logger.info("Takeoff command send successfully") return {"success": True, "message": "Takeoff command sent successfully"} else: - self.drone.is_listening = True return { "success": False, "message": "Could not takeoff", } + except serial.serialutil.SerialException: - self.drone.is_listening = True + self.drone.sending_command_lock.release() return { "success": False, "message": "Could not takeoff, serial exception", } + finally: + self.drone.release_message_type("COMMAND_ACK", self.controller_id) @sendingCommandLock def land(self) -> Response: @@ -173,13 +192,20 @@ def land(self) -> Response: Returns: Response: The response from the land command """ - self.drone.is_listening = False - - self.drone.sendCommand(mavutil.mavlink.MAV_CMD_NAV_LAND) + if not self.drone.reserve_message_type("COMMAND_ACK", self.controller_id): + return { + "success": False, + "message": "Could not reserve COMMAND_ACK messages", + } try: - response = self.drone.master.recv_match(type="COMMAND_ACK", blocking=True) - self.drone.is_listening = True + self.drone.sendCommand(mavutil.mavlink.MAV_CMD_NAV_LAND) + + response = self.drone.wait_for_message( + "COMMAND_ACK", + self.controller_id, + timeout=3, + ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_NAV_LAND): self.drone.logger.info("Land command send successfully") @@ -189,12 +215,14 @@ def land(self) -> Response: "success": False, "message": "Could not land", } + except serial.serialutil.SerialException: - self.drone.is_listening = True return { "success": False, "message": "Could not land, serial exception", } + finally: + self.drone.release_message_type("COMMAND_ACK", self.controller_id) def reposition(self, lat: float, lon: float, alt: float) -> Response: """ @@ -212,33 +240,29 @@ def reposition(self, lat: float, lon: float, alt: float) -> Response: if not guidedModeSetResult["success"]: return guidedModeSetResult - self.drone.is_listening = False - - with self.drone.sending_command_lock: - self.drone.master.mav.set_position_target_global_int_send( - 0, - self.drone.target_system, - self.drone.target_component, - mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, - 65016, # Bitmask to ignore all values except for x, y and z - int(lat * 1e7), - int(lon * 1e7), - alt, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - ) - - self.drone.logger.info(f"Reposition command sent to {lat}, {lon}, {alt}m") + try: + with self.drone.sending_command_lock: + self.drone.master.mav.set_position_target_global_int_send( + 0, + self.drone.target_system, + self.drone.target_component, + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, + 65016, # Bitmask to ignore all values except for x, y and z + int(lat * 1e7), + int(lon * 1e7), + alt, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + ) - self.drone.is_listening = True + self.drone.logger.info(f"Reposition command sent to {lat}, {lon}, {alt}m") - try: return { "success": True, "message": "Reposition command sent successfully", @@ -249,7 +273,6 @@ def reposition(self, lat: float, lon: float, alt: float) -> Response: }, } except serial.serialutil.SerialException: - self.drone.is_listening = True self.drone.logger.error("Reposition command not accepted, serial exception") return { "success": False, diff --git a/radio/app/controllers/paramsController.py b/radio/app/controllers/paramsController.py index 5b4b2597e..3f9b5d785 100644 --- a/radio/app/controllers/paramsController.py +++ b/radio/app/controllers/paramsController.py @@ -2,7 +2,7 @@ import struct import time -from threading import Thread +from threading import Thread, current_thread from typing import TYPE_CHECKING, Any, List, Optional, Union import serial @@ -29,6 +29,7 @@ def __init__(self, drone: Drone) -> None: Args: drone (Drone): The main drone object """ + self.controller_id = f"params_{current_thread().ident}" self.drone = drone self.params: List[Any] = [] self.current_param_index = 0 @@ -49,34 +50,32 @@ def getSingleParam(self, param_name: str, timeout: float = 3) -> Response: Returns: Response: The response from the retrieval of the specific parameter """ - self.drone.is_listening = False - time.sleep(0.05) # Give some time to stop listening failure_message = f"Failed to get parameter {param_name}" - self.drone.master.mav.param_request_read_send( - self.drone.target_system, - self.drone.target_component, - param_name.encode(), - -1, - ) + if not self.drone.reserve_message_type("PARAM_VALUE", self.controller_id): + return { + "success": False, + "message": f"{failure_message}, another controller is using PARAM_VALUE messages", + } try: - start_time = time.time() - response = None - while time.time() - start_time < timeout: - # timeout of 0.2 to recv_match to allow checking the overall timeout and not block forever - msg = self.drone.master.recv_match( - type="PARAM_VALUE", blocking=True, timeout=0.2 - ) + time.sleep(0.05) # Brief pause for stability - if msg is None: - continue + self.drone.master.mav.param_request_read_send( + self.drone.target_system, + self.drone.target_component, + param_name.encode(), + -1, + ) - if msg.param_id == param_name: - response = msg - break + # Wait for the specific parameter response + response = self.drone.wait_for_message( + "PARAM_VALUE", + self.controller_id, + timeout, + condition_func=lambda msg: msg.param_id == param_name, + ) - self.drone.is_listening = True if response: self.saveParam( response.param_id, response.param_value, response.param_type @@ -91,19 +90,27 @@ def getSingleParam(self, param_name: str, timeout: float = 3) -> Response: "success": False, "message": f"{failure_message}, timed out", } + except serial.serialutil.SerialException: - self.drone.is_listening = True return { "success": False, "message": f"{failure_message}, serial exception", } + finally: + # Always release the message type + self.drone.release_message_type("PARAM_VALUE", self.controller_id) def getAllParams(self) -> None: """ Request all parameters from the drone. """ self.drone.stopAllDataStreams() - self.drone.is_listening = False + if not self.drone.reserve_message_type("PARAM_VALUE", self.controller_id): + self.drone.logger.error( + "Could not reserve PARAM_VALUE messages for getAllParams" + ) + return + self.is_requesting_params = True self.getAllParamsThread = Thread( @@ -119,46 +126,56 @@ def getAllParamsThreadFunc(self) -> None: """ timeout = time.time() + 120 # 120 seconds from now - while self.is_requesting_params: - try: - if time.time() > timeout: - self.drone.logger.error("Get all params thread timed out") + try: + while self.is_requesting_params: + try: + if time.time() > timeout: + self.drone.logger.error("Get all params thread timed out") + self.is_requesting_params = False + self.current_param_index = 0 + self.current_param_id = "" + self.total_number_of_params = 0 + self.params = [] + return + + # Use the new wait_for_message system instead of recv_msg + msg = self.drone.wait_for_message( + "PARAM_VALUE", + self.controller_id, + timeout=1.0, # Short timeout to check for overall timeout + ) + + if msg: + self.saveParam(msg.param_id, msg.param_value, msg.param_type) + + self.current_param_index = msg.param_index + self.current_param_id = msg.param_id + + if self.total_number_of_params != msg.param_count: + self.total_number_of_params = msg.param_count + + if msg.param_index == msg.param_count - 1: + self.is_requesting_params = False + self.current_param_index = 0 + self.current_param_id = "" + self.total_number_of_params = 0 + self.params = sorted( + self.params, key=lambda k: k["param_id"] + ) + self.drone.logger.info("Got all params") + return + + except serial.serialutil.SerialException: self.is_requesting_params = False self.current_param_index = 0 self.current_param_id = "" self.total_number_of_params = 0 self.params = [] - self.drone.is_listening = True + self.drone.logger.error("Serial exception while getting all params") return - - msg = self.drone.master.recv_msg() - if msg and msg.msgname == "PARAM_VALUE": - self.saveParam(msg.param_id, msg.param_value, msg.param_type) - - self.current_param_index = msg.param_index - self.current_param_id = msg.param_id - - if self.total_number_of_params != msg.param_count: - self.total_number_of_params = msg.param_count - - if msg.param_index == msg.param_count - 1: - self.is_requesting_params = False - self.current_param_index = 0 - self.current_param_id = "" - self.total_number_of_params = 0 - self.params = sorted(self.params, key=lambda k: k["param_id"]) - self.drone.is_listening = True - self.drone.logger.info("Got all params") - return - except serial.serialutil.SerialException: - self.is_requesting_params = False - self.current_param_index = 0 - self.current_param_id = "" - self.total_number_of_params = 0 - self.params = [] - self.drone.is_listening = True - self.drone.logger.error("Serial exception while getting all params") - return + finally: + # Always release the message type when done + self.drone.release_message_type("PARAM_VALUE", self.controller_id) def setMultipleParams(self, params_list: list[IncomingParam]) -> Response: """ @@ -220,10 +237,13 @@ def setParam( Returns: bool: True if the parameter was set, False if it failed """ - self.drone.is_listening = False got_ack = False save_timeout = 5 + if not self.drone.reserve_message_type("PARAM_VALUE", self.controller_id): + self.drone.logger.error("Could not reserve PARAM_VALUE messages") + return False + try: # Check if value fits inside the param type # https://github.com/ArduPilot/pymavlink/blob/4d8c4ff274d41b9bc8da1a411cb172d39786e46b/mavparm.py#L30C10-L30C10 @@ -248,7 +268,6 @@ def setParam( self.drone.logger.error( "can't send %s of type %u" % (param_name, param_type) ) - self.drone.is_listening = True return False # vfloat, = struct.unpack(">f", vstr) vfloat = float(param_value) @@ -256,22 +275,25 @@ def setParam( self.drone.logger.error( f"Could not set parameter {param_name} with value {param_value}: {e}" ) - self.drone.is_listening = True return False - # Keep trying to set the parameter until we get an ack or run out of retries or timeout - while retries > 0 and not got_ack: - retries -= 1 - self.drone.master.param_set_send( - param_name.upper(), vfloat, parm_type=param_type - ) - tstart = time.time() - while time.time() - tstart < save_timeout: - ack = self.drone.master.recv_match(type="PARAM_VALUE") - if ack is None: - time.sleep(0.1) - continue - if str(param_name).upper() == str(ack.param_id).upper(): + # Keep trying to set the parameter until we get an ack or run out of retries or timeout + while retries > 0 and not got_ack: + retries -= 1 + self.drone.master.param_set_send( + param_name.upper(), vfloat, parm_type=param_type + ) + + # Wait for parameter acknowledgment using the new system + ack = self.drone.wait_for_message( + "PARAM_VALUE", + self.controller_id, + save_timeout, + condition_func=lambda msg: str(param_name).upper() + == str(msg.param_id).upper(), + ) + + if ack: got_ack = True self.drone.logger.debug( f"Got parameter saving ack for {param_name} for value {param_value}" @@ -279,13 +301,13 @@ def setParam( self.saveParam(ack.param_id, ack.param_value, ack.param_type) break - if not got_ack: - self.drone.logger.error(f"timeout setting {param_name} to {vfloat}") - self.drone.is_listening = True - return False + if not got_ack: + self.drone.logger.error(f"timeout setting {param_name} to {vfloat}") + + return got_ack - self.drone.is_listening = True - return True + finally: + self.drone.release_message_type("PARAM_VALUE", self.controller_id) def saveParam(self, param_name: str, param_value: Number, param_type: int) -> None: """ diff --git a/radio/app/drone.py b/radio/app/drone.py index f64936f92..a9729db33 100644 --- a/radio/app/drone.py +++ b/radio/app/drone.py @@ -8,7 +8,7 @@ from queue import Empty, Queue from secrets import token_hex from threading import Event, Lock, Thread, current_thread -from typing import Callable, Dict, List, Optional +from typing import Callable, Dict, List, Optional, Set import serial from pymavlink import mavutil @@ -30,6 +30,7 @@ getFlightSwVersionString, getVehicleType, sendingCommandLock, + sendMessage, ) # Constants @@ -205,12 +206,20 @@ def __init__( self.is_active = Event() self.is_active.set() - self.is_listening: bool = False + + self.reserved_messages: Set[str] = set() + self.controller_queues: Dict[str, Queue] = {} + self.reservation_lock = Lock() + self.controller_id = f"Drone_{current_thread().ident}" self.armed = False self.capabilities: Optional[list[str]] = None self.flight_sw_version: Optional[tuple[int, int, int, int]] = None + self.startThread() + + self.addMessageListener("STATUSTEXT", sendMessage) + self.getAutopilotVersion() if self.flight_sw_version is None: @@ -247,10 +256,6 @@ def __init__( self.setupControllers() - self.is_listening = True - - self.startThread() - self.sendConnectionStatusUpdate(12) self.sendStatusTextMessage( @@ -517,13 +522,84 @@ def removeMessageListener(self, message_id: str) -> bool: return True return False + def reserve_message_type(self, message_type: str, controller_id: str) -> bool: + """Reserve a message type for exclusive controller use. + + Args: + message_type: The MAVLink message type to reserve (e.g., "COMMAND_ACK") + controller_id: Unique identifier for the controller + + Returns: + bool: True if reservation successful, False if already reserved + """ + with self.reservation_lock: + if message_type in self.reserved_messages: + return False + + self.reserved_messages.add(message_type) + if controller_id not in self.controller_queues: + self.controller_queues[controller_id] = Queue() + + return True + + def release_message_type(self, message_type: str, controller_id: str) -> None: + """Release a reserved message type. + + Args: + message_type: The message type to release + controller_id: The controller releasing the message + """ + with self.reservation_lock: + self.reserved_messages.discard(message_type) + # Clear any remaining messages in the controller's queue for this type + if controller_id in self.controller_queues: + # We'll implement a cleanup mechanism if needed + pass + + def wait_for_message( + self, + message_type: str, + controller_id: str, + timeout: float = 3.0, + condition_func=None, + ) -> Optional[object]: + """Wait for a specific message type for a controller. + + Args: + message_type: The message type to wait for + controller_id: The controller waiting for the message + timeout: How long to wait before timing out + condition_func: Optional function to filter messages (e.g., lambda msg: msg.command == expected_cmd) + + Returns: + The message object if received, None if timeout + """ + if controller_id not in self.controller_queues: + self.controller_queues[controller_id] = Queue() + + start_time = time.time() + while time.time() - start_time < timeout: + try: + # Check controller's queue for the message + queue_item = self.controller_queues[controller_id].get(timeout=0.1) + msg_type, msg = queue_item + + if msg_type == message_type: + # Apply condition filter if provided + if condition_func is None or condition_func(msg): + return msg + else: + # Not the message we're looking for, continue waiting + continue + + except Empty: + continue + + return None + def checkForMessages(self) -> None: """Check for messages from the drone and add them to the message queue.""" 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 - try: msg = self.master.recv_msg() except mavutil.mavlink.MAVError as e: @@ -559,7 +635,9 @@ def checkForMessages(self) -> None: ) self.stopForwarding() - if msg.msgname == "HEARTBEAT": + msg_name = msg.get_type() + + if msg_name == "HEARTBEAT": if ( msg.autopilot == mavutil.mavlink.MAV_AUTOPILOT_INVALID ): # No valid autopilot, e.g. a GCS or other MAVLink component @@ -572,22 +650,35 @@ def checkForMessages(self) -> None: if self.armed: try: self.log_message_queue.put( - f"{msg._timestamp},{msg.msgname},{','.join([f'{message}:{msg.to_dict()[message]}' for message in msg.to_dict() if message != 'mavpackettype'])}" + f"{msg._timestamp},{msg_name},{','.join([f'{message}:{msg.to_dict()[message]}' for message in msg.to_dict() if message != 'mavpackettype'])}" ) except Exception as e: self.log_message_queue.put(f"Writing message failed! {e}") continue - if msg.msgname == "TIMESYNC": + if msg_name == "TIMESYNC": component_timestamp = msg.ts1 local_timestamp = time.time_ns() self.master.mav.timesync_send(local_timestamp, component_timestamp) continue - elif msg.msgname == "STATUSTEXT": + elif msg_name == "STATUSTEXT": self.logger.info(msg.text) - if msg.msgname in self.message_listeners: - self.message_queue.put([msg.msgname, msg]) + with self.reservation_lock: + if msg_name in self.reserved_messages: + # Route to controller queues + for controller_id, queue in self.controller_queues.items(): + try: + queue.put((msg_name, msg), block=False) + except Exception: + # Queue full + pass + else: + # Route to normal message listeners + if msg_name in self.message_listeners: + self.message_queue.put([msg_name, msg]) + else: + time.sleep(0.05) def executeMessages(self) -> None: """Executes message listeners based on messages from the message queue.""" @@ -763,7 +854,6 @@ def startThread(self) -> None: def stopAllThreads(self) -> None: """Stops all threads.""" self.is_active.clear() - self.is_listening = False this_thread = current_thread() @@ -787,20 +877,19 @@ def stopAllThreads(self) -> None: @sendingCommandLock def getAutopilotVersion(self) -> None: """Get the autopilot version.""" - was_listening = self.is_listening - self.is_listening = False - - self.sendCommand( - mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE, - param1=mavutil.mavlink.MAVLINK_MSG_ID_AUTOPILOT_VERSION, - ) + if not self.reserve_message_type("AUTOPILOT_VERSION", self.controller_id): + self.logger.error("Could not reserve AUTOPILOT_VERSION messages") + return try: - response = self.master.recv_match( - type="AUTOPILOT_VERSION", blocking=True, timeout=5 + self.sendCommand( + mavutil.mavlink.MAV_CMD_REQUEST_MESSAGE, + param1=mavutil.mavlink.MAVLINK_MSG_ID_AUTOPILOT_VERSION, + ) + + response = self.wait_for_message( + "AUTOPILOT_VERSION", self.controller_id, timeout=5 ) - if was_listening: - self.is_listening = True if response is None: self.logger.error("Failed to get autopilot version: Timeout") @@ -820,27 +909,31 @@ def getAutopilotVersion(self) -> None: self.flight_sw_version = decodeFlightSwVersion(flight_sw_version) except serial.serialutil.SerialException: - if was_listening: - self.is_listening = True - self.logger.error("Failed to get autopilot version due to serial exception") + finally: + self.release_message_type("AUTOPILOT_VERSION", self.controller_id) def rebootAutopilot(self) -> None: """Reboot the autopilot.""" - self.is_listening = False - self.sending_command_lock.acquire() - - self.sendCommand( - mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, - param1=1, # Autopilot - param2=0, # Companion - param3=0, # Component action - param4=0, # Component ID - ) + if not self.reserve_message_type("COMMAND_ACK", self.controller_id): + self.logger.error("Could not reserve COMMAND_ACK messages for reboot") + return try: - response = self.master.recv_match(type="COMMAND_ACK", blocking=True) - self.sending_command_lock.release() + self.sending_command_lock.acquire() + + self.sendCommand( + mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, + param1=1, # Autopilot + param2=0, # Companion + param3=0, # Component action + param4=0, # Component ID + ) + + response = self.wait_for_message( + "COMMAND_ACK", + self.controller_id, + ) if commandAccepted( response, mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN @@ -849,10 +942,12 @@ def rebootAutopilot(self) -> None: self.close() else: self.logger.error("Reboot failed") - self.is_listening = True except serial.serialutil.SerialException: self.logger.debug("Rebooting") self.close() + finally: + self.sending_command_lock.release() + self.release_message_type("COMMAND_ACK", self.controller_id) # TODO: Move this out into a controller @sendingCommandLock @@ -866,17 +961,24 @@ def setServo(self, servo_instance: int, pwm_value: int) -> Response: Returns: Response: The response from the servo set command """ - self.is_listening = False - - self.sendCommand( - mavutil.mavlink.MAV_CMD_DO_SET_SERVO, - param1=servo_instance, # Servo instance number - param2=pwm_value, # PWM value - ) + if not self.reserve_message_type("COMMAND_ACK", self.controller_id): + return { + "success": False, + "message": "Could not reserve COMMAND_ACK messages", + } try: - response = self.master.recv_match(type="COMMAND_ACK", blocking=True) - self.is_listening = True + self.sendCommand( + mavutil.mavlink.MAV_CMD_DO_SET_SERVO, + param1=servo_instance, # Servo instance number + param2=pwm_value, # PWM value + ) + + response = self.wait_for_message( + "COMMAND_ACK", + self.controller_id, + timeout=3, + ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_SET_SERVO): return {"success": True, "message": f"Setting servo to {pwm_value}"} @@ -890,11 +992,12 @@ def setServo(self, servo_instance: int, pwm_value: int) -> Response: } except serial.serialutil.SerialException: - self.is_listening = True return { "success": False, "message": "Setting servo failed, serial exception", } + finally: + self.release_message_type("COMMAND_ACK", self.controller_id) def sendCommand( self, From 1a414af8ee35c7af3c3a724e6aae1bf58ffde375 Mon Sep 17 00:00:00 2001 From: Kush Makkapati Date: Thu, 30 Oct 2025 22:39:25 +0000 Subject: [PATCH 02/12] Try to fix tests --- .../app/controllers/flightModesController.py | 23 +++++----- radio/app/controllers/paramsController.py | 5 +-- radio/tests/helpers.py | 42 +++++++++---------- radio/tests/test_FlightModesController.py | 8 +++- 4 files changed, 40 insertions(+), 38 deletions(-) diff --git a/radio/app/controllers/flightModesController.py b/radio/app/controllers/flightModesController.py index 874c17f06..ed17ac9f9 100644 --- a/radio/app/controllers/flightModesController.py +++ b/radio/app/controllers/flightModesController.py @@ -146,18 +146,19 @@ def setCurrentFlightMode(self, flightMode: int) -> Response: "message": "Could not reserve COMMAND_ACK messages", } time.sleep(0.3) - self.drone.sendCommand( - message=mavutil.mavlink.MAV_CMD_DO_SET_MODE, - param1=1, - param2=flightMode, - param3=0, - param4=0, - param5=0, - param6=0, - param7=0, - ) try: + self.drone.sendCommand( + message=mavutil.mavlink.MAV_CMD_DO_SET_MODE, + param1=1, + param2=flightMode, + param3=0, + param4=0, + param5=0, + param6=0, + param7=0, + ) + response = self.drone.wait_for_message( "COMMAND_ACK", self.controller_id, @@ -170,7 +171,7 @@ def setCurrentFlightMode(self, flightMode: int) -> Response: else: return { "success": False, - "message": "Could not set flight mode", + "message": "Could not set flight mode, command not accepted", } except serial.serialutil.SerialException: return { diff --git a/radio/app/controllers/paramsController.py b/radio/app/controllers/paramsController.py index 3f9b5d785..25d283b3f 100644 --- a/radio/app/controllers/paramsController.py +++ b/radio/app/controllers/paramsController.py @@ -138,7 +138,6 @@ def getAllParamsThreadFunc(self) -> None: self.params = [] return - # Use the new wait_for_message system instead of recv_msg msg = self.drone.wait_for_message( "PARAM_VALUE", self.controller_id, @@ -165,13 +164,13 @@ def getAllParamsThreadFunc(self) -> None: self.drone.logger.info("Got all params") return - except serial.serialutil.SerialException: + except Exception as e: + self.drone.logger.error(e, exc_info=True) self.is_requesting_params = False self.current_param_index = 0 self.current_param_id = "" self.total_number_of_params = 0 self.params = [] - self.drone.logger.error("Serial exception while getting all params") return finally: # Always release the message type when done diff --git a/radio/tests/helpers.py b/radio/tests/helpers.py index db0f571dd..d441b2498 100644 --- a/radio/tests/helpers.py +++ b/radio/tests/helpers.py @@ -9,12 +9,12 @@ class FakeTCP: """ - Context manager that replaces the mavlink mavtcp recv_match method in `droneStatus.drone.master` with one that raises a + Context manager that replaces the mavlink mavtcp recv_msg method in `droneStatus.drone.master` with one that raises a `serial.serialutils.SerialException`. Use if you want to simulate a serial issue for a unit test. """ @staticmethod - def recv_match_override( + def recv_msg_override( condition=None, type=None, blocking=False, timeout=None ) -> None: raise SerialException( @@ -22,15 +22,15 @@ def recv_match_override( ) def __enter__(self) -> None: - # Replace drone mavtcp recv_match function with one that raises SerialException + # Replace drone mavtcp recv_msg function with one that raises SerialException if droneStatus.drone is not None: - self.old_recv = droneStatus.drone.master.recv_match - droneStatus.drone.master.recv_match = FakeTCP.recv_match_override + self.old_recv = droneStatus.drone.master.recv_msg + droneStatus.drone.master.recv_msg = FakeTCP.recv_msg_override def __exit__(self, type, value, traceback) -> None: - # Reset recv_match method + # Reset recv_msg method if droneStatus.drone is not None: - droneStatus.drone.master.recv_match = self.old_recv + droneStatus.drone.master.recv_msg = self.old_recv class NoDrone: @@ -51,24 +51,24 @@ def __exit__(self, type, value, traceback) -> None: class ParamSetTimeout: - """Context manager that replaces the mavlink recv_match function in drone.master with a function that return a None value + """Context manager that replaces the mavlink recv_msg function in drone.master with a function that return a None value to cause the set multipleparams function to timeout """ @staticmethod - def recv_match_null_value( + def recv_msg_null_value( condition=None, type=None, blocking=False, timeout=None ) -> None: return None def __enter__(self) -> None: if droneStatus.drone is not None: - self.old_recv = droneStatus.drone.master.recv_match - droneStatus.drone.master.recv_match = ParamSetTimeout.recv_match_null_value + self.old_recv = droneStatus.drone.master.recv_msg + droneStatus.drone.master.recv_msg = ParamSetTimeout.recv_msg_null_value def __exit__(self, type, value, traceback) -> None: if droneStatus.drone is not None: - droneStatus.drone.master.recv_match = self.old_recv + droneStatus.drone.master.recv_msg = self.old_recv class ParamRefreshTimeout: @@ -100,7 +100,7 @@ def __exit__(self, type, value, traceback) -> None: class NoAcknowledgementMessage: - """Context manager that replaces the mavlink recv_match function in drone.master with a function that returns False + """Context manager that replaces the mavlink recv_msg function in drone.master with a function that returns False causing no acknowledgement messages to be received when used in the MotorTestController tests """ @@ -112,31 +112,29 @@ def recv_msg_false_value( def __enter__(self) -> None: if droneStatus.drone is not None: - self.old_recv = droneStatus.drone.master.recv_match - droneStatus.drone.master.recv_match = ( + self.old_recv = droneStatus.drone.master.recv_msg + droneStatus.drone.master.recv_msg = ( NoAcknowledgementMessage.recv_msg_false_value ) def __exit__(self, type, value, traceback) -> None: if droneStatus.drone is not None: - droneStatus.drone.master.recv_match = self.old_recv + droneStatus.drone.master.recv_msg = self.old_recv class RecvMsgReturnsNone: @staticmethod - def recv_match_false( - condition=None, type=None, blocking=False, timeout=None - ) -> None: + def recv_msg_false(condition=None, type=None, blocking=False, timeout=None) -> None: return None def __enter__(self) -> None: if droneStatus.drone is not None: - self.old_recv = droneStatus.drone.master.recv_match - droneStatus.drone.master.recv_match = RecvMsgReturnsNone.recv_match_false + self.old_recv = droneStatus.drone.master.recv_msg + droneStatus.drone.master.recv_msg = RecvMsgReturnsNone.recv_msg_false def __exit__(self, type, value, traceback) -> None: if droneStatus.drone is not None: - droneStatus.drone.master.recv_match = self.old_recv + droneStatus.drone.master.recv_msg = self.old_recv class SetAircraftType: diff --git a/radio/tests/test_FlightModesController.py b/radio/tests/test_FlightModesController.py index f24cdf760..0d29836f2 100644 --- a/radio/tests/test_FlightModesController.py +++ b/radio/tests/test_FlightModesController.py @@ -73,12 +73,16 @@ def test_setCurrentFlightMode(client: SocketIOTestClient, droneStatus): with FakeTCP(): response = droneStatus.drone.flightModesController.setCurrentFlightMode(1) assert response.get("success") is False - assert response.get("message") == "Could not set flight mode, serial exception" + assert ( + response.get("message") == "Could not set flight mode, command not accepted" + ) with RecvMsgReturnsNone(): response = droneStatus.drone.flightModesController.setCurrentFlightMode(1) assert response.get("success") is False - assert response.get("message") == "Could not set flight mode" + assert ( + response.get("message") == "Could not set flight mode, command not accepted" + ) response = droneStatus.drone.flightModesController.setCurrentFlightMode(1) assert response.get("success") is True From a1cb214a0598ec565ea2128f8e6454a08dabd2ae Mon Sep 17 00:00:00 2001 From: Kush Makkapati Date: Fri, 31 Oct 2025 11:37:49 +0000 Subject: [PATCH 03/12] Fix fakeTCP helper, fix bug in setParam function, fix flight modes controller tests --- .../app/controllers/flightModesController.py | 13 +- radio/app/controllers/paramsController.py | 5 +- radio/tests/helpers.py | 67 ++------- radio/tests/test_FlightModesController.py | 141 +++++++++--------- radio/tests/test_motorController.py | 8 +- 5 files changed, 95 insertions(+), 139 deletions(-) diff --git a/radio/app/controllers/flightModesController.py b/radio/app/controllers/flightModesController.py index ed17ac9f9..56b1c62a8 100644 --- a/radio/app/controllers/flightModesController.py +++ b/radio/app/controllers/flightModesController.py @@ -88,19 +88,13 @@ def setFlightMode(self, mode_number: int, flight_mode: int) -> Response: if mode_number < 1 or mode_number > 6: self.drone.logger.error( - "Invalid flight mode number, must be between 1 and 6 inclusive." + f"Invalid flight mode number, must be between 1 and 6 inclusive, got {mode_number}." ) return { "success": False, "message": f"Invalid flight mode number, must be between 1 and 6 inclusive, got {mode_number}.", } - param_type = 2 - - param_set_success = self.drone.paramsController.setParam( - f"FLTMODE{mode_number}", flight_mode, param_type - ) - if self.drone.aircraft_type == 1: if (flight_mode < 0) or (flight_mode > 24): return { @@ -116,6 +110,11 @@ def setFlightMode(self, mode_number: int, flight_mode: int) -> Response: } mode_name = mavutil.mavlink.enums["COPTER_MODE"][flight_mode].name + param_type = 2 + param_set_success = self.drone.paramsController.setParam( + f"FLTMODE{mode_number}", flight_mode, param_type + ) + if param_set_success: self.drone.logger.info(f"Flight mode {mode_number} set to {mode_name}") self.flight_modes[mode_number - 1] = flight_mode diff --git a/radio/app/controllers/paramsController.py b/radio/app/controllers/paramsController.py index 25d283b3f..292b2a324 100644 --- a/radio/app/controllers/paramsController.py +++ b/radio/app/controllers/paramsController.py @@ -276,6 +276,7 @@ def setParam( ) return False + try: # Keep trying to set the parameter until we get an ack or run out of retries or timeout while retries > 0 and not got_ack: retries -= 1 @@ -304,7 +305,9 @@ def setParam( self.drone.logger.error(f"timeout setting {param_name} to {vfloat}") return got_ack - + except serial.serialutil.SerialException: + self.drone.logger.error(f"Serial exception setting parameter {param_name}") + return False finally: self.drone.release_message_type("PARAM_VALUE", self.controller_id) diff --git a/radio/tests/helpers.py b/radio/tests/helpers.py index d441b2498..8c64c043c 100644 --- a/radio/tests/helpers.py +++ b/radio/tests/helpers.py @@ -9,12 +9,11 @@ class FakeTCP: """ - Context manager that replaces the mavlink mavtcp recv_msg method in `droneStatus.drone.master` with one that raises a - `serial.serialutils.SerialException`. Use if you want to simulate a serial issue for a unit test. + Context manager that replaces master mavlink functions with ones that raise SerialException """ @staticmethod - def recv_msg_override( + def return_serial_exception( condition=None, type=None, blocking=False, timeout=None ) -> None: raise SerialException( @@ -22,15 +21,13 @@ def recv_msg_override( ) def __enter__(self) -> None: - # Replace drone mavtcp recv_msg function with one that raises SerialException if droneStatus.drone is not None: - self.old_recv = droneStatus.drone.master.recv_msg - droneStatus.drone.master.recv_msg = FakeTCP.recv_msg_override + self.old_send = droneStatus.drone.master.mav.send + droneStatus.drone.master.mav.send = FakeTCP.return_serial_exception def __exit__(self, type, value, traceback) -> None: - # Reset recv_msg method if droneStatus.drone is not None: - droneStatus.drone.master.recv_msg = self.old_recv + droneStatus.drone.master.mav.send = self.old_send class NoDrone: @@ -50,27 +47,6 @@ def __exit__(self, type, value, traceback) -> None: droneStatus.drone = self.oldDrone -class ParamSetTimeout: - """Context manager that replaces the mavlink recv_msg function in drone.master with a function that return a None value - to cause the set multipleparams function to timeout - """ - - @staticmethod - def recv_msg_null_value( - condition=None, type=None, blocking=False, timeout=None - ) -> None: - return None - - def __enter__(self) -> None: - if droneStatus.drone is not None: - self.old_recv = droneStatus.drone.master.recv_msg - droneStatus.drone.master.recv_msg = ParamSetTimeout.recv_msg_null_value - - def __exit__(self, type, value, traceback) -> None: - if droneStatus.drone is not None: - droneStatus.drone.master.recv_msg = self.old_recv - - class ParamRefreshTimeout: """Context manager that replaces the mavlink recv_msg function in drone.master with a function that returns a False value and sets current_param_index to -1 to cause the refresh_params function to timeout with no params received from the drone @@ -99,42 +75,19 @@ def __exit__(self, type, value, traceback) -> None: ) -class NoAcknowledgementMessage: - """Context manager that replaces the mavlink recv_msg function in drone.master with a function that returns False - causing no acknowledgement messages to be received when used in the MotorTestController tests - """ - - @staticmethod - def recv_msg_false_value( - condition=None, type=None, blocking=False, timeout=None - ) -> bool: - return False - - def __enter__(self) -> None: - if droneStatus.drone is not None: - self.old_recv = droneStatus.drone.master.recv_msg - droneStatus.drone.master.recv_msg = ( - NoAcknowledgementMessage.recv_msg_false_value - ) - - def __exit__(self, type, value, traceback) -> None: - if droneStatus.drone is not None: - droneStatus.drone.master.recv_msg = self.old_recv - - -class RecvMsgReturnsNone: +class WaitForMessageReturnsNone: @staticmethod - def recv_msg_false(condition=None, type=None, blocking=False, timeout=None) -> None: + def returns_none(*args, **kwargs) -> None: return None def __enter__(self) -> None: if droneStatus.drone is not None: - self.old_recv = droneStatus.drone.master.recv_msg - droneStatus.drone.master.recv_msg = RecvMsgReturnsNone.recv_msg_false + self.wait_for_message = droneStatus.drone.wait_for_message + droneStatus.drone.wait_for_message = WaitForMessageReturnsNone.returns_none def __exit__(self, type, value, traceback) -> None: if droneStatus.drone is not None: - droneStatus.drone.master.recv_msg = self.old_recv + droneStatus.drone.wait_for_message = self.wait_for_message class SetAircraftType: diff --git a/radio/tests/test_FlightModesController.py b/radio/tests/test_FlightModesController.py index 0d29836f2..2f1899f34 100644 --- a/radio/tests/test_FlightModesController.py +++ b/radio/tests/test_FlightModesController.py @@ -4,7 +4,11 @@ from flask_socketio.test_client import SocketIOTestClient from . import falcon_test -from .helpers import FakeTCP, ParamSetTimeout, RecvMsgReturnsNone, SetAircraftType +from .helpers import ( + FakeTCP, + SetAircraftType, + WaitForMessageReturnsNone, +) @pytest.fixture(scope="module", autouse=True) @@ -41,7 +45,7 @@ def test_getFlightModes_success(client: SocketIOTestClient, droneStatus): @falcon_test(pass_drone_status=True) def test_getFlightModes_failure(client: SocketIOTestClient, droneStatus): - with RecvMsgReturnsNone(): + with WaitForMessageReturnsNone(): droneStatus.drone.flightModesController.getFlightModes() assert len(droneStatus.drone.flightModesController.flight_modes) == 6 for items in droneStatus.drone.flightModesController.flight_modes: @@ -50,7 +54,7 @@ def test_getFlightModes_failure(client: SocketIOTestClient, droneStatus): @falcon_test(pass_drone_status=True) def test_getFlightModeChannel_failure(client: SocketIOTestClient, droneStatus): - with RecvMsgReturnsNone(): + with WaitForMessageReturnsNone(): original_flight_mode_channel = ( droneStatus.drone.flightModesController.flight_mode_channel ) @@ -72,105 +76,102 @@ def test_refreshdata(client: SocketIOTestClient, droneStatus): def test_setCurrentFlightMode(client: SocketIOTestClient, droneStatus): with FakeTCP(): response = droneStatus.drone.flightModesController.setCurrentFlightMode(1) - assert response.get("success") is False - assert ( - response.get("message") == "Could not set flight mode, command not accepted" - ) + assert response == { + "success": False, + "message": "Could not set flight mode, serial exception", + } - with RecvMsgReturnsNone(): + with WaitForMessageReturnsNone(): response = droneStatus.drone.flightModesController.setCurrentFlightMode(1) - assert response.get("success") is False - assert ( - response.get("message") == "Could not set flight mode, command not accepted" - ) + assert response == { + "success": False, + "message": "Could not set flight mode, command not accepted", + } response = droneStatus.drone.flightModesController.setCurrentFlightMode(1) - assert response.get("success") is True - assert response.get("message") == "Flight mode set successfully" + assert response == { + "success": True, + "message": "Flight mode set successfully", + } @falcon_test(pass_drone_status=True) def test_setFlightMode(client: SocketIOTestClient, droneStatus): response = droneStatus.drone.flightModesController.setFlightMode(0, 1) - assert response.get("success") is False - assert ( - response.get("message") - == "Invalid flight mode number, must be between 1 and 6 inclusive, got 0." - ) + assert response == { + "success": False, + "message": "Invalid flight mode number, must be between 1 and 6 inclusive, got 0.", + } response = droneStatus.drone.flightModesController.setFlightMode(-100, 1) - assert response.get("success") is False - assert ( - response.get("message") - == "Invalid flight mode number, must be between 1 and 6 inclusive, got -100." - ) + assert response == { + "success": False, + "message": "Invalid flight mode number, must be between 1 and 6 inclusive, got -100.", + } response = droneStatus.drone.flightModesController.setFlightMode(7, 1) - assert response.get("success") is False - assert ( - response.get("message") - == "Invalid flight mode number, must be between 1 and 6 inclusive, got 7." - ) + assert response == { + "success": False, + "message": "Invalid flight mode number, must be between 1 and 6 inclusive, got 7.", + } response = droneStatus.drone.flightModesController.setFlightMode(100, 1) - assert response.get("success") is False - assert ( - response.get("message") - == "Invalid flight mode number, must be between 1 and 6 inclusive, got 100." - ) + assert response == { + "success": False, + "message": "Invalid flight mode number, must be between 1 and 6 inclusive, got 100.", + } + # TODO: Fix imitation of PLANE type while being on COPTER sim, should use a different simulator ideally. with SetAircraftType(1): response = droneStatus.drone.flightModesController.setFlightMode(1, -2) - assert response.get("success") is False - assert ( - response.get("message") - == "Invalid plane flight mode, must be between 0 and 24 inclusive, got -2" - ) + assert response == { + "success": False, + "message": "Invalid plane flight mode, must be between 0 and 24 inclusive, got -2", + } response = droneStatus.drone.flightModesController.setFlightMode(1, 25) - assert response.get("success") is False - assert ( - response.get("message") - == "Invalid plane flight mode, must be between 0 and 24 inclusive, got 25" - ) + assert response == { + "success": False, + "message": "Invalid plane flight mode, must be between 0 and 24 inclusive, got 25", + } - with ParamSetTimeout(): + with WaitForMessageReturnsNone(): response = droneStatus.drone.flightModesController.setFlightMode(1, 1) - assert response.get("success") is False - assert ( - response.get("message") - == "Failed to set flight mode 1 to PLANE_MODE_CIRCLE" - ) + assert response == { + "success": False, + "message": "Failed to set flight mode 1 to PLANE_MODE_CIRCLE", + } response = droneStatus.drone.flightModesController.setFlightMode(1, 24) - assert response.get("success") is True - assert response.get("message") == "Flight mode 1 set to PLANE_MODE_THERMAL" + assert response == { + "success": True, + "message": "Flight mode 1 set to PLANE_MODE_THERMAL", + } assert droneStatus.drone.flightModesController.flight_modes[0] == 24 with SetAircraftType(2): response = droneStatus.drone.flightModesController.setFlightMode(1, -2) - assert response.get("success") is False - assert ( - response.get("message") - == "Invalid copter flight mode, must be between 0 and 27 inclusive, got -2" - ) + assert response == { + "success": False, + "message": "Invalid copter flight mode, must be between 0 and 27 inclusive, got -2", + } response = droneStatus.drone.flightModesController.setFlightMode(1, 28) - assert response.get("success") is False - assert ( - response.get("message") - == "Invalid copter flight mode, must be between 0 and 27 inclusive, got 28" - ) + assert response == { + "success": False, + "message": "Invalid copter flight mode, must be between 0 and 27 inclusive, got 28", + } - with ParamSetTimeout(): + with WaitForMessageReturnsNone(): response = droneStatus.drone.flightModesController.setFlightMode(1, 1) - assert response.get("success") is False - assert ( - response.get("message") - == "Failed to set flight mode 1 to COPTER_MODE_ACRO" - ) + assert response == { + "success": False, + "message": "Failed to set flight mode 1 to COPTER_MODE_ACRO", + } response = droneStatus.drone.flightModesController.setFlightMode(1, 27) - assert response.get("success") is True - assert response.get("message") == "Flight mode 1 set to COPTER_MODE_AUTO_RTL" + assert response == { + "success": True, + "message": "Flight mode 1 set to COPTER_MODE_AUTO_RTL", + } assert droneStatus.drone.flightModesController.flight_modes[0] == 27 diff --git a/radio/tests/test_motorController.py b/radio/tests/test_motorController.py index b87bb426c..baac9f632 100644 --- a/radio/tests/test_motorController.py +++ b/radio/tests/test_motorController.py @@ -1,7 +1,7 @@ from flask_socketio.test_client import SocketIOTestClient from . import falcon_test -from .helpers import FakeTCP, NoAcknowledgementMessage +from .helpers import FakeTCP, WaitForMessageReturnsNone def validate_motor_test_values( @@ -179,7 +179,7 @@ def test_testOneMotor(client: SocketIOTestClient, droneStatus): ) # Motor test not started due to not receiving acknowledgement message - with NoAcknowledgementMessage(): + with WaitForMessageReturnsNone(): run_motor_test( droneStatus, test_func, @@ -296,7 +296,7 @@ def test_testMotorSequence(client: SocketIOTestClient, droneStatus): ) # Motor test not started due to not receiving acknowledgement message - with NoAcknowledgementMessage(): + with WaitForMessageReturnsNone(): run_motor_test( droneStatus, test_func, @@ -413,7 +413,7 @@ def test_testAllMotors(client: SocketIOTestClient, droneStatus): ) # Motor test not started due to not receiving acknowledgement message - with NoAcknowledgementMessage(): + with WaitForMessageReturnsNone(): run_motor_test( droneStatus, test_func, From 0fbc9546a7ce249cb60ffc221a498a88e362d7cc Mon Sep 17 00:00:00 2001 From: Kush Makkapati Date: Fri, 31 Oct 2025 12:28:30 +0000 Subject: [PATCH 04/12] Fix remaining tests, fix bug with setParam and testOneMotor functions, move telemetry messages to separate namespace --- radio/app/controllers/motorTestController.py | 4 ++-- radio/app/controllers/paramsController.py | 19 +++++++++++-------- radio/app/utils.py | 2 +- radio/tests/helpers.py | 12 +++++------- radio/tests/test_params.py | 6 +++--- 5 files changed, 22 insertions(+), 21 deletions(-) diff --git a/radio/app/controllers/motorTestController.py b/radio/app/controllers/motorTestController.py index 11e7adb35..07aefeebb 100644 --- a/radio/app/controllers/motorTestController.py +++ b/radio/app/controllers/motorTestController.py @@ -86,6 +86,8 @@ def testOneMotor(self, data: MotorTestAllValues) -> Response: "message": "Could not reserve COMMAND_ACK messages", } + motor_letter = chr(64 + motor_instance) + try: self.drone.sendCommand( mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, @@ -97,8 +99,6 @@ def testOneMotor(self, data: MotorTestAllValues) -> Response: param6=0, # test order ) - motor_letter = chr(64 + motor_instance) - response = self.drone.wait_for_message( "COMMAND_ACK", self.controller_id, diff --git a/radio/app/controllers/paramsController.py b/radio/app/controllers/paramsController.py index 292b2a324..3088c2bee 100644 --- a/radio/app/controllers/paramsController.py +++ b/radio/app/controllers/paramsController.py @@ -97,7 +97,6 @@ def getSingleParam(self, param_name: str, timeout: float = 3) -> Response: "message": f"{failure_message}, serial exception", } finally: - # Always release the message type self.drone.release_message_type("PARAM_VALUE", self.controller_id) def getAllParams(self) -> None: @@ -239,10 +238,6 @@ def setParam( got_ack = False save_timeout = 5 - if not self.drone.reserve_message_type("PARAM_VALUE", self.controller_id): - self.drone.logger.error("Could not reserve PARAM_VALUE messages") - return False - try: # Check if value fits inside the param type # https://github.com/ArduPilot/pymavlink/blob/4d8c4ff274d41b9bc8da1a411cb172d39786e46b/mavparm.py#L30C10-L30C10 @@ -268,7 +263,7 @@ def setParam( "can't send %s of type %u" % (param_name, param_type) ) return False - # vfloat, = struct.unpack(">f", vstr) + vfloat = float(param_value) except struct.error as e: self.drone.logger.error( @@ -276,6 +271,10 @@ def setParam( ) return False + if not self.drone.reserve_message_type("PARAM_VALUE", self.controller_id): + self.drone.logger.error("Could not reserve PARAM_VALUE messages") + return False + try: # Keep trying to set the parameter until we get an ack or run out of retries or timeout while retries > 0 and not got_ack: @@ -284,15 +283,19 @@ def setParam( param_name.upper(), vfloat, parm_type=param_type ) + print( + f"Setting param {param_name} to {vfloat}, retries left: {retries}" + ) + # Wait for parameter acknowledgment using the new system ack = self.drone.wait_for_message( "PARAM_VALUE", self.controller_id, save_timeout, - condition_func=lambda msg: str(param_name).upper() - == str(msg.param_id).upper(), ) + print(f"Received ack: {ack}") + if ack: got_ack = True self.drone.logger.debug( diff --git a/radio/app/utils.py b/radio/app/utils.py index 49c42a071..9e9bac50c 100644 --- a/radio/app/utils.py +++ b/radio/app/utils.py @@ -188,7 +188,7 @@ def sendMessage(msg: Any) -> None: """ data = msg.to_dict() data["timestamp"] = msg._timestamp - socketio.emit("incoming_msg", data) + socketio.emit("incoming_msg", data, namespace="/telemetry") FIXED_WING_TYPES = [ diff --git a/radio/tests/helpers.py b/radio/tests/helpers.py index 8c64c043c..939bd8bcc 100644 --- a/radio/tests/helpers.py +++ b/radio/tests/helpers.py @@ -53,15 +53,13 @@ class ParamRefreshTimeout: """ @staticmethod - def recv_msg_false_value( - condition=None, Type=None, blocking=False, timeout=None - ) -> bool: - return False + def returns_none(*args, **kwargs) -> None: + return None def __enter__(self) -> None: if droneStatus.drone is not None: - self.old_recv_msg = droneStatus.drone.master.recv_msg - droneStatus.drone.master.recv_msg = ParamRefreshTimeout.recv_msg_false_value + self.wait_for_message = droneStatus.drone.wait_for_message + droneStatus.drone.wait_for_message = WaitForMessageReturnsNone.returns_none self.old_param_index = ( droneStatus.drone.paramsController.current_param_index ) @@ -69,7 +67,7 @@ def __enter__(self) -> None: def __exit__(self, type, value, traceback) -> None: if droneStatus.drone is not None: - droneStatus.drone.master.recv_msg = self.old_recv_msg + droneStatus.drone.wait_for_message = self.wait_for_message droneStatus.drone.paramsController.current_param_index = ( self.old_param_index ) diff --git a/radio/tests/test_params.py b/radio/tests/test_params.py index b6350cd82..9db8f1079 100644 --- a/radio/tests/test_params.py +++ b/radio/tests/test_params.py @@ -4,7 +4,7 @@ from flask_socketio.test_client import SocketIOTestClient from . import falcon_test -from .helpers import ParamRefreshTimeout, ParamSetTimeout +from .helpers import ParamRefreshTimeout, WaitForMessageReturnsNone def send_and_receive_params( @@ -212,11 +212,11 @@ def test_setMultipleParams_invalidData( @falcon_test(pass_drone_status=True) -def test_setMultipleParams_paramSetTimeout( +def test_setMultipleParams_WaitForMessageReturnsNone( socketio_client: SocketIOTestClient, droneStatus ) -> None: droneStatus.state = "params" - with ParamSetTimeout(): + with WaitForMessageReturnsNone(): socketio_result = send_and_receive_params( socketio_client, "set_multiple_params", From 2a1766dd9ac9f48c3a0d70f2590978e3702839e3 Mon Sep 17 00:00:00 2001 From: Kush Makkapati Date: Fri, 31 Oct 2025 13:06:20 +0000 Subject: [PATCH 05/12] Convert frontend to accept telemetry from telemetry namespace --- gcs/src/helpers/socket.js | 2 + gcs/src/redux/middleware/socketMiddleware.js | 173 +++++++++++-------- radio/app/__init__.py | 5 + radio/app/drone.py | 5 +- radio/app/endpoints/telemetry_namespace.py | 15 ++ 5 files changed, 126 insertions(+), 74 deletions(-) create mode 100644 radio/app/endpoints/telemetry_namespace.py diff --git a/gcs/src/helpers/socket.js b/gcs/src/helpers/socket.js index 03423071b..c1243b7ac 100644 --- a/gcs/src/helpers/socket.js +++ b/gcs/src/helpers/socket.js @@ -3,10 +3,12 @@ import { io } from "socket.io-client" class SocketConnection { socket + telemetrySocket socketEndpoint = import.meta.env.VITE_BACKEND_URL constructor() { this.socket = io(this.socketEndpoint) + this.telemetrySocket = io(this.socketEndpoint + "/telemetry") } } diff --git a/gcs/src/redux/middleware/socketMiddleware.js b/gcs/src/redux/middleware/socketMiddleware.js index 2de0dcc65..22bac8f52 100644 --- a/gcs/src/redux/middleware/socketMiddleware.js +++ b/gcs/src/redux/middleware/socketMiddleware.js @@ -360,6 +360,103 @@ const socketMiddleware = (store) => { socket.socket.on(SocketEvents.linkDebugStats, (msg) => { window.ipcRenderer.invoke("app:update-link-stats", msg) }) + + /* + Telemetry Socket Connection Events + */ + socket.telemetrySocket.on("connect", () => { + console.log( + `Connected to telemetry socket: ${socket.telemetrySocket.id}`, + ) + }) + + socket.telemetrySocket.on("disconnect", () => { + console.log( + `Disconnected from telemetry socket: ${socket.telemetrySocket.id}`, + ) + }) + + socket.telemetrySocket.on("connect_error", (error) => { + console.error("Telemetry socket connection error:", error) + }) + + // I don't understand whatsoever why this doesn't work with the standard + // on method. + socket.telemetrySocket.onAny((eventName, ...args) => { + if (eventName !== DroneSpecificSocketEvents.onIncomingMsg) { + return + } + + const msg = args[0] + + incomingMessageHandler(msg) + + // Data points on dashboard, the below code updates the value in the store when a new message + // comes in in the type of specificData. + const packetType = msg.mavpackettype + const storeState = store.getState() + if (storeState !== undefined) { + const extraDroneData = storeState.droneInfo.extraDroneData + const updatedExtraDroneData = extraDroneData.map((dataItem) => { + if (dataItem.currently_selected.startsWith(packetType)) { + const specificData = dataItem.currently_selected.split(".")[1] + if (Object.prototype.hasOwnProperty.call(msg, specificData)) { + return { ...dataItem, value: msg[specificData] } + } + } + return dataItem + }) + + store.dispatch(setExtraData(updatedExtraDroneData)) + } + + // Handle graph messages + // Function to get the graph data from a message + function getGraphDataFromMessage(msg, targetMessageKey) { + const returnDataArray = [] + for (let graphKey in storeState.droneInfo.graphs.selectedGraphs) { + const messageKey = + storeState.droneInfo.graphs.selectedGraphs[graphKey] + if (messageKey && messageKey.includes(targetMessageKey)) { + const [, valueName] = messageKey.split(".") + + // Applying Data Formatters + let formatted_value = msg[valueName] + if (messageKey in dataFormatters) { + formatted_value = dataFormatters[messageKey]( + msg[valueName].toFixed(3), + ) + } + + returnDataArray.push({ + data: { x: Date.now(), y: formatted_value }, + graphKey: graphKey, + }) + } + } + if (returnDataArray.length) { + return returnDataArray + } + return false + } + store.dispatch( + setLastGraphMessage( + getGraphDataFromMessage(msg, msg.mavpackettype), + ), + ) + + // Handle Flight Mode incoming data + if ( + msg.mavpackettype === "RC_CHANNELS" && + storeState.config.flightModeChannel !== "UNKNOWN" + ) { + store.dispatch( + setCurrentPwmValue( + msg[`chan${storeState.config.flightModeChannel}_raw`], + ), + ) + } + }) } } @@ -870,79 +967,6 @@ const socketMiddleware = (store) => { } }, ) - - /* - Generic Drone Data - */ - socket.socket.on(DroneSpecificSocketEvents.onIncomingMsg, (msg) => { - incomingMessageHandler(msg) - - // Data points on dashboard, the below code updates the value in the store when a new message - // comes in in the type of specificData. - const packetType = msg.mavpackettype - const storeState = store.getState() - if (storeState !== undefined) { - const extraDroneData = storeState.droneInfo.extraDroneData - const updatedExtraDroneData = extraDroneData.map((dataItem) => { - if (dataItem.currently_selected.startsWith(packetType)) { - const specificData = dataItem.currently_selected.split(".")[1] - if (Object.prototype.hasOwnProperty.call(msg, specificData)) { - return { ...dataItem, value: msg[specificData] } - } - } - return dataItem - }) - - store.dispatch(setExtraData(updatedExtraDroneData)) - } - - // Handle graph messages - // Function to get the graph data from a message - function getGraphDataFromMessage(msg, targetMessageKey) { - const returnDataArray = [] - for (let graphKey in storeState.droneInfo.graphs.selectedGraphs) { - const messageKey = - storeState.droneInfo.graphs.selectedGraphs[graphKey] - if (messageKey && messageKey.includes(targetMessageKey)) { - const [, valueName] = messageKey.split(".") - - // Applying Data Formatters - let formatted_value = msg[valueName] - if (messageKey in dataFormatters) { - formatted_value = dataFormatters[messageKey]( - msg[valueName].toFixed(3), - ) - } - - returnDataArray.push({ - data: { x: Date.now(), y: formatted_value }, - graphKey: graphKey, - }) - } - } - if (returnDataArray.length) { - return returnDataArray - } - return false - } - store.dispatch( - setLastGraphMessage( - getGraphDataFromMessage(msg, msg.mavpackettype), - ), - ) - - // Handle Flight Mode incoming data - if ( - msg.mavpackettype === "RC_CHANNELS" && - storeState.config.flightModeChannel !== "UNKNOWN" - ) { - store.dispatch( - setCurrentPwmValue( - msg[`chan${storeState.config.flightModeChannel}_raw`], - ), - ) - } - }) } else { // Turn off socket events Object.values(DroneSpecificSocketEvents).map((event) => @@ -957,6 +981,9 @@ const socketMiddleware = (store) => { Object.values(ConfigSpecificSocketEvents).map((event) => socket.socket.off(event), ) + + // Turn off telemetry socket events + socket.telemetrySocket.off(DroneSpecificSocketEvents.onIncomingMsg) } } diff --git a/radio/app/__init__.py b/radio/app/__init__.py index 102ea4678..29fea1385 100644 --- a/radio/app/__init__.py +++ b/radio/app/__init__.py @@ -23,6 +23,7 @@ def create_app(debug: bool = False) -> Flask: debug: Boolean value for if the debugging should be True or False """ from app.endpoints import endpoints + from app.endpoints.telemetry_namespace import TelemetryNamespace app = Flask(__name__) app.debug = debug @@ -32,4 +33,8 @@ def create_app(debug: bool = False) -> Flask: logger.info("Initialising app") socketio.init_app(app) + + # Register telemetry namespace + socketio.on_namespace(TelemetryNamespace("/telemetry")) + return app diff --git a/radio/app/drone.py b/radio/app/drone.py index a9729db33..b46b0d0d9 100644 --- a/radio/app/drone.py +++ b/radio/app/drone.py @@ -242,7 +242,7 @@ def __init__( self.stopAllDataStreams() - if forwarding_address is not None: + if forwarding_address is not None and len(forwarding_address) > 0: try: start_forwarding_result = self.startForwardingToAddress( forwarding_address @@ -1126,6 +1126,9 @@ def startForwardingToAddress(self, address: str) -> Response: r"^((udpout|tcpout):(([0-9]{1,3}\.){3}[0-9]{1,3}):([0-9]{1,5}))$", address ) if not match: + self.logger.warning( + f"Invalid forwarding address format. Must be in the format udpout:IP:PORT or tcpout:IP:PORT, got {address}" + ) return { "success": False, "message": "Address must be in the format udpout:IP:PORT or tcpout:IP:PORT", diff --git a/radio/app/endpoints/telemetry_namespace.py b/radio/app/endpoints/telemetry_namespace.py new file mode 100644 index 000000000..9503582cb --- /dev/null +++ b/radio/app/endpoints/telemetry_namespace.py @@ -0,0 +1,15 @@ +from flask_socketio import Namespace + +from app import logger + + +class TelemetryNamespace(Namespace): + """Namespace handler for /telemetry""" + + def on_connect(self): + """Handle client connection to telemetry namespace""" + logger.info("Client connected to telemetry namespace") + + def on_disconnect(self): + """Handle client disconnection from telemetry namespace""" + logger.info("Client disconnected from telemetry namespace") From e75582d16fe18bcd80955c2d7d5e3ca7be880f7a Mon Sep 17 00:00:00 2001 From: Kush Makkapati Date: Fri, 31 Oct 2025 13:11:04 +0000 Subject: [PATCH 06/12] Fix mypy issues --- radio/app/drone.py | 2 +- radio/tests/helpers.py | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/radio/app/drone.py b/radio/app/drone.py index b46b0d0d9..9ba1fb2ee 100644 --- a/radio/app/drone.py +++ b/radio/app/drone.py @@ -562,7 +562,7 @@ def wait_for_message( controller_id: str, timeout: float = 3.0, condition_func=None, - ) -> Optional[object]: + ) -> Optional[mavutil.mavlink.MAVLink_message]: """Wait for a specific message type for a controller. Args: diff --git a/radio/tests/helpers.py b/radio/tests/helpers.py index 939bd8bcc..adba63a65 100644 --- a/radio/tests/helpers.py +++ b/radio/tests/helpers.py @@ -59,7 +59,7 @@ def returns_none(*args, **kwargs) -> None: def __enter__(self) -> None: if droneStatus.drone is not None: self.wait_for_message = droneStatus.drone.wait_for_message - droneStatus.drone.wait_for_message = WaitForMessageReturnsNone.returns_none + droneStatus.drone.wait_for_message = WaitForMessageReturnsNone.returns_none # type: ignore[method-assign] self.old_param_index = ( droneStatus.drone.paramsController.current_param_index ) @@ -67,7 +67,7 @@ def __enter__(self) -> None: def __exit__(self, type, value, traceback) -> None: if droneStatus.drone is not None: - droneStatus.drone.wait_for_message = self.wait_for_message + droneStatus.drone.wait_for_message = self.wait_for_message # type: ignore[method-assign] droneStatus.drone.paramsController.current_param_index = ( self.old_param_index ) @@ -81,11 +81,11 @@ def returns_none(*args, **kwargs) -> None: def __enter__(self) -> None: if droneStatus.drone is not None: self.wait_for_message = droneStatus.drone.wait_for_message - droneStatus.drone.wait_for_message = WaitForMessageReturnsNone.returns_none + droneStatus.drone.wait_for_message = WaitForMessageReturnsNone.returns_none # type: ignore[method-assign] def __exit__(self, type, value, traceback) -> None: if droneStatus.drone is not None: - droneStatus.drone.wait_for_message = self.wait_for_message + droneStatus.drone.wait_for_message = self.wait_for_message # type: ignore[method-assign] class SetAircraftType: From 1e8d17d1403c48c90dd99a58472a6ead382a9f11 Mon Sep 17 00:00:00 2001 From: Kush Makkapati Date: Fri, 31 Oct 2025 13:35:54 +0000 Subject: [PATCH 07/12] Update python packages, add timeout to reboot test, fix autopilot reboot function --- radio/app/drone.py | 8 +++++--- radio/requirements.txt | Bin 3136 -> 1425 bytes radio/tests/test_autopilot.py | 2 ++ 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/radio/app/drone.py b/radio/app/drone.py index 9ba1fb2ee..2bb265653 100644 --- a/radio/app/drone.py +++ b/radio/app/drone.py @@ -935,6 +935,9 @@ def rebootAutopilot(self) -> None: self.controller_id, ) + self.sending_command_lock.release() + self.release_message_type("COMMAND_ACK", self.controller_id) + if commandAccepted( response, mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN ): @@ -944,10 +947,9 @@ def rebootAutopilot(self) -> None: self.logger.error("Reboot failed") except serial.serialutil.SerialException: self.logger.debug("Rebooting") - self.close() - finally: self.sending_command_lock.release() self.release_message_type("COMMAND_ACK", self.controller_id) + self.close() # TODO: Move this out into a controller @sendingCommandLock @@ -1161,8 +1163,8 @@ def close(self) -> None: for message_id in copy.deepcopy(self.message_listeners): self.removeMessageListener(message_id) - self.stopForwarding() self.stopAllDataStreams() + self.stopForwarding() self.stopAllThreads() self.master.close() diff --git a/radio/requirements.txt b/radio/requirements.txt index 50cbdb97756fd433ffaab62ec9e84c2f4f889d16..61d4cf91438cea61db61f7fea21424e028778f6e 100644 GIT binary patch literal 1425 zcmYjR%Wm8-47}@KZ1}aCnlRc=p!yeO^*TgU!NQL*lD94HD;jQa}dbqzo>O!KoA@AQ7LF_9EL#ML@})65oKG-hzD zxc4Q1%n)3NcC}iyE6--+B#tk7Y;ir=Bu^>$G%Tb)p|1zDHnAfY6a}S~*=J`{K53`< zQ2fL_$(Qeae*)CmOfkHXcfeN)AZ4aq_DETbd&srab^g}wc_a^dhgL*-jetja#o=`e zQF|;_5<6N!9D-<)pWOaBUJ5WodRF}*$9V{T;#tx1j|m9c7kY0DOB$`8DTZBV0a+UJ zmHm@#GCiC=D$tb;wEhq((nbR&F<4;;1%x~ho#4~H9YXQX5R~Q#b8C!yUEzl=8)UWd zI))ctuoN4(wz<1UMEC8dosgs5v}PUmh$NT>0Bi6;g6Bc`oz1#CdH2m3=_j)u3RR;( zmA?ym74qrUTb(QT%cQQjNdUJQA4Vb%@6_cz(l!{+&g@k%@WojcWtm%0VjHOg-l;b5 z$>yvm=E&;j>_@=3eV%hKCk2gE3}IavWm^ojGCz)a#hh-Ut2W!gK8bZs&3Z0tU5XE} zVhQ1z0OBR8MIkgsh(A{QtjnQtb!DeHi6ZOXp%6|&YC3ofD|b5;!YhRxQBBbdS%NR% zCfbFO*gyzv=oSu8Fsp+R{11|cLc~FbnMJ$(o3iaSjNa62+wsD_RWDWN|A%6bVZ%SG5BiTXtnO+qcTZzC#k^GPcg1v-I5c|7>Ro1S>R88ao?+Ofkrx4qi zvZH{fy~TZ+-gm1tcop_dLU2ev!zJLE7IIKQU%28Kk-;z(NpbG40f#2~9|MI2Dz&qp)9KWy+ewz*Z!mB=$k*{#chH)u{@ nrC$`UZ?waYx+1lCNn6AcXd~*8!|HmdPTKmyANphMtOKv5*ch?_}jp?-Yg`nsn1hKOts zqlLXySJ&$||NT2E^Rh1EvMW#J7k%1tRt9=5%MayUvGrcRH~JjPB%WLSztCqF5qMwd zIX{Wmm%Sp$IFzlGmAp0QPIYHx6`8WGJ?*nl{H$!F@_pnXLtB2-&xJj?G^*x{a>{i=cqZ3T6@)<>M`nb<%8|Y!&ug|gQ+c7M^xBm-DK&t z&S_kjD(q3aQDg$MlZ zf}^>5PdgQy;Z7)aF|WPQt@WRKCO+rzLC6-z*nY1Q^*K?DXHfYZ_M(|TtZw9m?pL9) zb$RTxn(Ft=VU>B(RrUHky*ocXYy$9-*J_wcMH?})jftX|M&D3>B>z$TCcT{ zh4WF9zKa-Z))?@I&!J9OKLSmo1|~iW9AK=IWEw_72C!QL-e_}S*##!_#C||gbvs&B zsO5L%UzLA^eh^jY*7L=>5ay+RQUeCTXnwC1v6aTq>5cxLstR77YkDoC2lf!X&{Otn zg)J4&UOnA(-WIu4mzoiqJ|innX=zmZMOk2!Elw@>N+*>wHVU)Huih;v)u-c=(7@b- z`pI7C#~s5q(J1dullPv&4`y5m&NhnXY>vb{+~)ME0yoN#C)st_p)gN1Yi%Q5P}EeI z*@A4UyjeV>enr$Z)2Fh8BHYy`5I3Qxr83bWRmsx#I>-a;-oU<5z%$UteI247_N8yz zymy#ay?2-ueV_U@u|L=1Whd^eZSKn4y91&1Jz`~$h28%o9?0fx1|E34IQE7%jJa~1 z8m_pR6}7-!d(W;=4rsBU2C5`?2Yv6&-)II*WFoFntTW;3n3}Enqx@5~`4;}aFJ9Km zM7h_2g-)wFVWeW|-_z;uEl~AN#B;DWR8O$J*Eguorvpz<7(9Q<_PM+-*X3hW&r|4) zi0aWbowLnfjtI%(_aA(|h#j+8w++?v6m4ba?EtRL1v>V-9cPyf%TE8+O>lami7*nr z@87*_95z?JrFajxm&$_Q#oktjBT|>=IxpNPFy(mb?eE~?t=;3TP^ZBS$vY~FA4dP? zo*9t!NfvIF$53fi4G8cdpU=i$s>$x4xo$2Y{`*qh;4>*Lotq`Iv-ytBR XTkcxUW#;upS;aYI7iULosKopg$l%CU diff --git a/radio/tests/test_autopilot.py b/radio/tests/test_autopilot.py index 816b6be04..cfc608680 100644 --- a/radio/tests/test_autopilot.py +++ b/radio/tests/test_autopilot.py @@ -1,8 +1,10 @@ +import pytest from flask_socketio.test_client import SocketIOTestClient from . import falcon_test +@pytest.mark.timeout(15) @falcon_test() def test_reboot_success(socketio_client: SocketIOTestClient): """ From a96ce3d8f177e92776b726fed5b11a20c27f8adf Mon Sep 17 00:00:00 2001 From: Kush Makkapati Date: Fri, 31 Oct 2025 13:36:37 +0000 Subject: [PATCH 08/12] Decrease python tests workflow timeout --- .github/workflows/python_tests.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/python_tests.yml b/.github/workflows/python_tests.yml index 7df579b46..de44019b7 100644 --- a/.github/workflows/python_tests.yml +++ b/.github/workflows/python_tests.yml @@ -39,5 +39,5 @@ jobs: - name: Test with pytest working-directory: radio - timeout-minutes: 25 + timeout-minutes: 15 run: pytest --log-cli-level=DEBUG From 5c38fc6b6e09ca93d4db17fdcbe12f489b2903bc Mon Sep 17 00:00:00 2001 From: Kush Makkapati Date: Fri, 31 Oct 2025 13:40:32 +0000 Subject: [PATCH 09/12] Remove pywin32 from requirements --- radio/requirements.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/radio/requirements.txt b/radio/requirements.txt index 61d4cf914..dba902c97 100644 --- a/radio/requirements.txt +++ b/radio/requirements.txt @@ -56,7 +56,6 @@ python-dateutil==2.8.2 python-dotenv==1.0.1 python-engineio==4.8.0 python-socketio==5.10.0 -pywin32==306 pywin32-ctypes==0.2.3 PyYAML==6.0.1 reactivex==4.0.4 From 978ce9576fe16fafd01a9c3b4d16cad0ef351e83 Mon Sep 17 00:00:00 2001 From: Kush Makkapati Date: Fri, 31 Oct 2025 13:50:39 +0000 Subject: [PATCH 10/12] Address copilot review comments, increase reboot test timeout, attempt to fix reboot test --- radio/app/controllers/armController.py | 2 - radio/app/controllers/paramsController.py | 6 -- radio/app/drone.py | 100 +++++++++++----------- radio/tests/test_autopilot.py | 2 +- 4 files changed, 52 insertions(+), 58 deletions(-) diff --git a/radio/app/controllers/armController.py b/radio/app/controllers/armController.py index 6b040c611..8ce7d5af7 100644 --- a/radio/app/controllers/armController.py +++ b/radio/app/controllers/armController.py @@ -132,5 +132,3 @@ def disarm(self, force: bool = False) -> Response: return {"success": False, "message": "Could not disarm, serial exception"} finally: self.drone.release_message_type("COMMAND_ACK", self.controller_id) - - return {"success": False, "message": "Could not disarm, command not accepted"} diff --git a/radio/app/controllers/paramsController.py b/radio/app/controllers/paramsController.py index 3088c2bee..0307302c6 100644 --- a/radio/app/controllers/paramsController.py +++ b/radio/app/controllers/paramsController.py @@ -283,10 +283,6 @@ def setParam( param_name.upper(), vfloat, parm_type=param_type ) - print( - f"Setting param {param_name} to {vfloat}, retries left: {retries}" - ) - # Wait for parameter acknowledgment using the new system ack = self.drone.wait_for_message( "PARAM_VALUE", @@ -294,8 +290,6 @@ def setParam( save_timeout, ) - print(f"Received ack: {ack}") - if ack: got_ack = True self.drone.logger.debug( diff --git a/radio/app/drone.py b/radio/app/drone.py index 2bb265653..eb7b3bbc0 100644 --- a/radio/app/drone.py +++ b/radio/app/drone.py @@ -624,61 +624,61 @@ def checkForMessages(self) -> None: self.droneErrorCb(str(e)) continue - if msg: - if self.forwarding_connection is not None: - try: - msg_buf = msg.get_msgbuf() - self.forwarding_connection.write(msg_buf) - except Exception as e: - self.logger.error( - f"Failed to forward message: {e}", exc_info=True - ) - self.stopForwarding() + if msg is None: + # Avoid busy waiting + time.sleep(0.05) + continue - msg_name = msg.get_type() + if self.forwarding_connection is not None: + try: + msg_buf = msg.get_msgbuf() + self.forwarding_connection.write(msg_buf) + except Exception as e: + self.logger.error(f"Failed to forward message: {e}", exc_info=True) + self.stopForwarding() - if msg_name == "HEARTBEAT": - if ( - msg.autopilot == mavutil.mavlink.MAV_AUTOPILOT_INVALID - ): # No valid autopilot, e.g. a GCS or other MAVLink component - continue + msg_name = msg.get_type() - self.armed = bool( - msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED - ) + if msg_name == "HEARTBEAT": + if ( + msg.autopilot == mavutil.mavlink.MAV_AUTOPILOT_INVALID + ): # No valid autopilot, e.g. a GCS or other MAVLink component + continue - if self.armed: - try: - self.log_message_queue.put( - f"{msg._timestamp},{msg_name},{','.join([f'{message}:{msg.to_dict()[message]}' for message in msg.to_dict() if message != 'mavpackettype'])}" - ) - except Exception as e: - self.log_message_queue.put(f"Writing message failed! {e}") - continue + self.armed = bool( + msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED + ) - if msg_name == "TIMESYNC": - component_timestamp = msg.ts1 - local_timestamp = time.time_ns() - self.master.mav.timesync_send(local_timestamp, component_timestamp) + if self.armed: + try: + self.log_message_queue.put( + f"{msg._timestamp},{msg_name},{','.join([f'{message}:{msg.to_dict()[message]}' for message in msg.to_dict() if message != 'mavpackettype'])}" + ) + except Exception as e: + self.log_message_queue.put(f"Writing message failed! {e}") continue - elif msg_name == "STATUSTEXT": - self.logger.info(msg.text) - - with self.reservation_lock: - if msg_name in self.reserved_messages: - # Route to controller queues - for controller_id, queue in self.controller_queues.items(): - try: - queue.put((msg_name, msg), block=False) - except Exception: - # Queue full - pass - else: - # Route to normal message listeners - if msg_name in self.message_listeners: - self.message_queue.put([msg_name, msg]) - else: - time.sleep(0.05) + + if msg_name == "TIMESYNC": + component_timestamp = msg.ts1 + local_timestamp = time.time_ns() + self.master.mav.timesync_send(local_timestamp, component_timestamp) + continue + elif msg_name == "STATUSTEXT": + self.logger.info(msg.text) + + with self.reservation_lock: + if msg_name in self.reserved_messages: + # Route to controller queues + for controller_id, queue in self.controller_queues.items(): + try: + queue.put((msg_name, msg), block=False) + except Exception: + # Queue full + pass + else: + # Route to normal message listeners + if msg_name in self.message_listeners: + self.message_queue.put([msg_name, msg]) def executeMessages(self) -> None: """Executes message listeners based on messages from the message queue.""" @@ -1163,6 +1163,8 @@ def close(self) -> None: for message_id in copy.deepcopy(self.message_listeners): self.removeMessageListener(message_id) + self.is_active.clear() + self.stopAllDataStreams() self.stopForwarding() self.stopAllThreads() diff --git a/radio/tests/test_autopilot.py b/radio/tests/test_autopilot.py index cfc608680..19e9698de 100644 --- a/radio/tests/test_autopilot.py +++ b/radio/tests/test_autopilot.py @@ -4,7 +4,7 @@ from . import falcon_test -@pytest.mark.timeout(15) +@pytest.mark.timeout(30) @falcon_test() def test_reboot_success(socketio_client: SocketIOTestClient): """ From 1cfe277e43e6d3b88ef619cd7a63ad2392cc4131 Mon Sep 17 00:00:00 2001 From: Kush Makkapati Date: Fri, 31 Oct 2025 13:59:07 +0000 Subject: [PATCH 11/12] Attempt to fix reboot autopilot test --- radio/app/controllers/armController.py | 6 ++-- .../app/controllers/flightModesController.py | 3 +- radio/app/controllers/gripperController.py | 3 +- radio/app/controllers/missionController.py | 9 ++++-- radio/app/controllers/motorTestController.py | 8 +++-- radio/app/controllers/navController.py | 9 ++++-- radio/app/drone.py | 27 ++++++++++++----- radio/app/endpoints/autopilot.py | 15 +++++++--- radio/app/utils.py | 29 +++++++++++++++---- 9 files changed, 80 insertions(+), 29 deletions(-) diff --git a/radio/app/controllers/armController.py b/radio/app/controllers/armController.py index 8ce7d5af7..c643fdc7b 100644 --- a/radio/app/controllers/armController.py +++ b/radio/app/controllers/armController.py @@ -53,7 +53,8 @@ def arm(self, force: bool = False) -> Response: response = self.drone.wait_for_message( "COMMAND_ACK", self.controller_id, - timeout=3, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM): @@ -108,7 +109,8 @@ def disarm(self, force: bool = False) -> Response: response = self.drone.wait_for_message( "COMMAND_ACK", self.controller_id, - timeout=3, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM): diff --git a/radio/app/controllers/flightModesController.py b/radio/app/controllers/flightModesController.py index 56b1c62a8..f2b6d05e5 100644 --- a/radio/app/controllers/flightModesController.py +++ b/radio/app/controllers/flightModesController.py @@ -161,7 +161,8 @@ def setCurrentFlightMode(self, flightMode: int) -> Response: response = self.drone.wait_for_message( "COMMAND_ACK", self.controller_id, - timeout=3, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_DO_SET_MODE, ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_SET_MODE): diff --git a/radio/app/controllers/gripperController.py b/radio/app/controllers/gripperController.py index 1cae88b50..4224a89f0 100644 --- a/radio/app/controllers/gripperController.py +++ b/radio/app/controllers/gripperController.py @@ -134,7 +134,8 @@ def setGripper(self, action: str) -> Response: response = self.drone.wait_for_message( "COMMAND_ACK", self.controller_id, - timeout=3, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_DO_GRIPPER, ) self.drone.sending_command_lock.release() diff --git a/radio/app/controllers/missionController.py b/radio/app/controllers/missionController.py index 81a480351..ea12e5028 100644 --- a/radio/app/controllers/missionController.py +++ b/radio/app/controllers/missionController.py @@ -260,7 +260,8 @@ def getMissionItems( if progressUpdateCallback and response.count != 0: progressUpdateCallback( - f"Received waypoint {i+1}", (i + 1) / response.count + f"Received waypoint {i + 1}", + (i + 1) / response.count, ) break @@ -388,7 +389,8 @@ def startMission(self) -> Response: response = self.drone.wait_for_message( "COMMAND_ACK", self.controller_id, - timeout=3, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_MISSION_START, ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_MISSION_START): @@ -431,7 +433,8 @@ def restartMission(self) -> Response: response = self.drone.wait_for_message( "COMMAND_ACK", self.controller_id, - timeout=3, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT, ) if commandAccepted( diff --git a/radio/app/controllers/motorTestController.py b/radio/app/controllers/motorTestController.py index 07aefeebb..5d58ec51d 100644 --- a/radio/app/controllers/motorTestController.py +++ b/radio/app/controllers/motorTestController.py @@ -102,7 +102,8 @@ def testOneMotor(self, data: MotorTestAllValues) -> Response: response = self.drone.wait_for_message( "COMMAND_ACK", self.controller_id, - timeout=3, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST): @@ -173,7 +174,8 @@ def testMotorSequence(self, data: MotorTestThrottleDurationAndNumber) -> Respons response = self.drone.wait_for_message( "COMMAND_ACK", self.controller_id, - timeout=3, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST): @@ -244,6 +246,8 @@ def testAllMotors(self, data: MotorTestThrottleDurationAndNumber) -> Response: "COMMAND_ACK", self.controller_id, timeout=RESPONSE_TIMEOUT, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST): successful_responses += 1 diff --git a/radio/app/controllers/navController.py b/radio/app/controllers/navController.py index 1d89bfc08..af7ea3275 100644 --- a/radio/app/controllers/navController.py +++ b/radio/app/controllers/navController.py @@ -105,7 +105,8 @@ def setHomePosition(self, lat: float, lon: float, alt: float) -> Response: response = self.drone.wait_for_message( "COMMAND_ACK", self.controller_id, - timeout=2, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_DO_SET_HOME, ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_SET_HOME): @@ -161,7 +162,8 @@ def takeoff(self, alt: float) -> Response: response = self.drone.wait_for_message( "COMMAND_ACK", self.controller_id, - timeout=3, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, ) self.drone.sending_command_lock.release() @@ -204,7 +206,8 @@ def land(self) -> Response: response = self.drone.wait_for_message( "COMMAND_ACK", self.controller_id, - timeout=3, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_NAV_LAND, ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_NAV_LAND): diff --git a/radio/app/drone.py b/radio/app/drone.py index eb7b3bbc0..7568991f0 100644 --- a/radio/app/drone.py +++ b/radio/app/drone.py @@ -595,6 +595,9 @@ def wait_for_message( except Empty: continue + self.logger.debug( + f"Timeout waiting for message {message_type} for controller {controller_id}" + ) return None def checkForMessages(self) -> None: @@ -913,11 +916,15 @@ def getAutopilotVersion(self) -> None: finally: self.release_message_type("AUTOPILOT_VERSION", self.controller_id) - def rebootAutopilot(self) -> None: - """Reboot the autopilot.""" + def rebootAutopilot(self) -> bool: + """Reboot the autopilot. + + Returns: + bool: True if the reboot command was successfully sent and accepted, False otherwise. + """ if not self.reserve_message_type("COMMAND_ACK", self.controller_id): self.logger.error("Could not reserve COMMAND_ACK messages for reboot") - return + return False try: self.sending_command_lock.acquire() @@ -933,6 +940,8 @@ def rebootAutopilot(self) -> None: response = self.wait_for_message( "COMMAND_ACK", self.controller_id, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, ) self.sending_command_lock.release() @@ -941,15 +950,18 @@ def rebootAutopilot(self) -> None: if commandAccepted( response, mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN ): - self.logger.debug("Rebooting") + self.logger.info("Rebooting autopilot") self.close() + return True else: - self.logger.error("Reboot failed") + self.logger.error("Reboot failed, command not accepted") + return False except serial.serialutil.SerialException: - self.logger.debug("Rebooting") + self.logger.info("Rebooting autopilot") self.sending_command_lock.release() self.release_message_type("COMMAND_ACK", self.controller_id) self.close() + return True # TODO: Move this out into a controller @sendingCommandLock @@ -979,7 +991,8 @@ def setServo(self, servo_instance: int, pwm_value: int) -> Response: response = self.wait_for_message( "COMMAND_ACK", self.controller_id, - timeout=3, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_DO_SET_SERVO, ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_SET_SERVO): diff --git a/radio/app/endpoints/autopilot.py b/radio/app/endpoints/autopilot.py index 62314d27c..a03032373 100644 --- a/radio/app/endpoints/autopilot.py +++ b/radio/app/endpoints/autopilot.py @@ -28,11 +28,18 @@ def rebootAutopilot() -> None: socketio.emit("disconnected_from_drone") - droneStatus.drone.rebootAutopilot() + reboot_success = droneStatus.drone.rebootAutopilot() - while droneStatus.drone.is_active.is_set(): - print("Waiting for drone to disconnect...") - time.sleep(0.05) + if not reboot_success: + logger.error("Failed to send reboot command to autopilot.") + socketio.emit( + "reboot_autopilot", + { + "success": False, + "message": "Failed to send reboot command to autopilot.", + }, + ) + return droneStatus.drone = None diff --git a/radio/app/utils.py b/radio/app/utils.py index 9e9bac50c..1693e5fe3 100644 --- a/radio/app/utils.py +++ b/radio/app/utils.py @@ -1,3 +1,4 @@ +import logging import sys from typing import Any, List, Optional @@ -9,6 +10,8 @@ from . import socketio +logger = logging.getLogger("fgcs") + def getComPort() -> str: """ @@ -98,12 +101,26 @@ def commandAccepted(response: Any, command: int) -> bool: Returns: True if the command has been accepted, False otherwise. """ - return bool( - response - and command - and response.command == command - and response.result == mavutil.mavlink.MAV_RESULT_ACCEPTED - ) + if command is None: + logger.warning("Command is None, cannot check if command accepted") + return False + + if response is None: + logger.warning(f"Response is None, cannot check if command {command} accepted") + return False + + if response.command != command: + logger.warning( + f"Command {command} does not match response command {response.command}" + ) + logger.debug(f"Full response: {response.to_dict()}, command: {command}") + return False + + if response.result != mavutil.mavlink.MAV_RESULT_ACCEPTED: + logger.warning(f"Command {command} not accepted, result: {response.result}") + return False + + return True def normalisePwmValue(val: float, min_val: float = 1000, max_val: float = 2000) -> int: From e8bb4fd1b0aaae1e146df7c29fba0f7ad727bedf Mon Sep 17 00:00:00 2001 From: Kush Makkapati Date: Fri, 31 Oct 2025 15:06:03 +0000 Subject: [PATCH 12/12] Address copilot review comments --- radio/app/controllers/missionController.py | 1 + radio/app/drone.py | 6 +++--- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/radio/app/controllers/missionController.py b/radio/app/controllers/missionController.py index ea12e5028..5bbb633b0 100644 --- a/radio/app/controllers/missionController.py +++ b/radio/app/controllers/missionController.py @@ -614,6 +614,7 @@ def uploadMission( "success": False, "message": "Could not reserve MISSION_REQUEST messages", } + if not self.drone.reserve_message_type("MISSION_ACK", self.controller_id): self.drone.release_message_type("MISSION_REQUEST", self.controller_id) return { diff --git a/radio/app/drone.py b/radio/app/drone.py index 7568991f0..5cbf816d3 100644 --- a/radio/app/drone.py +++ b/radio/app/drone.py @@ -242,7 +242,7 @@ def __init__( self.stopAllDataStreams() - if forwarding_address is not None and len(forwarding_address) > 0: + if forwarding_address: try: start_forwarding_result = self.startForwardingToAddress( forwarding_address @@ -552,9 +552,9 @@ def release_message_type(self, message_type: str, controller_id: str) -> None: with self.reservation_lock: self.reserved_messages.discard(message_type) # Clear any remaining messages in the controller's queue for this type + # by creating a new, empty queue if controller_id in self.controller_queues: - # We'll implement a cleanup mechanism if needed - pass + self.controller_queues[controller_id] = Queue() def wait_for_message( self,