Rc calib

ardupilot_methodic_configurator/backend_data_telemetry.py

@@ -11,10 +11,11 @@
 from collections.abc import Callable
 from logging import debug as logging_debug
 from time import monotonic
-from typing import TYPE_CHECKING, Any, cast
+from typing import TYPE_CHECKING, Any
 
 from serial import SerialException
 
+from ardupilot_methodic_configurator.backend_mavlink_telemetry import MavlinkTelemetryBackend
 from ardupilot_methodic_configurator.data_model_data_telemetry import (
     MessageKey,
     MessageSample,
@@ -28,25 +29,21 @@
 _MAX_MESSAGE_TYPES = 512
 
 
-class DataTelemetryBackend:
+class DataTelemetryBackend(MavlinkTelemetryBackend):
     """Observe parsed packets without replacing hooks or reading from a second connection."""
 
     def __init__(self, flight_controller: "FlightController", clock: Callable[[], float] = monotonic) -> None:
-        self.flight_controller = flight_controller
-        self._clock = clock
-        self._master: Any = None
+        super().__init__(flight_controller, clock)
         self._messages: dict[MessageKey, MessageSample] = {}
         self._radio: RadioSample | None = None
 
     def close(self) -> None:
         """Detach only our observer and discard data at lifecycle boundaries."""
-        if self._master is not None and self._capture in self._master.message_hooks:
-            self._master.message_hooks.remove(self._capture)
-        self._master = None
+        super().close()
         self._messages.clear()
         self._radio = None
 
-    def _capture(self, _master: object, message: Any) -> None:  # noqa: ANN401
+    def _capture(self, _master: object, message: Any, /) -> None:  # noqa: ANN401
         """Count all sources independently, including modem-injected RADIO messages."""
         name = str(message.get_type())
         if name == "BAD_DATA":
@@ -83,20 +80,12 @@ def _radio_value(value: int, maximum: int) -> int | None:
 
     def poll(self) -> TelemetrySample:
         """Enable connection-owned heartbeats and drain at most 200 packets/10 ms."""
-        master = cast("Any", self.flight_controller.master)
-        if master is not self._master:
-            self.close()
-            self._master = master
-            if master is not None:
-                master.message_hooks.append(self._capture)
+        master = self._sync_connection()
         if master is None:
             return TelemetrySample(None, self._clock())
         try:
             self.flight_controller.start_gcs_heartbeat()
-            deadline = self._clock() + 0.01
-            for _index in range(200):
-                if self._clock() >= deadline or master.recv_match(blocking=False) is None:
-                    break
+            self._drain_messages()
             return TelemetrySample(
                 connection=master,
                 time=self._clock(),

ardupilot_methodic_configurator/backend_esc_telemetry.py

@@ -11,11 +11,12 @@
 from collections.abc import Callable
 from logging import debug as logging_debug
 from time import monotonic
-from typing import TYPE_CHECKING, Any, cast
+from typing import TYPE_CHECKING, Any
 
 from pymavlink.dialects.v20 import ardupilotmega as mavlink
 from serial import SerialException
 
+from ardupilot_methodic_configurator.backend_mavlink_telemetry import MavlinkTelemetryBackend
 from ardupilot_methodic_configurator.data_model_esc_telemetry import (
     ESC_FIELDS,
     MAX_ESCS,
@@ -27,23 +28,22 @@
     from ardupilot_methodic_configurator.backend_flightcontroller import FlightController
 
 ESC_MESSAGE_OFFSETS = {f"ESC_TELEMETRY_{start}_TO_{start + 3}": start - 1 for start in range(1, MAX_ESCS + 1, 4)}
+_UNKNOWN_ESC_MESSAGES = {f"UNKNOWN_{getattr(mavlink, f'MAVLINK_MSG_ID_{name}')}" for name in ESC_MESSAGE_OFFSETS}
+_UNUSED_REQUEST_PARAMETERS = (0, 0, 0, 0, 0, 0)
 
 
-class EscTelemetryBackend:
+class EscTelemetryBackend(MavlinkTelemetryBackend):
     """Observe ESC packets without opening another connection or changing stream rates."""
 
     def __init__(self, flight_controller: "FlightController", clock: Callable[[], float] = monotonic) -> None:
-        self.flight_controller = flight_controller
-        self._clock = clock
-        self._master: Any = None
+        super().__init__(flight_controller, clock)
         self._last_request: float | None = None
         self._readings: dict[int, EscTelemetryReading] = {}
+        self._decoder = mavlink.MAVLink(None)
 
     def close(self) -> None:
         """Remove only this observer and reset request timing."""
-        if self._master is not None and self._capture in self._master.message_hooks:
-            self._master.message_hooks.remove(self._capture)
-        self._master = None
+        super().close()
         self._last_request = None
         self._readings.clear()
 
@@ -55,7 +55,18 @@ def _target_ids(self, master: Any) -> tuple[int, int]:  # noqa: ANN401
             int(info.component_id) if info.component_id else int(master.target_component),
         )
 
-    def _capture(self, master: Any, message: Any) -> None:  # noqa: ANN401
+    def _capture(self, master: Any, message: Any, /) -> None:  # noqa: ANN401
+        if message.get_type() in _UNKNOWN_ESC_MESSAGES:
+            # A connection that initially detected MAVLink 1 can still receive
+            # MAVLink 2 frames, but its v1 dialect does not know ESC message IDs.
+            # Decode and validate the original packet without replacing the
+            # shared parser or disturbing other connection consumers.
+            try:
+                self._decoder.signing = master.mav.signing
+                message = self._decoder.decode(bytearray(message.get_msgbuf()))
+            except mavlink.MAVError as exc:
+                logging_debug("Invalid ESC telemetry packet: %s", exc)
+                return
         offset = ESC_MESSAGE_OFFSETS.get(str(message.get_type()))
         if offset is None:
             return
@@ -73,39 +84,28 @@ def _capture(self, master: Any, message: Any) -> None:  # noqa: ANN401
 
     def poll(self) -> EscTelemetrySnapshot:
         """Request all groups once per second and drain at most 200 packets/10 ms."""
-        # This observer lifecycle and MAVLink command layout intentionally match other backends.
-        # pylint: disable=duplicate-code
-        master = cast("Any", self.flight_controller.master)
-        if master is not self._master:
-            self.close()
-            self._master = master
-            if master is not None:
-                master.message_hooks.append(self._capture)
+        master = self._sync_connection()
         if master is None:
             return EscTelemetrySnapshot(None, self._clock())
         try:
             now = self._clock()
             if self._last_request is None or now - self._last_request >= 1.0:
                 # ArduPilot maps this first group to its shared sender for ALL ESC groups.
                 system_id, component_id = self._target_ids(master)
-                master.mav.command_long_send(
-                    system_id,
-                    component_id,
-                    mavlink.MAV_CMD_REQUEST_MESSAGE,
-                    0,
-                    mavlink.MAVLINK_MSG_ID_ESC_TELEMETRY_1_TO_4,
-                    0,
-                    0,
-                    0,
-                    0,
-                    0,
-                    0,
+                # ESC_TELEMETRY_* IDs require MAVLink 2. Encode this request
+                # as v2 even if the link initially negotiated MAVLink 1.
+                master.mav.send(
+                    mavlink.MAVLink_command_long_message(
+                        system_id,
+                        component_id,
+                        mavlink.MAV_CMD_REQUEST_MESSAGE,
+                        0,
+                        mavlink.MAVLINK_MSG_ID_ESC_TELEMETRY_1_TO_4,
+                        *_UNUSED_REQUEST_PARAMETERS,
+                    )
                 )
                 self._last_request = now
-            deadline = self._clock() + 0.01
-            for _index in range(200):
-                if self._clock() >= deadline or master.recv_match(blocking=False) is None:
-                    break
+            self._drain_messages()
             readings, self._readings = self._readings, {}
             return EscTelemetrySnapshot(master, self._clock(), readings)
         except (OSError, SerialException, AttributeError, TypeError, ValueError) as exc:

ardupilot_methodic_configurator/backend_mavlink_telemetry.py

@@ -0,0 +1,53 @@
+"""
+Shared observer lifecycle and bounded reads for MAVLink telemetry backends.
+
+This file is part of ArduPilot Methodic Configurator. https://github.com/ArduPilot/MethodicConfigurator
+
+SPDX-FileCopyrightText: 2026 Amilcar do Carmo Lucas <amilcar.lucas@iav.de>
+
+SPDX-License-Identifier: GPL-3.0-or-later
+"""
+
+from abc import ABC, abstractmethod
+from collections.abc import Callable
+from time import monotonic
+from typing import TYPE_CHECKING, Any, cast
+
+if TYPE_CHECKING:
+    from ardupilot_methodic_configurator.backend_flightcontroller import FlightController
+
+
+class MavlinkTelemetryBackend(ABC):  # pylint: disable=too-few-public-methods
+    """Attach one observer to the shared connection and keep each poll bounded."""
+
+    def __init__(self, flight_controller: "FlightController", clock: Callable[[], float] = monotonic) -> None:
+        self.flight_controller = flight_controller
+        self._clock = clock
+        self._master: Any = None
+
+    def close(self) -> None:
+        """Detach only this backend's hook, preserving other connection consumers."""
+        if self._master is not None and self._capture in self._master.message_hooks:
+            self._master.message_hooks.remove(self._capture)
+        self._master = None
+
+    def _sync_connection(self) -> Any:  # noqa: ANN401
+        """Reset observations through close() before attaching to a replacement connection."""
+        master = cast("Any", self.flight_controller.master)
+        if master is not self._master:
+            self.close()
+            self._master = master
+            if master is not None:
+                master.message_hooks.append(self._capture)
+        return master
+
+    def _drain_messages(self) -> None:
+        """Read at most 200 packets or 10 ms, counting unrelated traffic toward both limits."""
+        deadline = self._clock() + 0.01
+        for _index in range(200):
+            if self._clock() >= deadline or self._master.recv_match(blocking=False) is None:
+                break
+
+    @abstractmethod
+    def _capture(self, master: Any, message: Any, /) -> None:  # noqa: ANN401
+        """Handle one parsed message according to the consumer's telemetry policy."""

ardupilot_methodic_configurator/data_model_esc_telemetry.py

@@ -46,7 +46,7 @@ def __init__(self, flight_controller: "FlightController") -> None:
         self.flight_controller = flight_controller
         self._connection: object | None = None
         self._telemetry: dict[int, tuple[float, EscTelemetryReading]] = {}
-        self._rpm: dict[int, tuple[float, int]] = {}
+        self._rpm: dict[int, tuple[float, int | str]] = {}
 
     @property
     def visible_esc_indices(self) -> tuple[int, ...]:
@@ -71,15 +71,35 @@ def row_labels() -> tuple[str, ...]:
             _("Telemetry packets"),
         )
 
-    def update(self, snapshot: EscTelemetrySnapshot) -> tuple[tuple[str, ...], ...]:
+    def _configured_bdshot_output(self, index: int) -> bool:
+        """Identify configured outputs, accounting for the ESC MAVLink offset."""
+        params = self.flight_controller.fc_parameters
+        if not isinstance(params, dict):
+            return False
+        try:
+            mask = int(params.get("SERVO_BLH_BDMASK", 0))
+            offset = int(params.get("ESC_TLM_MAV_OFS", 0))
+        except (TypeError, ValueError, OverflowError):
+            return False
+        output = index + offset
+        return 0 <= offset < MAX_ESCS and 0 <= output < MAX_ESCS and bool(mask & (1 << output))
+
+    def update(self, snapshot: EscTelemetrySnapshot) -> tuple[tuple[str, ...], ...]:  # pylint: disable=too-many-branches
         """Return measurement rows; zeros remain valid for an ESC reporting telemetry."""
         if snapshot.connection is not self._connection or snapshot.error or snapshot.connection is None:
             self.clear_live_data()
             self._connection = snapshot.connection
         if not snapshot.error and snapshot.connection is not None:
             for index, reading in snapshot.readings.items():
-                if not 0 <= index < MAX_ESCS or len(reading.values) != len(ESC_FIELDS) or not any(reading.values):
-                    continue  # ArduPilot pads unused members of a four-ESC group with zeros.
+                if not 0 <= index < MAX_ESCS or len(reading.values) != len(ESC_FIELDS):
+                    continue
+                if not any(reading.values):
+                    # The wire format cannot distinguish a stopped BDShot motor from
+                    # an unavailable member of an otherwise active group.
+                    if self._configured_bdshot_output(index):
+                        received_at = snapshot.time if reading.received_at is None else reading.received_at
+                        self._rpm[index] = (received_at, "0*")
+                    continue
                 old = self._telemetry.get(index)
                 count = reading.values[-1]
                 received_at = snapshot.time if reading.received_at is None else reading.received_at

ardupilot_methodic_configurator/frontend_tkinter_esc_telemetry.py

@@ -48,7 +48,9 @@ def __init__(self, parent: tk.Frame | ttk.Frame, model: EscTelemetryDataModel, b
             self.table,
             _(
                 "Live ESC readings. Missing or stale data is shown as '-'. "
-                "ESC numbers follow the flight controller's ESC telemetry offset."
+                "ESC numbers follow the flight controller's ESC telemetry offset. "
+                "For configured bidirectional DShot outputs, 0* means the group reported zero RPM; "
+                "the individual output may instead be unavailable."
             ),
         )
         self.scroll_tooltip = Tooltip(scrollbar, _("Scroll to view additional ESC columns."))

ardupilot_methodic_configurator/plugins/backend_rc_telemetry.py

@@ -0,0 +1,86 @@
+"""
+Shared MAVLink telemetry transport for RC monitoring and calibration.
+
+This file is part of ArduPilot Methodic Configurator. https://github.com/ArduPilot/MethodicConfigurator
+
+SPDX-FileCopyrightText: 2026 Amilcar do Carmo Lucas <amilcar.lucas@iav.de>
+
+SPDX-License-Identifier: GPL-3.0-or-later
+"""
+
+from collections.abc import Iterator
+from logging import debug as logging_debug
+from typing import TYPE_CHECKING, Any, cast
+
+if TYPE_CHECKING:
+    from ardupilot_methodic_configurator.backend_flightcontroller import FlightController
+
+_MAX_MESSAGES_PER_POLL = 50
+
+
+def rc_channel_values(message: Any, channel_count: int) -> list[int]:  # noqa: ANN401
+    """Decode only advertised RC channels, capped at the consumer's display limit."""
+    count = max(0, min(int(message.chancount), channel_count))
+    return [int(getattr(message, f"chan{channel}_raw")) for channel in range(1, count + 1)]
+
+
+class RCTelemetryBackend:
+    """Own one monitor's stream lifecycle and bounded reads from the shared MAVLink link."""
+
+    def __init__(self, flight_controller: "FlightController") -> None:
+        self.flight_controller = flight_controller
+        self.connection: object | None = None
+        self._stream_requested = False
+
+    def sync_connection(self) -> bool:
+        """Reset stream ownership on a new connection; tell consumers to clear cached readings."""
+        master = self.flight_controller.master
+        if master is self.connection:
+            return False
+        self.connection = master
+        self._stream_requested = False
+        return True
+
+    def stop_monitoring(self) -> None:
+        """Restore defaults once, only on the connection whose intervals we requested."""
+        if self._stream_requested and self.flight_controller.master is self.connection:
+            success, error = self.flight_controller.request_rc_receiver_messages(0)
+            if not success:
+                logging_debug("Could not restore RC receiver telemetry intervals: %s", error)
+        self._stream_requested = False
+
+    def get_rc_telemetry(self) -> Iterator[Any]:
+        """Yield packets from the selected controller in a bounded, nonblocking batch."""
+        self.sync_connection()
+        master = cast("Any", self.connection)
+        if master is None:
+            return
+        if not self._stream_requested:
+            # Older firmware may reject the interval request while already streaming data.
+            self._stream_requested = True
+            success, error = self.flight_controller.request_rc_receiver_messages()
+            if not success:
+                logging_debug("Could not request RC receiver telemetry intervals: %s", error)
+
+        for _index in range(_MAX_MESSAGES_PER_POLL):
+            # A type filter can consume an unbounded unrelated backlog inside pymavlink.
+            message = master.recv_match(blocking=False)
+            if message is None:
+                break
+            if self.is_selected_source(message):
+                yield message
+
+    def is_selected_source(self, message: Any) -> bool:  # noqa: ANN401
+        """Identify RC monitoring and calibration packets from the selected flight controller."""
+        master = cast("Any", self.connection)
+        if master is None:
+            return False
+        info = self.flight_controller.info
+        system_id, component_id = info.system_id, info.component_id
+        target_system = int(system_id) if isinstance(system_id, str) and system_id else master.target_system
+        target_component = int(component_id) if isinstance(component_id, str) and component_id else master.target_component
+        if not isinstance(target_system, int) or target_system <= 0 or message.get_srcSystem() != target_system:
+            return False
+        return not (
+            isinstance(target_component, int) and target_component > 0 and message.get_srcComponent() != target_component
+        )