diff --git a/gcs/src/params.jsx b/gcs/src/params.jsx index 6c2578025..e34ddb75b 100644 --- a/gcs/src/params.jsx +++ b/gcs/src/params.jsx @@ -81,10 +81,10 @@ export default function Params() { return ( + + {connected ? ( <> - - {fetchingVars && ( socket.socket.emit("set_state", action.payload), + callback: () => { + store.dispatch(setState(action.payload)) + socket.socket.emit("set_state", action.payload) + }, }, { emitter: emitGetHomePosition, diff --git a/gcs/src/redux/middleware/socketMiddleware.js b/gcs/src/redux/middleware/socketMiddleware.js index 5f57ec1e5..3ef4e831f 100644 --- a/gcs/src/redux/middleware/socketMiddleware.js +++ b/gcs/src/redux/middleware/socketMiddleware.js @@ -261,11 +261,18 @@ const socketMiddleware = (store) => { store.dispatch(setConnecting(false)) store.dispatch(setConnectionModal(false)) - store.dispatch(emitSetState({ state: "dashboard" })) - store.dispatch(emitGetHomePosition()) // use actual home position - if (msg.aircraft_type === 1) { - store.dispatch(emitGetLoiterRadius()) + const currentState = store.getState().droneConnection + store.dispatch(emitSetState(currentState)) + + if (["dashboard", "missions"].includes(currentState.state)) { + store.dispatch(emitGetHomePosition()) // fetch the actual home position of the drone + if (msg.aircraft_type === 1) { + store.dispatch(emitGetLoiterRadius()) + } } + + store.dispatch(setRebootData({})) + store.dispatch(setAutoPilotRebootModalOpen(false)) }) // Link stats @@ -321,6 +328,8 @@ const socketMiddleware = (store) => { store.dispatch(setRebootData(msg)) if (msg.success) { store.dispatch(setAutoPilotRebootModalOpen(false)) + store.dispatch(queueSuccessNotification(msg.message)) + store.dispatch(setRebootData({})) } }) diff --git a/gcs/src/redux/slices/droneConnectionSlice.js b/gcs/src/redux/slices/droneConnectionSlice.js index dc0aa66be..f5619592a 100644 --- a/gcs/src/redux/slices/droneConnectionSlice.js +++ b/gcs/src/redux/slices/droneConnectionSlice.js @@ -29,6 +29,8 @@ const initialState = { network_type: "tcp", // local ip: "127.0.0.1", // local port: "5760", // local + + state: "dashboard", } const droneConnectionSlice = createSlice({ @@ -101,6 +103,11 @@ const droneConnectionSlice = createSlice({ state.wireless = action.payload } }, + setState: (state, action) => { + if (action.payload !== state.state) { + state.state = action.payload + } + }, // Emits emitIsConnectedToDrone: () => {}, @@ -128,6 +135,7 @@ const droneConnectionSlice = createSlice({ selectConnectionModal: (state) => state.connection_modal, selectConnectionStatus: (state) => state.connection_status, selectWireless: (state) => state.wireless, + selectState: (state) => state.state, }, }) @@ -146,6 +154,7 @@ export const { setConnectionModal, setConnectionStatus, setWireless, + setState, // Emitters emitIsConnectedToDrone, @@ -171,6 +180,7 @@ export const { selectConnectionModal, selectConnectionStatus, selectWireless, + selectState, } = droneConnectionSlice.selectors export default droneConnectionSlice diff --git a/radio/app.py b/radio/app.py index 218c895d7..5480f0c62 100644 --- a/radio/app.py +++ b/radio/app.py @@ -1,7 +1,8 @@ import os +from pathlib import Path + import app.droneStatus as droneStatus from app import create_app, socketio -from pathlib import Path from dotenv import load_dotenv app = create_app(debug=True) @@ -20,7 +21,6 @@ host = "127.0.0.1" print("Starting backend.") - print(host) socketio.run(app, allow_unsafe_werkzeug=True, host=host, port=port) if droneStatus.drone: droneStatus.drone.close() diff --git a/radio/app/controllers/paramsController.py b/radio/app/controllers/paramsController.py index a0f9149c6..8c403ba39 100644 --- a/radio/app/controllers/paramsController.py +++ b/radio/app/controllers/paramsController.py @@ -30,7 +30,7 @@ def __init__(self, drone: Drone) -> None: self.getAllParamsThread: Optional[Thread] = None @sendingCommandLock - def getSingleParam(self, param_name: str, timeout: Optional[float] = 5) -> Response: + def getSingleParam(self, param_name: str, timeout: float = 3) -> Response: """ Gets a specific parameter value. @@ -53,18 +53,29 @@ def getSingleParam(self, param_name: str, timeout: Optional[float] = 5) -> Respo ) try: - response = self.drone.master.recv_match( - type="PARAM_VALUE", blocking=True, timeout=timeout - ) + 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 + ) + + if msg is None: + continue - if response and response.param_id == param_name: - self.drone.is_listening = True + if msg.param_id == param_name: + response = msg + break + + self.drone.is_listening = True + if response: return { "success": True, "data": response, } else: - self.drone.is_listening = True + self.drone.logger.error(f"Did not receive {param_name} within timeout") return { "success": False, "message": f"{failure_message}, timed out", diff --git a/radio/app/drone.py b/radio/app/drone.py index f03453ecf..dd9aa9b7f 100644 --- a/radio/app/drone.py +++ b/radio/app/drone.py @@ -99,7 +99,7 @@ def __init__( self.connectionError: Optional[str] = None - self.logger.debug("Trying to setup master") + self.logger.debug(f"Trying to setup master with port {port} and baud {baud}") if not Drone.checkBaudrateValid(baud): self.connectionError = ( diff --git a/radio/app/endpoints/autopilot.py b/radio/app/endpoints/autopilot.py index 9b4bd9a24..6641c8ef6 100644 --- a/radio/app/endpoints/autopilot.py +++ b/radio/app/endpoints/autopilot.py @@ -8,8 +8,10 @@ @socketio.on("reboot_autopilot") def rebootAutopilot() -> None: """ - Attempt to reboot the autopilot, this will try to reconnect to the drone 3 times before stopping. This will also stop if the port - is not open for 10 seconds. + Attempt to reboot the autopilot, this will try to reconnect to the drone 3 times before stopping. + + Note: If SITL is running and you are connected via TCP 5763 then rebooting does not work as expected. + Use TCP 5760 instead. """ if not droneStatus.drone: return @@ -20,12 +22,20 @@ def rebootAutopilot() -> None: droneErrorCb = droneStatus.drone.droneErrorCb droneDisconnectCb = droneStatus.drone.droneDisconnectCb droneConnectStatusCb = droneStatus.drone.droneConnectStatusCb + linkDebugStatsCb = droneStatus.drone.linkDebugStatsCb + socketio.emit("disconnected_from_drone") + droneStatus.drone.rebootAutopilot() - while droneStatus.drone is not None and droneStatus.drone.is_active.is_set(): + while droneStatus.drone.is_active.is_set(): + print("Waiting for drone to disconnect...") time.sleep(0.05) + droneStatus.drone = None + + time.sleep(1.5) # Wait for the port to be released and let the autopilot reboot + tries = 0 while tries < 3: droneStatus.drone = Drone( @@ -35,6 +45,7 @@ def rebootAutopilot() -> None: droneErrorCb=droneErrorCb, droneDisconnectCb=droneDisconnectCb, droneConnectStatusCb=droneConnectStatusCb, + linkDebugStatsCb=linkDebugStatsCb, ) if droneStatus.drone.connectionError: tries += 1 @@ -42,6 +53,7 @@ def rebootAutopilot() -> None: else: break else: + droneStatus.drone = None logger.error("Could not reconnect to drone after 3 attempts.") socketio.emit( "reboot_autopilot", @@ -53,7 +65,9 @@ def rebootAutopilot() -> None: return time.sleep(1) - socketio.emit("connected_to_drone") + socketio.emit( + "connected_to_drone", {"aircraft_type": droneStatus.drone.aircraft_type} + ) logger.info("Rebooted autopilot successfully.") socketio.emit( "reboot_autopilot", diff --git a/radio/tests/helpers.py b/radio/tests/helpers.py index dc00c69c9..db0f571dd 100644 --- a/radio/tests/helpers.py +++ b/radio/tests/helpers.py @@ -1,8 +1,9 @@ -import pytest from typing import Optional, Union -from serial.serialutil import SerialException +import pytest from app import droneStatus, logger +from serial.serialutil import SerialException + from . import socketio_client @@ -121,17 +122,17 @@ def __exit__(self, type, value, traceback) -> None: droneStatus.drone.master.recv_match = self.old_recv -class RecvMsgReturnsFalse: +class RecvMsgReturnsNone: @staticmethod def recv_match_false( condition=None, type=None, blocking=False, timeout=None - ) -> bool: - return False + ) -> 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 = RecvMsgReturnsFalse.recv_match_false + droneStatus.drone.master.recv_match = RecvMsgReturnsNone.recv_match_false def __exit__(self, type, value, traceback) -> None: if droneStatus.drone is not None: @@ -167,8 +168,10 @@ def send_and_recieve(endpoint: str, args: Optional[Union[dict, str]] = None) -> dict The data recieved from the client """ - socketio_client.emit(endpoint, args) if args is not None else socketio_client.emit( - endpoint + ( + socketio_client.emit(endpoint, args) + if args is not None + else socketio_client.emit(endpoint) ) return socketio_client.get_received()[0]["args"][0] diff --git a/radio/tests/test_FlightModesController.py b/radio/tests/test_FlightModesController.py index 5e6d43c60..8f063445a 100644 --- a/radio/tests/test_FlightModesController.py +++ b/radio/tests/test_FlightModesController.py @@ -4,7 +4,7 @@ from flask_socketio.test_client import SocketIOTestClient from . import falcon_test -from .helpers import FakeTCP, ParamSetTimeout, RecvMsgReturnsFalse, SetAircraftType +from .helpers import FakeTCP, ParamSetTimeout, RecvMsgReturnsNone, SetAircraftType @pytest.fixture(scope="module", autouse=True) @@ -41,7 +41,7 @@ def test_getFlightModes_success(client: SocketIOTestClient, droneStatus): @falcon_test(pass_drone_status=True) def test_getFlightModes_failure(client: SocketIOTestClient, droneStatus): - with RecvMsgReturnsFalse(): + with RecvMsgReturnsNone(): droneStatus.drone.flightModesController.getFlightModes() assert len(droneStatus.drone.flightModesController.flight_modes) == 6 for items in droneStatus.drone.flightModesController.flight_modes: @@ -50,7 +50,7 @@ def test_getFlightModes_failure(client: SocketIOTestClient, droneStatus): @falcon_test(pass_drone_status=True) def test_getFlightModeChannel_failure(client: SocketIOTestClient, droneStatus): - with RecvMsgReturnsFalse(): + with RecvMsgReturnsNone(): droneStatus.drone.flightModesController.getFlightModeChannel() assert droneStatus.drone.flightModesController.flight_mode_channel == "UNKNOWN" @@ -68,7 +68,7 @@ def test_setCurrentFlightMode(client: SocketIOTestClient, droneStatus): assert response.get("success") is False assert response.get("message") == "Could not set flight mode, serial exception" - with RecvMsgReturnsFalse(): + with RecvMsgReturnsNone(): response = droneStatus.drone.flightModesController.setCurrentFlightMode(1) assert response.get("success") is False assert response.get("message") == "Could not set flight mode" diff --git a/radio/tests/test_autopilot.py b/radio/tests/test_autopilot.py index c76a52405..816b6be04 100644 --- a/radio/tests/test_autopilot.py +++ b/radio/tests/test_autopilot.py @@ -3,7 +3,6 @@ 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): """