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 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/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..22bac8f52 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,13 +354,109 @@ const socketMiddleware = (store) => { store.dispatch(setAutoPilotRebootModalOpen(false)) store.dispatch(setShouldFetchAllMissionsOnDashboard(true)) store.dispatch(setShowMotorTestWarningModal(true)) - store.dispatch(resetMessages()) }) // Link stats 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`], + ), + ) + } + }) } } @@ -871,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) => @@ -958,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/controllers/armController.py b/radio/app/controllers/armController.py index b01f71e9a..c643fdc7b 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,25 @@ 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, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + ) 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 +66,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 +93,25 @@ 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, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + ) 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 +122,15 @@ 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"} - - return {"success": False, "message": "Could not disarm, command not accepted"} + finally: + self.drone.release_message_type("COMMAND_ACK", self.controller_id) diff --git a/radio/app/controllers/flightModesController.py b/radio/app/controllers/flightModesController.py index 9dd6b7a82..f2b6d05e5 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 @@ -86,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 { @@ -114,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 @@ -138,40 +139,47 @@ 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, - param1=1, - param2=flightMode, - param3=0, - param4=0, - param5=0, - param6=0, - param7=0, - ) try: - response = self.drone.master.recv_match( - type="COMMAND_ACK", blocking=True, timeout=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, + ) + + response = self.drone.wait_for_message( + "COMMAND_ACK", + self.controller_id, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_DO_SET_MODE, ) 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", + "message": "Could not set flight mode, command not accepted", } 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..4224a89f0 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,33 @@ 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, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_DO_GRIPPER, + ) - self.drone.is_listening = True self.drone.sending_command_lock.release() if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_GRIPPER): @@ -139,13 +150,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..5bbb633b0 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: @@ -257,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 @@ -284,11 +288,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 +315,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 +357,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 +364,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 +375,23 @@ 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, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_MISSION_START, + ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_MISSION_START): return { @@ -387,11 +404,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 +419,23 @@ 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, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT, + ) if commandAccepted( response, mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT @@ -423,11 +450,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 +469,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 +504,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 +609,65 @@ def uploadMission( "message": f"Cleared mission type {mission_type}, no waypoints to upload", } - self.drone.is_listening = False - self.drone.sending_command_lock.acquire() + if not self.drone.reserve_message_type("MISSION_REQUEST", self.controller_id): + return { + "success": False, + "message": "Could not reserve MISSION_REQUEST messages", + } - 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_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 +680,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 +702,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..5d58ec51d 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,31 @@ 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 - ) + if not self.drone.reserve_message_type("COMMAND_ACK", self.controller_id): + return { + "success": False, + "message": "Could not reserve COMMAND_ACK messages", + } motor_letter = chr(64 + motor_instance) 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 + response = self.drone.wait_for_message( + "COMMAND_ACK", + self.controller_id, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, + ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST): self.drone.logger.info(f"Motor test started for motor {motor_instance}") @@ -111,9 +120,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 +129,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 +143,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 +154,29 @@ 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, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, + ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST): self.drone.logger.info("Motor sequence test started") @@ -167,14 +184,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 +208,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 +220,68 @@ 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, + 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 - 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..af7ea3275 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,29 @@ 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, - timeout=2, + 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, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_DO_SET_HOME, ) - 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 +121,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 +148,43 @@ 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, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + ) + 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 +194,21 @@ 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, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_NAV_LAND, + ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_NAV_LAND): self.drone.logger.info("Land command send successfully") @@ -189,12 +218,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 +243,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 +276,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..0307302c6 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,26 @@ 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: + 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 +125,55 @@ 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 + + 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 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.is_listening = True 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,7 +235,6 @@ 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 @@ -248,30 +262,35 @@ 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) except struct.error as e: 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(): + 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: + 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, + ) + + if ack: got_ack = True self.drone.logger.debug( f"Got parameter saving ack for {param_name} for value {param_value}" @@ -279,13 +298,15 @@ 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}") - self.drone.is_listening = True - return True + 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) 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..5cbf816d3 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: @@ -233,7 +242,7 @@ def __init__( self.stopAllDataStreams() - if forwarding_address is not None: + if forwarding_address: try: start_forwarding_result = self.startForwardingToAddress( forwarding_address @@ -247,10 +256,6 @@ def __init__( self.setupControllers() - self.is_listening = True - - self.startThread() - self.sendConnectionStatusUpdate(12) self.sendStatusTextMessage( @@ -517,13 +522,87 @@ 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 + # by creating a new, empty queue + if controller_id in self.controller_queues: + self.controller_queues[controller_id] = Queue() + + def wait_for_message( + self, + message_type: str, + controller_id: str, + timeout: float = 3.0, + condition_func=None, + ) -> Optional[mavutil.mavlink.MAVLink_message]: + """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 + + self.logger.debug( + f"Timeout waiting for message {message_type} for controller {controller_id}" + ) + 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: @@ -548,46 +627,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 - if msg.msgname == "HEARTBEAT": - if ( - msg.autopilot == mavutil.mavlink.MAV_AUTOPILOT_INVALID - ): # No valid autopilot, e.g. a GCS or other MAVLink component - continue + 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() - self.armed = bool( - msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED - ) + msg_name = msg.get_type() - 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'])}" - ) - except Exception as e: - self.log_message_queue.put(f"Writing message failed! {e}") - continue + 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 msg.msgname == "TIMESYNC": - component_timestamp = msg.ts1 - local_timestamp = time.time_ns() - self.master.mav.timesync_send(local_timestamp, component_timestamp) + self.armed = bool( + msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED + ) + + 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.msgname == "STATUSTEXT": - self.logger.info(msg.text) - if msg.msgname in self.message_listeners: - self.message_queue.put([msg.msgname, msg]) + 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.""" @@ -763,7 +857,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 +880,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,39 +912,56 @@ 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 - ) + 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 False try: - response = self.master.recv_match(type="COMMAND_ACK", blocking=True) + 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, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, + ) + self.sending_command_lock.release() + self.release_message_type("COMMAND_ACK", self.controller_id) 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.is_listening = True + 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 @@ -866,17 +975,25 @@ 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, + condition_func=lambda msg: msg.command + == mavutil.mavlink.MAV_CMD_DO_SET_SERVO, + ) if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_SET_SERVO): return {"success": True, "message": f"Setting servo to {pwm_value}"} @@ -890,11 +1007,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, @@ -1023,6 +1141,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", @@ -1055,8 +1176,10 @@ def close(self) -> None: for message_id in copy.deepcopy(self.message_listeners): self.removeMessageListener(message_id) - self.stopForwarding() + self.is_active.clear() + self.stopAllDataStreams() + self.stopForwarding() self.stopAllThreads() self.master.close() 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/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") diff --git a/radio/app/utils.py b/radio/app/utils.py index 49c42a071..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: @@ -188,7 +205,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/requirements.txt b/radio/requirements.txt index 50cbdb977..dba902c97 100644 Binary files a/radio/requirements.txt and b/radio/requirements.txt differ diff --git a/radio/tests/helpers.py b/radio/tests/helpers.py index db0f571dd..adba63a65 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_match 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_match_override( + def return_serial_exception( condition=None, type=None, blocking=False, timeout=None ) -> None: raise SerialException( @@ -22,15 +21,13 @@ def recv_match_override( ) def __enter__(self) -> None: - # Replace drone mavtcp recv_match 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_send = droneStatus.drone.master.mav.send + droneStatus.drone.master.mav.send = FakeTCP.return_serial_exception def __exit__(self, type, value, traceback) -> None: - # Reset recv_match method if droneStatus.drone is not None: - droneStatus.drone.master.recv_match = self.old_recv + droneStatus.drone.master.mav.send = self.old_send class NoDrone: @@ -50,42 +47,19 @@ def __exit__(self, type, value, traceback) -> None: droneStatus.drone = self.oldDrone -class ParamSetTimeout: - """Context manager that replaces the mavlink recv_match 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( - 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 - - def __exit__(self, type, value, traceback) -> None: - if droneStatus.drone is not None: - droneStatus.drone.master.recv_match = 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 """ @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 # type: ignore[method-assign] self.old_param_index = ( droneStatus.drone.paramsController.current_param_index ) @@ -93,50 +67,25 @@ 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 # type: ignore[method-assign] droneStatus.drone.paramsController.current_param_index = ( self.old_param_index ) -class NoAcknowledgementMessage: - """Context manager that replaces the mavlink recv_match function in drone.master with a function that returns False - causing no acknowledgement messages to be received when used in the MotorTestController tests - """ - +class WaitForMessageReturnsNone: @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_match - droneStatus.drone.master.recv_match = ( - 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 - - -class RecvMsgReturnsNone: - @staticmethod - def recv_match_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_match - droneStatus.drone.master.recv_match = RecvMsgReturnsNone.recv_match_false + self.wait_for_message = droneStatus.drone.wait_for_message + 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.master.recv_match = self.old_recv + droneStatus.drone.wait_for_message = self.wait_for_message # type: ignore[method-assign] class SetAircraftType: diff --git a/radio/tests/test_FlightModesController.py b/radio/tests/test_FlightModesController.py index f24cdf760..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,101 +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, serial exception" + 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" + 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_autopilot.py b/radio/tests/test_autopilot.py index 816b6be04..19e9698de 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(30) @falcon_test() def test_reboot_success(socketio_client: SocketIOTestClient): """ 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, 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",