diff --git a/gcs/src/redux/middleware/emitters.js b/gcs/src/redux/middleware/emitters.js
index 70560892e..e3f3937f3 100644
--- a/gcs/src/redux/middleware/emitters.js
+++ b/gcs/src/redux/middleware/emitters.js
@@ -1,6 +1,7 @@
import { showErrorNotification } from "../../helpers/notification"
import {
emitBatchSetRcConfigParams,
+ emitGetFailsafeConfig,
emitBatchSetServoConfigParams,
emitGetFlightModeConfig,
emitGetFrameConfig,
@@ -14,6 +15,7 @@ import {
emitSetFlightModeChannel,
emitSetGripper,
emitSetGripperConfigParam,
+ emitSetFailsafeConfigParam,
emitSetGripperDisabled,
emitSetGripperEnabled,
emitSetRcConfigParam,
@@ -22,6 +24,7 @@ import {
emitTestAllMotors,
emitTestMotorSequence,
emitTestOneMotor,
+ setRefreshingFailsafeConfigData,
emitTestServoPwm,
setRefreshingGripperConfigData,
} from "../slices/configSlice"
@@ -351,6 +354,22 @@ export function handleEmitters(socket, store, action) {
= CONFIG =
==========
*/
+ {
+ emitter: emitGetFailsafeConfig,
+ callback: () => {
+ socket.socket.emit("get_failsafe_config")
+ store.dispatch(setRefreshingFailsafeConfigData(true))
+ },
+ },
+ {
+ emitter: emitSetFailsafeConfigParam,
+ callback: () => {
+ socket.socket.emit("set_failsafe_config_param", {
+ param_id: action.payload.param_id,
+ value: action.payload.value,
+ })
+ },
+ },
{
emitter: emitGetGripperEnabled,
callback: () => socket.socket.emit("get_gripper_enabled"),
diff --git a/gcs/src/redux/middleware/socketMiddleware.js b/gcs/src/redux/middleware/socketMiddleware.js
index 3d8343afa..4379002db 100644
--- a/gcs/src/redux/middleware/socketMiddleware.js
+++ b/gcs/src/redux/middleware/socketMiddleware.js
@@ -50,6 +50,7 @@ import {
emitGetGripperConfig,
setChannelsConfig,
setCurrentPwmValue,
+ setFailsafeConfig,
setFlightModeChannel,
setFlightModesList,
setFrameClass,
@@ -61,6 +62,7 @@ import {
setNumberOfMotors,
setRadioCalibrationModalOpen,
setRadioPwmChannels,
+ setRefreshingFailsafeConfigData,
setRefreshingFlightModeData,
setRefreshingGripperConfigData,
setSerialPortsConfig,
@@ -68,6 +70,7 @@ import {
setServoPwmOutputs,
setShowMotorTestWarningModal,
updateChannelsConfigParam,
+ updateFailsafeConfigParam,
updateGripperConfigParam,
updateSerialPortConfigParam,
updateServoConfigParam,
@@ -195,12 +198,14 @@ const MissionSpecificSocketEvents = Object.freeze({
})
const ConfigSpecificSocketEvents = Object.freeze({
+ onFailsafeConfig: "failsafe_config",
onGripperEnabled: "is_gripper_enabled",
onSetGripperEnabledResult: "set_gripper_enabled_result",
onSetGripperDisabledResult: "set_gripper_disabled_result",
onSetGripperResult: "set_gripper_result",
onGripperConfig: "gripper_config",
setGripperParamResult: "set_gripper_param_result",
+ setFailsafeParamResult: "set_failsafe_param_result",
onMotorTestResult: "motor_test_result",
onFlightModeConfig: "flight_mode_config",
onSetFlightModeResult: "set_flight_mode_result",
@@ -1237,6 +1242,11 @@ const socketMiddleware = (store) => {
= CONFIG =
==========
*/
+ socket.socket.on(ConfigSpecificSocketEvents.onFailsafeConfig, (msg) => {
+ store.dispatch(setFailsafeConfig(msg.params))
+ store.dispatch(setRefreshingFailsafeConfigData(false))
+ })
+
socket.socket.on(
ConfigSpecificSocketEvents.onGripperEnabled,
(enabled) => {
@@ -1303,6 +1313,23 @@ const socketMiddleware = (store) => {
},
)
+ socket.socket.on(
+ ConfigSpecificSocketEvents.setFailsafeParamResult,
+ (msg) => {
+ if (msg.success) {
+ showSuccessNotification(msg.message)
+ store.dispatch(
+ updateFailsafeConfigParam({
+ param_id: msg.param_id,
+ value: msg.value,
+ }),
+ )
+ } else {
+ showErrorNotification(msg.message)
+ }
+ },
+ )
+
socket.socket.on(
ConfigSpecificSocketEvents.onMotorTestResult,
(msg) => {
diff --git a/gcs/src/redux/slices/configSlice.js b/gcs/src/redux/slices/configSlice.js
index 4957c563f..9c4e16d4e 100644
--- a/gcs/src/redux/slices/configSlice.js
+++ b/gcs/src/redux/slices/configSlice.js
@@ -4,6 +4,8 @@ const configSlice = createSlice({
name: "config",
initialState: {
activeTab: null,
+ failsafeConfig: {},
+ refreshingFailsafeConfigData: false,
getGripperEnabled: false,
gripperConfig: {},
refreshingGripperConfigData: false,
@@ -82,6 +84,19 @@ const configSlice = createSlice({
if (state.gripperConfig[param_id] === value) return
state.gripperConfig[param_id] = value
},
+ setFailsafeConfig: (state, action) => {
+ if (action.payload === state.failsafeConfig) return
+ state.failsafeConfig = action.payload
+ },
+ updateFailsafeConfigParam: (state, action) => {
+ const { param_id, value } = action.payload
+ if (state.failsafeConfig[param_id] === value) return
+ state.failsafeConfig[param_id] = value
+ },
+ setRefreshingFailsafeConfigData: (state, action) => {
+ if (action.payload === state.refreshingFailsafeConfigData) return
+ state.refreshingFailsafeConfigData = action.payload
+ },
setRefreshingGripperConfigData: (state, action) => {
if (action.payload === state.refreshingGripperConfigData) return
state.refreshingGripperConfigData = action.payload
@@ -214,11 +229,13 @@ const configSlice = createSlice({
},
// Emits
+ emitGetFailsafeConfig: () => {},
emitGetGripperEnabled: () => {},
emitSetGripperEnabled: () => {},
emitSetGripperDisabled: () => {},
emitGetGripperConfig: () => {},
emitSetGripperConfigParam: () => {},
+ emitSetFailsafeConfigParam: () => {},
emitGetFlightModeConfig: () => {},
emitSetFlightMode: () => {},
emitSetFlightModeChannel: () => {},
@@ -241,6 +258,9 @@ const configSlice = createSlice({
},
selectors: {
selectActiveTab: (state) => state.activeTab,
+ selectFailsafeConfig: (state) => state.failsafeConfig,
+ selectRefreshingFailsafeConfigData: (state) =>
+ state.refreshingFailsafeConfigData,
selectGetGripperEnabled: (state) => state.getGripperEnabled,
selectGripperConfig: (state) => state.gripperConfig,
selectRefreshingGripperConfigData: (state) =>
@@ -266,9 +286,12 @@ const configSlice = createSlice({
export const {
setActiveTab,
+ setFailsafeConfig,
setGetGripperEnabled,
setGripperConfig,
updateGripperConfigParam,
+ updateFailsafeConfigParam,
+ setRefreshingFailsafeConfigData,
setRefreshingGripperConfigData,
setFlightModesList,
setFlightModeChannel,
@@ -290,11 +313,13 @@ export const {
setSerialPortsConfig,
updateSerialPortConfigParam,
+ emitGetFailsafeConfig,
emitGetGripperEnabled,
emitSetGripperEnabled,
emitSetGripperDisabled,
emitGetGripperConfig,
emitSetGripperConfigParam,
+ emitSetFailsafeConfigParam,
emitGetFlightModeConfig,
emitSetFlightMode,
emitSetFlightModeChannel,
@@ -318,6 +343,8 @@ export const {
export const {
selectActiveTab,
+ selectFailsafeConfig,
+ selectRefreshingFailsafeConfigData,
selectGetGripperEnabled,
selectGripperConfig,
selectRefreshingGripperConfigData,
diff --git a/radio/app/controllers/failsafeController.py b/radio/app/controllers/failsafeController.py
new file mode 100644
index 000000000..45423ad85
--- /dev/null
+++ b/radio/app/controllers/failsafeController.py
@@ -0,0 +1,81 @@
+from __future__ import annotations
+
+from typing import TYPE_CHECKING
+
+from app.customTypes import Number, VehicleType
+
+if TYPE_CHECKING:
+ from app.drone import Drone
+
+COMMON_FAILSAFE_PARAMS = [
+ "BATT_LOW_VOLT",
+ "BATT_LOW_MAH",
+ "BATT_FS_LOW_ACT",
+ "BATT_CRT_VOLT",
+ "BATT_CRT_MAH",
+ "BATT_FS_CRT_ACT",
+]
+
+COPTER_FS_PARAMS = [
+ "FS_THR_ENABLE",
+ "RC_FS_TIMEOUT",
+ "FS_GCS_TIMEOUT",
+ "FS_GCS_ENABLE",
+ "FS_EKF_THRESH",
+ "FS_EKF_ACTION",
+]
+
+PLANE_FS_PARAMS = [
+ "THR_FS_VALUE",
+ "THR_FAILSAFE",
+ "FS_GCS_ENABL",
+ "FS_SHORT_ACTN",
+ "FS_LONG_ACTN",
+ "FS_SHORT_TIMEOUT",
+ "FS_LONG_TIMEOUT",
+]
+
+
+class FailsafeController:
+ def __init__(self, drone: Drone) -> None:
+ self.drone = drone
+
+ self.params: dict = {}
+
+ self.valid_params = []
+ if self.drone.aircraft_type == VehicleType.FIXED_WING.value:
+ self.valid_params = COMMON_FAILSAFE_PARAMS + PLANE_FS_PARAMS
+ if self.drone.aircraft_type == VehicleType.MULTIROTOR.value:
+ self.valid_params = COMMON_FAILSAFE_PARAMS + COPTER_FS_PARAMS
+
+ self.getFailsafeParams()
+
+ def getConfig(self):
+ config = {}
+ for param in self.valid_params:
+ self.params[param] = self.drone.paramsController.getSingleParam(param)
+ config[param] = self.params[param].get("param_value", "UNKNOWN")
+
+ return config
+
+ def setFailsafeParam(self, param_id: str, value: Number) -> bool:
+ """
+ Sets a failsafe related parameter on the drone.
+ """
+ if param_id not in self.valid_params:
+ self.drone.logger.error(
+ f"Parameter {param_id} is not a valid failsafe parameter"
+ )
+ return False
+
+ param_type = self.params.get(param_id, {}).get("param_type", None)
+
+ return self.drone.paramsController.setParam(param_id, value, param_type)
+
+ def getFailsafeParams(self) -> None:
+ """
+ Gets the gripper related parameters from the drone.
+ """
+ self.drone.logger.debug("Fetching gripper parameters")
+ for param in self.valid_params:
+ self.params[param] = self.drone.paramsController.getSingleParam(param)
diff --git a/radio/app/controllers/flightModesController.py b/radio/app/controllers/flightModesController.py
index 3786bab14..7370d9fe2 100644
--- a/radio/app/controllers/flightModesController.py
+++ b/radio/app/controllers/flightModesController.py
@@ -5,7 +5,7 @@
from typing import TYPE_CHECKING, List, Union
import serial
-from app.customTypes import Number, Response
+from app.customTypes import Number, Response, VehicleType
from app.utils import commandAccepted, sendingCommandLock
from pymavlink import mavutil
@@ -87,7 +87,7 @@ def setFlightMode(self, mode_number: int, flight_mode: int) -> Response:
"message": f"Invalid flight mode number, must be between 1 and 6 inclusive, got {mode_number}.",
}
- if self.drone.aircraft_type == 1:
+ if self.drone.aircraft_type == VehicleType.FIXED_WING.value:
if (flight_mode < 0) or (flight_mode > 24):
return {
"success": False,
@@ -219,7 +219,7 @@ def setGuidedMode(self) -> Response:
mode = mavutil.mavlink.COPTER_MODE_GUIDED
- if self.drone.aircraft_type == 1:
+ if self.drone.aircraft_type == VehicleType.FIXED_WING.value:
mode = mavutil.mavlink.PLANE_MODE_GUIDED
return self.setCurrentFlightMode(mode)
diff --git a/radio/app/controllers/frameController.py b/radio/app/controllers/frameController.py
index 2a195a67e..81e86004c 100644
--- a/radio/app/controllers/frameController.py
+++ b/radio/app/controllers/frameController.py
@@ -2,7 +2,7 @@
from typing import TYPE_CHECKING, Union
-from app.customTypes import Number
+from app.customTypes import Number, VehicleType
if TYPE_CHECKING:
from app.drone import Drone
@@ -21,7 +21,7 @@ def __init__(self, drone: Drone) -> None:
self.frame_class: Number = 0
# Plane type doesn't have a frame type or class
- if self.drone.aircraft_type != 1:
+ if self.drone.aircraft_type != VehicleType.FIXED_WING.value:
self.getFrameType()
self.getFrameClass()
diff --git a/radio/app/controllers/navController.py b/radio/app/controllers/navController.py
index 1cfaa549f..acab63f9b 100644
--- a/radio/app/controllers/navController.py
+++ b/radio/app/controllers/navController.py
@@ -5,7 +5,7 @@
from typing import TYPE_CHECKING
import serial
-from app.customTypes import Response
+from app.customTypes import Response, VehicleType
from app.utils import commandAccepted, sendingCommandLock
from pymavlink import mavutil
@@ -27,7 +27,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
if (
- self.drone.aircraft_type == 1
+ self.drone.aircraft_type == VehicleType.FIXED_WING.value
): # Copter doesn't have loiter radius, only Plane
self.getLoiterRadius()
@@ -263,7 +263,7 @@ def reposition(self, lat: float, lon: float, alt: float) -> Response:
try:
# drone.aircraft_type == 1 for fixed wing. Check customTypes.py
- if self.drone.aircraft_type == 1:
+ if self.drone.aircraft_type == VehicleType.FIXED_WING.value:
with self.drone.sending_command_lock:
# https://mavlink.io/en/messages/common.html#MISSION_ITEM_INT
# https://ardupilot.org/dev/docs/plane-commands-in-guided-mode.html
diff --git a/radio/app/drone.py b/radio/app/drone.py
index 2a75a7bbd..04b8d67aa 100644
--- a/radio/app/drone.py
+++ b/radio/app/drone.py
@@ -18,6 +18,7 @@
from app.controllers.flightModesController import FlightModesController
from app.controllers.frameController import FrameController
from app.controllers.ftpController import FtpController
+from app.controllers.failsafeController import FailsafeController
from app.controllers.gripperController import GripperController
from app.controllers.missionController import MissionController
from app.controllers.motorTestController import MotorTestController
@@ -301,6 +302,7 @@ def setupControllers(self) -> None:
self.serialPortsController = SerialPortsController(self)
self.navController = NavController(self)
self.ftpController = FtpController(self)
+ self.failsafeController = FailsafeController(self)
def _emitConnectionStatus(
self, message: str, progress: float, sub_message: str = ""
diff --git a/radio/app/endpoints/__init__.py b/radio/app/endpoints/__init__.py
index 1a8e240f7..e7da65326 100644
--- a/radio/app/endpoints/__init__.py
+++ b/radio/app/endpoints/__init__.py
@@ -4,6 +4,7 @@
from . import autopilot as autopilot
from . import comPorts as comPorts
from . import connections as connections
+from . import failsafe as failsafe
from . import flightMode as flightMode
from . import frames as frames
from . import ftp as ftp
diff --git a/radio/app/endpoints/failsafe.py b/radio/app/endpoints/failsafe.py
new file mode 100644
index 000000000..647c06dd6
--- /dev/null
+++ b/radio/app/endpoints/failsafe.py
@@ -0,0 +1,79 @@
+import app.droneStatus as droneStatus
+from app import logger, socketio
+from app.utils import droneErrorCb
+from app.customTypes import SetConfigParam
+
+
+@socketio.on("get_failsafe_config")
+def getFailsafeConfig() -> None:
+ """
+ Sends the failsafe config to the frontend, only works when the config page is loaded.
+ """
+ if droneStatus.state != "config.failsafe":
+ socketio.emit(
+ "params_error",
+ {
+ "message": "You must be on the config screen to access the failsafe configuration."
+ },
+ )
+ logger.debug(f"Current state: {droneStatus.state}")
+ return
+
+ if not droneStatus.drone:
+ logger.warning("Attempted to get the failsafe config when drone is None.")
+ droneErrorCb(
+ "You must be connected to the drone to access the failsafe configuration."
+ )
+ return
+
+ failsafe_config = droneStatus.drone.failsafeController.getConfig()
+
+ socketio.emit(
+ "failsafe_config",
+ {"params": failsafe_config},
+ )
+
+
+@socketio.on("set_failsafe_config_param")
+def setFailsafeParam(data: SetConfigParam) -> None:
+ """
+ Sets a failsafe parameter based off data passed in, only works when the config page is loaded.
+ """
+ if droneStatus.state != "config.failsafe":
+ socketio.emit(
+ "params_error",
+ {
+ "message": "You must be on the config screen to access the failsafe configuration."
+ },
+ )
+ logger.debug(f"Current state: {droneStatus.state}")
+ return
+
+ if not droneStatus.drone:
+ logger.warning("Attempted to set a failsafe param when drone is None.")
+ droneErrorCb(
+ "You must be connected to the drone to access the failsafe configuration."
+ )
+ return
+
+ param_id = data.get("param_id", None)
+ value = data.get("value", None)
+
+ if param_id is None or value is None:
+ droneErrorCb("Param ID and value must be specified.")
+ return
+
+ success = droneStatus.drone.failsafeController.setFailsafeParam(param_id, value)
+ if success:
+ result = {
+ "success": True,
+ "message": f"Parameter {param_id} successfully set to {value}.",
+ "param_id": param_id,
+ "value": value,
+ }
+ else:
+ result = {
+ "success": False,
+ "message": f"Failed to set parameter {param_id} to {value}.",
+ }
+ socketio.emit("set_failsafe_param_result", result)
diff --git a/radio/tests/conftest.py b/radio/tests/conftest.py
index 7830e2309..1b7220561 100644
--- a/radio/tests/conftest.py
+++ b/radio/tests/conftest.py
@@ -3,6 +3,7 @@
from app.drone import Drone
from tests import socketio_client as _socketio_client
+from app.customTypes import VehicleType
def pytest_configure(config):
@@ -71,9 +72,9 @@ def check_aircraft_type(request):
aircraft_type = _droneStatus_module.drone.aircraft_type
# Skip if marked as plane_only but not a plane
- if "plane_only" in markers and aircraft_type != 1:
+ if "plane_only" in markers and aircraft_type != VehicleType.FIXED_WING.value:
pytest.skip(f"Test requires plane SITL (current type: {aircraft_type})")
# Skip if marked as copter_only but not a copter
- if "copter_only" in markers and aircraft_type != 2:
+ if "copter_only" in markers and aircraft_type != VehicleType.MULTIROTOR.value:
pytest.skip(f"Test requires copter SITL (current type: {aircraft_type})")
diff --git a/radio/tests/test_failsafe.py b/radio/tests/test_failsafe.py
new file mode 100644
index 000000000..20460057d
--- /dev/null
+++ b/radio/tests/test_failsafe.py
@@ -0,0 +1,115 @@
+import pytest
+from flask_socketio import SocketIOTestClient
+from pymavlink import mavutil
+
+from .helpers import NoDrone, send_and_receive, set_params
+from app.customTypes import VehicleType
+
+
+# Common parameters for Plane and Copter
+FAILSAFE_TEST_PARAMS = [
+ ("BATT_LOW_VOLT", 10.5, mavutil.mavlink.MAV_PARAM_TYPE_REAL32),
+ ("BATT_LOW_MAH", 1000, mavutil.mavlink.MAV_PARAM_TYPE_REAL32),
+ ("BATT_FS_LOW_ACT", 1, mavutil.mavlink.MAV_PARAM_TYPE_UINT8),
+ ("BATT_CRT_VOLT", 9.5, mavutil.mavlink.MAV_PARAM_TYPE_REAL32),
+ ("BATT_CRT_MAH", 500, mavutil.mavlink.MAV_PARAM_TYPE_REAL32),
+ ("BATT_FS_CRT_ACT", 2, mavutil.mavlink.MAV_PARAM_TYPE_UINT8),
+]
+
+
+@pytest.fixture(scope="session", autouse=True)
+def setup_failsafe_params():
+ """
+ Ensure required failsafe parameters exist before tests run.
+ """
+ set_params(FAILSAFE_TEST_PARAMS)
+
+
+@pytest.mark.copter_only
+def test_getFailsafeConfig(socketio_client: SocketIOTestClient, droneStatus):
+ # Failure: wrong state
+ droneStatus.state = "params"
+ assert send_and_receive("get_failsafe_config") == {
+ "message": "You must be on the config screen to access the failsafe configuration."
+ }
+
+ # Failure: no drone connected
+ droneStatus.state = "config.failsafe"
+ with NoDrone():
+ assert send_and_receive("get_failsafe_config") == {
+ "message": "You must be connected to the drone to access the failsafe configuration."
+ }
+
+ # Success: multirotor
+ droneStatus.state = "config.failsafe"
+ droneStatus.drone.aircraft_type = VehicleType.MULTIROTOR.value
+
+ result = send_and_receive("get_failsafe_config")
+ assert "params" in result
+ assert "BATT_LOW_VOLT" in result["params"]
+ assert "FS_THR_ENABLE" in result["params"]
+
+
+@pytest.mark.plane_only
+def test_getFailsafeConfig_fixedWing(socketio_client: SocketIOTestClient, droneStatus):
+ droneStatus.state = "config.failsafe"
+ droneStatus.drone.aircraft_type = VehicleType.FIXED_WING.value
+
+ result = send_and_receive("get_failsafe_config")
+ assert "params" in result
+ assert "THR_FS_VALUE" in result["params"]
+
+
+def test_setFailsafeParam(socketio_client: SocketIOTestClient, droneStatus):
+ # Failure: wrong state
+ droneStatus.state = "params"
+ assert send_and_receive(
+ "set_failsafe_config_param",
+ {"param_id": "BATT_LOW_VOLT", "value": 11.0},
+ ) == {
+ "message": "You must be on the config screen to access the failsafe configuration."
+ }
+
+ # Failure: no drone connected
+ droneStatus.state = "config.failsafe"
+ with NoDrone():
+ assert send_and_receive(
+ "set_failsafe_config_param",
+ {"param_id": "BATT_LOW_VOLT", "value": 11.0},
+ ) == {
+ "message": "You must be connected to the drone to access the failsafe configuration."
+ }
+
+ # Failure: missing param_id
+ assert send_and_receive(
+ "set_failsafe_config_param",
+ {"value": 11.0},
+ ) == {"message": "Param ID and value must be specified."}
+
+ # Failure: missing value
+ assert send_and_receive(
+ "set_failsafe_config_param",
+ {"param_id": "BATT_LOW_VOLT"},
+ ) == {"message": "Param ID and value must be specified."}
+
+ # Success
+ result = send_and_receive(
+ "set_failsafe_config_param",
+ {"param_id": "BATT_LOW_VOLT", "value": 11.0},
+ )
+
+ assert result == {
+ "success": True,
+ "message": "Parameter BATT_LOW_VOLT successfully set to 11.0.",
+ "param_id": "BATT_LOW_VOLT",
+ "value": 11.0,
+ }
+
+ # Failure: invalid param
+ result = send_and_receive(
+ "set_failsafe_config_param",
+ {"param_id": "INVALID_PARAM", "value": 1},
+ )
+
+ assert result["success"] is False
+ assert "Failed to set parameter" in result["message"]
diff --git a/radio/tests/test_flightMode.py b/radio/tests/test_flightMode.py
index 95128c6d3..1e560e9c0 100644
--- a/radio/tests/test_flightMode.py
+++ b/radio/tests/test_flightMode.py
@@ -131,6 +131,7 @@ def test_setFlightMode_wrongModeNumber(
}
+@pytest.mark.copter_only
def test_setFlightMode_successfullySet(
socketio_client: SocketIOTestClient, droneStatus
):