Improve Dual RX mixed SITL test

Author: xznhj8129

src/test/dualrx/dualrx_mixed_sitl_test.py

@@ -5,11 +5,13 @@
 
 import argparse
 from pathlib import Path
+import shutil
+import socket
+import struct
 import tempfile
 import threading
 import time
-
-from pymavlink import mavutil
+import traceback
 
 from dualrx_sitl_test import (
     BOX_BEEPER,
@@ -40,6 +42,13 @@
 UART_RX2 = 4
 BOX_MSP_RC_OVERRIDE = 50
 
+MAVLINK_V1_MAGIC = 0xFE
+MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE = 70
+MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC = 124
+MAVLINK_TARGET_SYSTEM = 1
+MAVLINK_TARGET_COMPONENT = 1
+MAVLINK_SOURCE_COMPONENT = 191
+
 
 class PeriodicIngress:
     def __init__(self, name: str, channels: list[int], period_s: float = 0.02) -> None:
@@ -50,7 +59,7 @@ def __init__(self, name: str, channels: list[int], period_s: float = 0.02) -> No
         self._lock = threading.Lock()
         self._stop = threading.Event()
         self._thread: threading.Thread | None = None
-        self.error: BaseException | None = None
+        self.error: Exception | None = None
 
     def start(self) -> None:
         self._thread = threading.Thread(target=self._run_guarded, name=self.name, daemon=True)
@@ -69,12 +78,12 @@ def set_mode(self, mode: str) -> None:
 
     def check(self) -> None:
         if self.error is not None:
-            raise TestFailure(f"{self.name} background thread failed: {self.error}")
+            raise TestFailure(f"{self.name} background thread failed: {type(self.error).__name__}: {self.error}") from self.error
 
     def _run_guarded(self) -> None:
         try:
             self._run()
-        except BaseException as exc:
+        except Exception as exc:
             if not self._stop.is_set():
                 self.error = exc
                 self._stop.set()
@@ -83,21 +92,64 @@ def _run(self) -> None:
         raise NotImplementedError
 
 
+def mavlink_x25_crc(data: bytes, crc: int = 0xFFFF) -> int:
+    for value in data:
+        tmp = value ^ (crc & 0xFF)
+        tmp ^= (tmp << 4) & 0xFF
+        crc = ((crc >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4)) & 0xFFFF
+    return crc
+
+
+def mavlink_v1_rc_override(seq: int, source_system: int, channels: list[int]) -> bytes:
+    if len(channels) < 8:
+        raise ValueError("RC_CHANNELS_OVERRIDE test frame needs at least 8 channels")
+    payload = struct.pack(
+        "<8HBB",
+        *[int(value) & 0xFFFF for value in channels[:8]],
+        MAVLINK_TARGET_SYSTEM,
+        MAVLINK_TARGET_COMPONENT,
+    )
+    header = bytes([
+        len(payload),
+        seq & 0xFF,
+        source_system & 0xFF,
+        MAVLINK_SOURCE_COMPONENT,
+        MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE,
+    ])
+    crc = mavlink_x25_crc(header + payload)
+    crc = mavlink_x25_crc(bytes([MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC]), crc)
+    return bytes([MAVLINK_V1_MAGIC]) + header + payload + struct.pack("<H", crc)
+
+
 class MavlinkIngress(PeriodicIngress):
     def __init__(self, name: str, host: str, port: int, channels: list[int], source_system: int) -> None:
         super().__init__(name, channels)
-        self.connection = mavutil.mavlink_connection(
-            f"tcp:{host}:{port}",
-            source_system=source_system,
-            source_component=191,
-            autoreconnect=False,
-        )
+        self.source_system = source_system
+        self.sequence = 0
+        self.sock: socket.socket | None = None
+        deadline = time.monotonic() + 5.0
+        last_error: OSError | None = None
+        while time.monotonic() < deadline:
+            try:
+                self.sock = socket.create_connection((host, port), timeout=1.0)
+                self.sock.settimeout(None)
+                break
+            except OSError as exc:
+                last_error = exc
+                time.sleep(0.05)
+        if self.sock is None:
+            raise TimeoutError(f"{name}: could not connect to {host}:{port}: {last_error}")
 
     def close(self) -> None:
         super().close()
-        self.connection.close()
+        if self.sock is not None:
+            try:
+                self.sock.close()
+            except OSError:
+                pass
 
     def _run(self) -> None:
+        assert self.sock is not None
         next_send = time.monotonic()
         while not self._stop.is_set():
             now = time.monotonic()
@@ -107,11 +159,12 @@ def _run(self) -> None:
             if mode != "off" and now >= next_send:
                 if mode == "partial":
                     channels[:4] = [0xFFFF] * 4
-                self.connection.mav.rc_channels_override_send(1, 1, *channels[:8])
+                frame = mavlink_v1_rc_override(self.sequence, self.source_system, channels)
+                self.sock.sendall(frame)
+                self.sequence = (self.sequence + 1) & 0xFF
                 next_send = now + self.period_s
             elif mode == "off":
                 next_send = now
-            self.connection.recv_match(blocking=False)
             time.sleep(0.002)
 
 
@@ -251,7 +304,12 @@ def exercise_legacy_msp_override(msp: MspClient, rx1, rx2) -> None:
 
 
 def configure_case(kind: str) -> list[str]:
-    commands = ["set dual_rx_enabled = ON", "aux 0 13 0 1700 2100", "feature TELEMETRY"]
+    commands = [
+        "set dual_rx_enabled = ON",
+        "set mavlink_sysid = 1",
+        "aux 0 13 0 1700 2100",
+        "feature TELEMETRY",
+    ]
     if kind == "crsf-mavlink":
         commands += [
             "set receiver_type = SERIAL",
@@ -284,22 +342,62 @@ def configure_case(kind: str) -> list[str]:
     return commands
 
 
+def sitl_log_count(sitl: SitlProcess, marker: str) -> int:
+    try:
+        text = sitl.log_path.read_text(encoding="utf-8", errors="replace")
+    except OSError:
+        return 0
+    return text.count(marker)
+
+
+def wait_for_sitl_reset(sitl: SitlProcess, reset_count_before: int, timeout_s: float = 8.0) -> None:
+    """Wait until CLI `save` has persisted configuration and entered reset."""
+    deadline = time.monotonic() + timeout_s
+    last_reset_count = reset_count_before
+    while time.monotonic() < deadline:
+        sitl.check_alive()
+        last_reset_count = sitl_log_count(sitl, "[SYSTEM] Reset")
+        if last_reset_count > reset_count_before:
+            return
+        time.sleep(0.02)
+    raise TestFailure(
+        "SITL did not enter CLI save/reset cycle within "
+        f"{timeout_s:.1f}s (reset {reset_count_before}->{last_reset_count})\n"
+        f"{sitl.tail_log(lines=80)}"
+    )
+
+
+def hard_restart_sitl_after_configuration(sitl: SitlProcess, reset_count_before: int) -> None:
+    """Replace SITL's in-process exec reboot with a clean harness restart.
+
+    The mixed serial layout opens UARTs lazily. During the exec-based reboot a
+    TCP listener can survive long enough for the new instance to hit EADDRINUSE,
+    leaving a receiver connected to the dying listener. Once `save` has reached
+    systemReset(), terminate that process completely and start a fresh one from
+    the saved EEPROM so every UART listener is recreated from a clean process.
+    """
+    wait_for_sitl_reset(sitl, reset_count_before)
+    sitl.stop()
+    time.sleep(0.05)
+    sitl.start()
+
+
 def run_case(kind: str, binary: Path, repo: Path, tcp_base: int, temp_dir: Path) -> None:
     eeprom = temp_dir / f"{kind}.bin"
-    sitl = SitlProcess(binary, repo, eeprom, tcp_base, temp_dir / f"{kind}.log")
+    log_path = temp_dir / f"{kind}.log"
+    sitl = SitlProcess(binary, repo, eeprom, tcp_base, log_path)
     msp: MspClient | None = None
     ingresses: list = []
+    print(f"\n[CASE] {kind}")
     try:
         sitl.start()
         msp_port = tcp_port(tcp_base, UART_MSP)
         wait_tcp("127.0.0.1", msp_port, 8.0, sitl)
         time.sleep(0.15)
+        reset_count = sitl_log_count(sitl, "[SYSTEM] Reset")
         configure_cli("127.0.0.1", msp_port, configure_case(kind))
-        time.sleep(1.0)
+        hard_restart_sitl_after_configuration(sitl, reset_count)
         wait_tcp("127.0.0.1", msp_port, 8.0, sitl)
-        wait_tcp("127.0.0.1", tcp_port(tcp_base, UART_RX1), 8.0, sitl)
-        if kind != "crsf-msp":
-            wait_tcp("127.0.0.1", tcp_port(tcp_base, UART_RX2), 8.0, sitl)
 
         msp = MspClient("127.0.0.1", msp_port, timeout_s=2.0)
         status = msp.link_status()
@@ -333,6 +431,17 @@ def run_case(kind: str, binary: Path, repo: Path, tcp_base: int, temp_dir: Path)
         if kind == "crsf-mavlink":
             exercise_legacy_msp_override(msp, rx1, rx2)
         exercise_selector(msp, rx1, rx2, kind.replace("-", " + ").upper())
+    except Exception as exc:
+        process_code = sitl.process.poll() if sitl.process is not None else None
+        sitl_state = "still running" if sitl.process is not None and process_code is None else f"exited with code {process_code}"
+        log_tail = sitl.tail_log(lines=120)
+        raise TestFailure(
+            f"{kind} failed: {type(exc).__name__}: {exc}\n"
+            f"SITL state: {sitl_state}\n"
+            f"SITL log: {log_path}\n"
+            f"EEPROM: {eeprom}\n"
+            f"--- SITL LOG TAIL ---\n{log_tail}\n--- END SITL LOG TAIL ---"
+        ) from exc
     finally:
         for ingress in reversed(ingresses):
             ingress.close()
@@ -357,14 +466,17 @@ def main() -> int:
     print("Dual RX mixed-ingress SITL test")
     print(f"  repo: {repo}")
     print(f"  SITL: {binary}\n")
+
+    temp_dir = Path(tempfile.mkdtemp(prefix="inav-dualrx-mixed-"))
     try:
-        with tempfile.TemporaryDirectory(prefix="inav-dualrx-mixed-") as temp_name:
-            temp_dir = Path(temp_name)
-            for kind in ("crsf-mavlink", "crsf-msp", "mavlink-mavlink"):
-                run_case(kind, binary, repo, args.tcp_base, temp_dir)
-    except BaseException as exc:
-        print(f"\n[FAIL] {exc}")
+        for kind in ("crsf-mavlink", "crsf-msp", "mavlink-mavlink"):
+            run_case(kind, binary, repo, args.tcp_base, temp_dir)
+    except Exception:
+        print(f"\n[FAIL] Full diagnostics retained in: {temp_dir}")
+        traceback.print_exc()
         return 1
+
+    shutil.rmtree(temp_dir, ignore_errors=True)
     print("\nALL DUAL RX MIXED-INGRESS SITL TESTS PASSED")
     return 0