diff --git a/gcs/src/components/dashboard/tabsSectionTabs/actionTabsSection.jsx b/gcs/src/components/dashboard/tabsSectionTabs/actionTabsSection.jsx index ea65d7b86..5d524c852 100644 --- a/gcs/src/components/dashboard/tabsSectionTabs/actionTabsSection.jsx +++ b/gcs/src/components/dashboard/tabsSectionTabs/actionTabsSection.jsx @@ -39,8 +39,9 @@ export default function ActionTabsSection({ ) : (
- {/** Loiter Radius */} - + {aircraftType === 1 && ( + + )} {/** Flight Mode */} { // Flags that the drone is connected socket.socket.on("connected_to_drone", (msg) => { store.dispatch(setDroneAircraftType(msg.aircraft_type)) // There are two aircraftTypes, make sure to not use FLA one haha :D - if (msg.aircraft_type != 1 && msg.aircraft_type != 2) { + if (msg.aircraft_type !== 1 && msg.aircraft_type !== 2) { store.dispatch( queueErrorNotification( "Aircraft not of type quadcopter or plane", @@ -241,7 +241,9 @@ const socketMiddleware = (store) => { store.dispatch(emitSetState({ state: "dashboard" })) store.dispatch(emitGetHomePosition()) - store.dispatch(emitGetLoiterRadius()) + if (msg.aircraft_type === 1) { + store.dispatch(emitGetLoiterRadius()) + } }) // Link stats diff --git a/radio/app/controllers/armController.py b/radio/app/controllers/armController.py index 62a9e58e6..b01f71e9a 100644 --- a/radio/app/controllers/armController.py +++ b/radio/app/controllers/armController.py @@ -4,7 +4,7 @@ from typing import TYPE_CHECKING from app.customTypes import Response -from app.utils import commandAccepted +from app.utils import commandAccepted, sendingCommandLock from pymavlink import mavutil if TYPE_CHECKING: @@ -21,6 +21,7 @@ def __init__(self, drone: Drone) -> None: """ self.drone = drone + @sendingCommandLock def arm(self, force: bool = False) -> Response: """ Arm the drone. @@ -63,6 +64,7 @@ def arm(self, force: bool = False) -> Response: return {"success": False, "message": "Could not arm, serial exception"} return {"success": False, "message": "Could not arm, command not accepted"} + @sendingCommandLock def disarm(self, force: bool = False) -> Response: """ Disarm the drone. diff --git a/radio/app/controllers/flightModesController.py b/radio/app/controllers/flightModesController.py index 1ab678bf6..278dfbdf9 100644 --- a/radio/app/controllers/flightModesController.py +++ b/radio/app/controllers/flightModesController.py @@ -5,7 +5,7 @@ import serial from app.customTypes import Response -from app.utils import commandAccepted +from app.utils import commandAccepted, sendingCommandLock from pymavlink import mavutil if TYPE_CHECKING: @@ -126,6 +126,7 @@ def setFlightMode(self, mode_number: int, flight_mode: int) -> Response: "message": f"Failed to set flight mode {mode_number} to {mode_name}", } + @sendingCommandLock def setCurrentFlightMode(self, flightMode: int) -> Response: """ Sends a Mavlink message to the drone for setting its current flight mode diff --git a/radio/app/controllers/gripperController.py b/radio/app/controllers/gripperController.py index 4685dd1be..b999c713d 100644 --- a/radio/app/controllers/gripperController.py +++ b/radio/app/controllers/gripperController.py @@ -103,6 +103,7 @@ def setGripper(self, action: str) -> Response: } self.drone.is_listening = False + self.drone.sending_command_lock.acquire() self.drone.sendCommand( mavutil.mavlink.MAV_CMD_DO_GRIPPER, @@ -119,6 +120,7 @@ def setGripper(self, action: str) -> Response: response = self.drone.master.recv_match(type="COMMAND_ACK", blocking=True) self.drone.is_listening = True + self.drone.sending_command_lock.release() if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_GRIPPER): return { @@ -132,6 +134,7 @@ def setGripper(self, action: str) -> Response: } except serial.serialutil.SerialException: self.drone.is_listening = True + self.drone.sending_command_lock.release() return { "success": False, "message": "Setting gripper failed, serial exception", diff --git a/radio/app/controllers/missionController.py b/radio/app/controllers/missionController.py index 9c3b8d69b..c6139aa3a 100644 --- a/radio/app/controllers/missionController.py +++ b/radio/app/controllers/missionController.py @@ -5,7 +5,7 @@ import serial from app.customTypes import Number, Response -from app.utils import commandAccepted +from app.utils import commandAccepted, sendingCommandLock from pymavlink import mavutil, mavwp if TYPE_CHECKING: @@ -161,6 +161,7 @@ def getCurrentMissionAll(self) -> Response: }, } + @sendingCommandLock def getMissionItems( self, mission_type: int, @@ -282,7 +283,6 @@ def getMissionItems( "success": False, "message": failure_message, } - except serial.serialutil.SerialException: self.drone.is_listening = True return { @@ -311,6 +311,7 @@ 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, @@ -356,6 +357,7 @@ def getItemDetails( "message": f"{failure_message}, serial exception", } + @sendingCommandLock def startMission(self) -> Response: """ Start the mission on the drone. @@ -391,6 +393,7 @@ def startMission(self) -> Response: "message": "Failed to start mission, serial exception", } + @sendingCommandLock def restartMission(self) -> Response: """ Restarts the mission on the drone. @@ -426,6 +429,7 @@ def restartMission(self) -> Response: "message": "Failed to restart mission, serial exception", } + @sendingCommandLock def clearMission(self, mission_type: int) -> Response: """ Clears the specified mission type from the drone. @@ -438,6 +442,7 @@ def clearMission(self, mission_type: int) -> Response: return mission_type_check self.drone.is_listening = False + self.drone.master.mav.mission_clear_all_send( self.drone.target_system, self.drone.target_component, @@ -575,6 +580,7 @@ def uploadMission( } self.drone.is_listening = False + self.drone.sending_command_lock.acquire() self.drone.master.mav.mission_count_send( self.drone.target_system, @@ -592,6 +598,7 @@ def uploadMission( ) if not response: self.drone.is_listening = True + self.drone.sending_command_lock.release() return { "success": False, @@ -601,6 +608,8 @@ def uploadMission( self.drone.logger.error( f"Error uploading mission, mission ack response: {response.type}" ) + self.drone.is_listening = True + self.drone.sending_command_lock.release() return { "success": False, "message": "Could not upload mission, received mission acknowledgement error", @@ -628,6 +637,7 @@ def uploadMission( timeout=2, ) self.drone.is_listening = True + self.drone.sending_command_lock.release() if ( mission_ack_response @@ -641,6 +651,7 @@ def uploadMission( self.fenceLoader = new_loader else: self.rallyLoader = new_loader + return { "success": True, "message": "Mission uploaded successfully", @@ -655,6 +666,7 @@ def uploadMission( } 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", diff --git a/radio/app/controllers/motorTestController.py b/radio/app/controllers/motorTestController.py index 50bbe5ef7..2652ae0d8 100644 --- a/radio/app/controllers/motorTestController.py +++ b/radio/app/controllers/motorTestController.py @@ -9,7 +9,7 @@ MotorTestThrottleDurationAndNumber, Response, ) -from app.utils import commandAccepted +from app.utils import commandAccepted, sendingCommandLock from pymavlink import mavutil if TYPE_CHECKING: @@ -55,6 +55,7 @@ def checkMotorTestValues( return throttle, duration, None + @sendingCommandLock def testOneMotor(self, data: MotorTestAllValues) -> Response: """ Test a single motor. @@ -121,6 +122,7 @@ def testOneMotor(self, data: MotorTestAllValues) -> Response: "message": f"Motor test for motor {motor_letter} not started, serial exception", } + @sendingCommandLock def testMotorSequence(self, data: MotorTestThrottleDurationAndNumber) -> Response: """ Test a sequence of motors. @@ -174,6 +176,7 @@ def testMotorSequence(self, data: MotorTestThrottleDurationAndNumber) -> Respons "message": "Motor sequence test not started, serial exception", } + @sendingCommandLock def testAllMotors(self, data: MotorTestThrottleDurationAndNumber) -> Response: """ Test all motors. diff --git a/radio/app/controllers/navController.py b/radio/app/controllers/navController.py index 5dfa60656..89a4954ec 100644 --- a/radio/app/controllers/navController.py +++ b/radio/app/controllers/navController.py @@ -4,7 +4,7 @@ import serial from app.customTypes import Response -from app.utils import commandAccepted +from app.utils import commandAccepted, sendingCommandLock from pymavlink import mavutil if TYPE_CHECKING: @@ -24,6 +24,7 @@ def __init__(self, drone: Drone) -> None: self.loiter_radius_param_type = mavutil.mavlink.MAV_PARAM_TYPE_INT16 self.loiter_radius = 80.0 # Default loiter radius + @sendingCommandLock def getHomePosition(self) -> Response: """ Request the current home position from the drone. @@ -41,7 +42,7 @@ def getHomePosition(self) -> Response: self.drone.is_listening = True if response: - self.drone.logger.info(f"Home position received, {response}") + self.drone.logger.info("Home position received") home_position = { "lat": response.latitude, @@ -68,6 +69,7 @@ def getHomePosition(self) -> Response: "message": "Could not get home position, serial exception", } + @sendingCommandLock def setHomePosition(self, lat: float, lon: float, alt: float) -> Response: """ Set the home point of the drone. @@ -133,12 +135,14 @@ def takeoff(self, alt: float) -> Response: return guidedModeSetResult self.drone.is_listening = False + self.drone.sending_command_lock.acquire() self.drone.sendCommand(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, param7=alt) try: response = self.drone.master.recv_match(type="COMMAND_ACK", blocking=True) self.drone.is_listening = True + self.drone.sending_command_lock.release() if commandAccepted(response, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF): self.drone.is_listening = True @@ -157,6 +161,7 @@ def takeoff(self, alt: float) -> Response: "message": "Could not takeoff, serial exception", } + @sendingCommandLock def land(self) -> Response: """ Tells the drone to land. @@ -205,24 +210,25 @@ def reposition(self, lat: float, lon: float, alt: float) -> Response: self.drone.is_listening = False - 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, - ) + 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") @@ -250,7 +256,6 @@ def getLoiterRadius(self) -> Response: """ Get the loiter radius of the drone. """ - loiter_radius_data = self.drone.paramsController.getSingleParam( "WP_LOITER_RAD", timeout=1.5 ) diff --git a/radio/app/controllers/paramsController.py b/radio/app/controllers/paramsController.py index 19dde3ee5..2358d63d4 100644 --- a/radio/app/controllers/paramsController.py +++ b/radio/app/controllers/paramsController.py @@ -7,6 +7,7 @@ import serial from app.customTypes import IncomingParam, Number, Response +from app.utils import sendingCommandLock from pymavlink import mavutil if TYPE_CHECKING: @@ -28,6 +29,7 @@ def __init__(self, drone: Drone) -> None: self.is_requesting_params = False self.getAllParamsThread: Optional[Thread] = None + @sendingCommandLock def getSingleParam(self, param_name: str, timeout: Optional[float] = 5) -> Response: """ Gets a specific parameter value. @@ -158,6 +160,7 @@ def setMultipleParams(self, params_list: list[IncomingParam]) -> bool: return True + @sendingCommandLock def setParam( self, param_name: str, diff --git a/radio/app/drone.py b/radio/app/drone.py index cb5e2129a..8a8fb8826 100644 --- a/radio/app/drone.py +++ b/radio/app/drone.py @@ -6,7 +6,7 @@ from pathlib import Path from queue import Queue from secrets import token_hex -from threading import Thread +from threading import Lock, Thread from typing import Callable, Dict, List, Optional import serial @@ -23,7 +23,7 @@ from app.controllers.paramsController import ParamsController from app.controllers.rcController import RcController from app.customTypes import Number, Response, VehicleType -from app.utils import commandAccepted, getVehicleType +from app.utils import commandAccepted, getVehicleType, sendingCommandLock # Constants @@ -166,6 +166,11 @@ def __init__( self.sendConnectionStatusUpdate("Cleaned temp logs") self.is_active = True + self.is_listening = False + + # To ensure that only one command is sent at a time and we wait for a + # response before sending another command, a thread-safe lock is used + self.sending_command_lock = Lock() self.number_of_motors = 4 # Is there a way to get this from the drone? @@ -371,6 +376,7 @@ def setupSingleDataStream(self, stream: int) -> None: else: self.sendDataStreamRequestMessage(stream, DATASTREAM_RATES_WIRED[stream]) + @sendingCommandLock def sendDataStreamRequestMessage(self, stream: int, rate: int) -> None: """Send a request for a specific data stream. @@ -386,6 +392,7 @@ def sendDataStreamRequestMessage(self, stream: int, rate: int) -> None: 1, ) + @sendingCommandLock def stopAllDataStreams(self) -> None: """Stop all data streams""" self.master.mav.request_data_stream_send( @@ -637,6 +644,8 @@ def startThread(self) -> None: 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, # Autpilot @@ -647,6 +656,7 @@ def rebootAutopilot(self) -> None: try: response = self.master.recv_match(type="COMMAND_ACK", blocking=True) + self.sending_command_lock.release() if commandAccepted( response, mavutil.mavlink.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN @@ -660,6 +670,7 @@ def rebootAutopilot(self) -> None: self.logger.debug("Rebooting") self.close() + @sendingCommandLock def setServo(self, servo_instance: int, pwm_value: int) -> Response: """Set a servo to a specific PWM value. @@ -680,14 +691,21 @@ def setServo(self, servo_instance: int, pwm_value: int) -> Response: try: response = self.master.recv_match(type="COMMAND_ACK", blocking=True) + self.is_listening = True if commandAccepted(response, mavutil.mavlink.MAV_CMD_DO_SET_SERVO): return {"success": True, "message": f"Setting servo to {pwm_value}"} - else: - return {"success": False, "message": "Setting servo failed"} + self.logger.error( + f"Failed to set servo {servo_instance} to {pwm_value}" + ) + return { + "success": False, + "message": f"Failed to set servo {servo_instance} to {pwm_value}", + } except serial.serialutil.SerialException: + self.is_listening = True return { "success": False, "message": "Setting servo failed, serial exception", diff --git a/radio/app/utils.py b/radio/app/utils.py index d336eda34..f28fad66c 100644 --- a/radio/app/utils.py +++ b/radio/app/utils.py @@ -215,3 +215,17 @@ def getVehicleType(typeId: int) -> int: return VehicleType.MULTIROTOR.value else: return VehicleType.UNKNOWN.value + + +def sendingCommandLock(func): + """A decorator to ensure that only one command is sent at a time.""" + + def wrapper(self, *args, **kwargs): + lock = getattr(self, "drone", self).sending_command_lock + lock.acquire() + try: + return func(self, *args, **kwargs) + finally: + lock.release() + + return wrapper diff --git a/radio/tests/test_autopilot.py b/radio/tests/test_autopilot.py index 816b6be04..c76a52405 100644 --- a/radio/tests/test_autopilot.py +++ b/radio/tests/test_autopilot.py @@ -3,6 +3,7 @@ from . import falcon_test +# @pytest.mark.skip(reason="Test fails due to reboot_autopilot not functional") @falcon_test() def test_reboot_success(socketio_client: SocketIOTestClient): """