diff --git a/gcs/data/default_settings.json b/gcs/data/default_settings.json index fbc68b971..5b052fc0f 100644 --- a/gcs/data/default_settings.json +++ b/gcs/data/default_settings.json @@ -112,5 +112,95 @@ } ] } + }, + "Developer": { + "MAV_DATA_STREAM_RAW_SENSORS": { + "default": 1, + "type": "number", + "range": [ + 0, + 15 + ], + "display": "RAW_SENSORS", + "suffix": "Hz", + "group": "Data stream rates" + }, + "MAV_DATA_STREAM_EXTENDED_STATUS": { + "default": 1, + "type": "number", + "range": [ + 0, + 15 + ], + "display": "EXTENDED_STATUS", + "suffix": "Hz", + "group": "Data stream rates" + }, + "MAV_DATA_STREAM_RC_CHANNELS": { + "default": 1, + "type": "number", + "range": [ + 0, + 15 + ], + "display": "RC_CHANNELS", + "suffix": "Hz", + "group": "Data stream rates" + }, + "MAV_DATA_STREAM_RAW_CONTROLLER": { + "default": 1, + "type": "number", + "range": [ + 0, + 15 + ], + "display": "RAW_CONTROLLER", + "suffix": "Hz", + "group": "Data stream rates" + }, + "MAV_DATA_STREAM_POSITION": { + "default": 1, + "type": "number", + "range": [ + 0, + 15 + ], + "display": "POSITION", + "suffix": "Hz", + "group": "Data stream rates" + }, + "MAV_DATA_STREAM_EXTRA1": { + "default": 4, + "type": "number", + "range": [ + 0, + 15 + ], + "display": "EXTRA1", + "suffix": "Hz", + "group": "Data stream rates" + }, + "MAV_DATA_STREAM_EXTRA2": { + "default": 3, + "type": "number", + "range": [ + 0, + 15 + ], + "display": "EXTRA2", + "suffix": "Hz", + "group": "Data stream rates" + }, + "MAV_DATA_STREAM_EXTRA3": { + "default": 1, + "type": "number", + "range": [ + 0, + 15 + ], + "display": "EXTRA3", + "suffix": "Hz", + "group": "Data stream rates" + } } -} +} \ No newline at end of file diff --git a/gcs/src/components/settingsModal.jsx b/gcs/src/components/settingsModal.jsx index d80c0c342..5bdea40fd 100644 --- a/gcs/src/components/settingsModal.jsx +++ b/gcs/src/components/settingsModal.jsx @@ -24,13 +24,16 @@ import { } from "@tabler/icons-react" import { Octokit } from "octokit" import { memo, useEffect, useState } from "react" +import { useDispatch } from "react-redux" import semverGt from "semver/functions/gt" import DefaultSettings from "../../data/default_settings.json" +import { DATA_STREAM_MAP } from "../helpers/mavlinkConstants" import { closeLoadingNotification, redColor, showLoadingNotification, } from "../helpers/notification" +import { emitSetStreamRates } from "../redux/slices/droneConnectionSlice" const octokit = new Octokit({}) @@ -197,6 +200,37 @@ function ReleaseCheckRow() { ) } +function SetRatesRow() { + const { getSetting } = useSettings() + const dispatch = useDispatch() + const developerDefaults = DefaultSettings?.Developer ?? {} + + const onClick = () => { + for (const [name, value] of Object.entries(DATA_STREAM_MAP)) { + if (!Object.hasOwn(developerDefaults, name)) continue + + const rateSetting = getSetting(`Developer.${name}`) + const rate = Number(rateSetting) + if (!Number.isFinite(rate)) continue + if (rate < 0 || rate > 15) continue + dispatch(emitSetStreamRates({ stream: value, rate: rate })) + } + } + + return ( +
+

+ Note: Data stream rates here apply to the dashboard only. +

+
+ +
+
+ ) +} + function OptionSetting({ settingName, options }) { const { getSetting, setSetting } = useSettings() return ( @@ -741,6 +775,13 @@ function SettingsModal() { > {settingTabs.map((t) => { + // Only show developer tag when developer features are on + if ( + !getSetting("General.experimentalDeveloperFeatures") && + t === "Developer" + ) { + return <> + } return ( {t} @@ -797,6 +838,11 @@ function SettingsModal() { )} + {tab === "Developer" && ( + <> + + + )} ) })} diff --git a/gcs/src/helpers/mavlinkConstants.js b/gcs/src/helpers/mavlinkConstants.js index 8ac246c9e..b39fab30b 100644 --- a/gcs/src/helpers/mavlinkConstants.js +++ b/gcs/src/helpers/mavlinkConstants.js @@ -557,3 +557,15 @@ export const EXCLUDE_PARAMS_LOAD = [ "STAT_RESET", "STAT_RUNTIME", ] + +export const DATA_STREAM_MAP = { + MAV_DATA_STREAM_ALL: 0, + MAV_DATA_STREAM_RAW_SENSORS: 1, + MAV_DATA_STREAM_EXTENDED_STATUS: 2, + MAV_DATA_STREAM_RC_CHANNELS: 3, + MAV_DATA_STREAM_RAW_CONTROLLER: 4, + MAV_DATA_STREAM_POSITION: 6, + MAV_DATA_STREAM_EXTRA1: 10, + MAV_DATA_STREAM_EXTRA2: 11, + MAV_DATA_STREAM_EXTRA3: 12, +} diff --git a/gcs/src/helpers/settingsProvider.jsx b/gcs/src/helpers/settingsProvider.jsx index c0bf3b35a..b32226e77 100644 --- a/gcs/src/helpers/settingsProvider.jsx +++ b/gcs/src/helpers/settingsProvider.jsx @@ -56,10 +56,15 @@ export const SettingsProvider = ({ children }) => { const getSetting = (setting) => { const userSetting = getSettingFromSettings(setting, settings.settings) + const defaultSetting = getSettingFromSettings(setting, DefaultSettings) - return userSetting === null - ? getSettingFromSettings(setting, DefaultSettings).default - : userSetting + if (userSetting !== null) return userSetting + + if (defaultSetting === null || defaultSetting === undefined) { + return null + } + + return defaultSetting.default } return ( diff --git a/gcs/src/redux/middleware/emitters.js b/gcs/src/redux/middleware/emitters.js index 70560892e..036d94c23 100644 --- a/gcs/src/redux/middleware/emitters.js +++ b/gcs/src/redux/middleware/emitters.js @@ -39,6 +39,7 @@ import { emitSetCurrentFlightMode, emitSetLoiterRadius, emitSetState, + emitSetStreamRates, emitStartForwarding, emitStopForwarding, emitTakeoff, @@ -226,6 +227,14 @@ export function handleEmitters(socket, store, action) { newFlightMode: action.payload.newFlightMode, }), }, + { + emitter: emitSetStreamRates, + callback: () => + socket.socket.emit("set_stream_rate", { + stream: action.payload.stream, + rate: action.payload.rate, + }), + }, { emitter: emitStartSimulation, diff --git a/gcs/src/redux/slices/droneConnectionSlice.js b/gcs/src/redux/slices/droneConnectionSlice.js index c7583f47d..15401d189 100644 --- a/gcs/src/redux/slices/droneConnectionSlice.js +++ b/gcs/src/redux/slices/droneConnectionSlice.js @@ -205,6 +205,7 @@ const droneConnectionSlice = createSlice({ emitTakeoff: () => {}, emitLand: () => {}, emitSetCurrentFlightMode: () => {}, + emitSetStreamRates: () => {}, }, selectors: { selectConnecting: (state) => state.connecting, @@ -285,6 +286,7 @@ export const { emitTakeoff, emitLand, emitSetCurrentFlightMode, + emitSetStreamRates, } = droneConnectionSlice.actions export const { diff --git a/radio/app/drone.py b/radio/app/drone.py index 2a75a7bbd..5a8573a41 100644 --- a/radio/app/drone.py +++ b/radio/app/drone.py @@ -46,6 +46,7 @@ mavutil.mavlink.MAV_DATA_STREAM_RAW_SENSORS: 1, mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS: 1, mavutil.mavlink.MAV_DATA_STREAM_RC_CHANNELS: 1, + mavutil.mavlink.MAV_DATA_STREAM_RAW_CONTROLLER: 1, mavutil.mavlink.MAV_DATA_STREAM_POSITION: 1, mavutil.mavlink.MAV_DATA_STREAM_EXTRA1: 4, mavutil.mavlink.MAV_DATA_STREAM_EXTRA2: 3, @@ -520,7 +521,13 @@ def setupSingleDataStream(self, stream: int) -> None: Args: stream (int): The data stream to set up """ - self.sendDataStreamRequestMessage(stream, DATASTREAM_RATES[stream]) + rate = DATASTREAM_RATES.get(stream) + if rate is None: + self.logger.warning( + f"No configured rate for stream {stream}; skipping setup request" + ) + return + self.sendDataStreamRequestMessage(stream, rate) @sendingCommandLock def sendDataStreamRequestMessage(self, stream: int, rate: int) -> None: @@ -888,7 +895,15 @@ def getLinkDebugData(self) -> None: def sendHeartbeatMessage(self) -> None: """Sends a heartbeat message to the drone every second.""" + heartbeat_interval_secs = 1.0 + next_heartbeat_time = time.monotonic() + while self.is_active.is_set(): + now = time.monotonic() + sleep_time = next_heartbeat_time - now + if sleep_time > 0: + time.sleep(sleep_time) + try: self.master.mav.heartbeat_send( mavutil.mavlink.MAV_TYPE_GCS, @@ -899,7 +914,11 @@ def sendHeartbeatMessage(self) -> None: ) except Exception as e: self.logger.error(f"Failed to send heartbeat: {e}", exc_info=True) - time.sleep(1) + + # Keep a stable 1Hz cadence and recover if we fall behind. + next_heartbeat_time += heartbeat_interval_secs + if next_heartbeat_time < time.monotonic(): + next_heartbeat_time = time.monotonic() + heartbeat_interval_secs def startThread(self) -> None: """Starts the listener and sender threads.""" diff --git a/radio/app/endpoints/states.py b/radio/app/endpoints/states.py index cc7b2a062..7aa0b5bd5 100644 --- a/radio/app/endpoints/states.py +++ b/radio/app/endpoints/states.py @@ -1,8 +1,11 @@ +import copy + from pymavlink import mavutil from typing_extensions import TypedDict import app.droneStatus as droneStatus from app import logger, socketio +from app.drone import DATASTREAM_RATES from app.utils import ( missingParameterError, sendMessage, @@ -13,6 +16,11 @@ class SetStateType(TypedDict): state: str +class SetStreamRateType(TypedDict): + stream: int + rate: int + + GLOBAL_MESSAGE_LISTENERS = ["HEARTBEAT", "STATUSTEXT", "GLOBAL_POSITION_INT", "VFR_HUD"] STATES_MESSAGE_LISTENERS = { @@ -43,6 +51,17 @@ class SetStateType(TypedDict): } +DASHBOARD_STREAM_RATES = copy.deepcopy(DATASTREAM_RATES) + + +def apply_dashboard_stream_rates() -> None: + if not droneStatus.drone: + return + + for stream, rate in DASHBOARD_STREAM_RATES.items(): + droneStatus.drone.sendDataStreamRequestMessage(stream, rate) + + @socketio.on("set_state") def set_state(data: SetStateType) -> None: """ @@ -68,29 +87,37 @@ def set_state(data: SetStateType) -> None: # Remove all existing message listeners droneStatus.drone.clearAllMessageListeners() - # Always setup position stream to get GLOBAL_POSITION_INT messages - droneStatus.drone.setupSingleDataStream(mavutil.mavlink.MAV_DATA_STREAM_POSITION) + # Always setup position stream to get GLOBAL_POSITION_INT messages on + # non-dashboard pages + if droneStatus.state != "dashboard": + droneStatus.drone.sendDataStreamRequestMessage( + mavutil.mavlink.MAV_DATA_STREAM_POSITION, 1 + ) for message in GLOBAL_MESSAGE_LISTENERS: droneStatus.drone.addMessageListener(message, sendMessage) if droneStatus.state == "dashboard": - droneStatus.drone.setupDataStreams() + apply_dashboard_stream_rates() for message in STATES_MESSAGE_LISTENERS["dashboard"]: droneStatus.drone.addMessageListener(message, sendMessage) elif droneStatus.state == "missions": - droneStatus.drone.setupSingleDataStream( - mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS + droneStatus.drone.sendDataStreamRequestMessage( + mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS, 1 ) for message in STATES_MESSAGE_LISTENERS["missions"]: droneStatus.drone.addMessageListener(message, sendMessage) elif droneStatus.state == "graphs": - droneStatus.drone.setupSingleDataStream( - mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS + droneStatus.drone.sendDataStreamRequestMessage( + mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS, 1 + ) + droneStatus.drone.sendDataStreamRequestMessage( + mavutil.mavlink.MAV_DATA_STREAM_EXTRA1, 4 + ) + droneStatus.drone.sendDataStreamRequestMessage( + mavutil.mavlink.MAV_DATA_STREAM_EXTRA2, 3 ) - droneStatus.drone.setupSingleDataStream(mavutil.mavlink.MAV_DATA_STREAM_EXTRA1) - droneStatus.drone.setupSingleDataStream(mavutil.mavlink.MAV_DATA_STREAM_EXTRA2) for message in STATES_MESSAGE_LISTENERS["graphs"]: droneStatus.drone.addMessageListener(message, sendMessage) @@ -115,3 +142,33 @@ def set_state(data: SetStateType) -> None: for message in STATES_MESSAGE_LISTENERS["config.servo"]: droneStatus.drone.addMessageListener(message, sendMessage) + + +@socketio.on("set_stream_rate") +def set_stream_rate(data: SetStreamRateType): + if not droneStatus.drone: + return + + if (rate := data.get("rate", None)) is None: + return missingParameterError("set_stream_rate", "rate") + + if (stream := data.get("stream", None)) is None: + return missingParameterError("set_stream_rate", "stream") + + try: + rate = int(rate) + stream = int(stream) + except (ValueError, TypeError): + logger.error("Invalid set_stream_rate payload types") + return + + if rate > 15 or rate < 0: + logger.error("Cannot set data stream rate outside of range [0, 15]") + return + + DASHBOARD_STREAM_RATES[stream] = rate + + # Dashboard-only behavior: only apply immediately while dashboard is active. + if droneStatus.state == "dashboard": + logger.info(f"Setting dashboard data stream {stream} rate to {rate}") + droneStatus.drone.sendDataStreamRequestMessage(stream, rate)