Fix bug with datastreams not getting set, fix bug with heartbeat thread not sending at 1hz

Author: 1Blademaster

gcs/src/components/settingsModal.jsx

@@ -24,16 +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 { useDispatch } from "react-redux"
 import { emitSetStreamRates } from "../redux/slices/droneConnectionSlice"
-import { DATA_STREAM_MAP } from "../helpers/mavlinkConstants"
 
 const octokit = new Octokit({})
 
@@ -203,10 +203,16 @@ 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)) {
-      let rate = getSetting(`Developer.${name}`)
+      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 }))
     }
   }

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 (

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."""

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,
@@ -48,6 +51,17 @@ class SetStreamRateType(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:
     """
@@ -73,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)
@@ -128,11 +150,25 @@ def set_stream_rate(data: SetStreamRateType):
         return
 
     if (rate := data.get("rate", None)) is None:
-        return missingParameterError("set_state", "state")
+        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
 
-    logger.info(f"Setting data stream rate {data['rate']}")
-    droneStatus.drone.sendDataStreamRequestMessage(data["stream"], rate)
+    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)