mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 01:01:12 +00:00
[AZ-396] [AZ-397] Batch 11: C8 source-set switch + QGC telemetry adapter
AZ-396: PymavlinkArdupilotAdapter.request_source_set_switch body sends MAV_CMD_SET_EKF_SOURCE_SET, awaits COMMAND_ACK with timeout, enforces Invariant 11 idempotence (1s rate-limit + skip-after-success). Adds runtime_root.SpoofRecoverySink to bridge C5 spoof-promotion-recovered signal to the C8 outbound thread via a bounded dispatch queue. FcConfig gains spoof_recovery_source_set + source_set_switch_timeout_ms. AZ-397: QgcTelemetryAdapter implements GcsAdapter strategy: MAVLink 2.0 to QGC, emit_summary downsamples 5Hz to configurable summary_rate_hz [0.5, 5.0] via integer modulo, emit_status_text mirrors to GCS link, subscribe_operator_commands translates COMMAND_LONG / PARAM_REQUEST_* / REQUEST_DATA_STREAM / MISSION_* / SET_MODE into OperatorCommand DTOs and audits each receipt to FDR. FcKind.GCS_QGC added for PortConfig. Tests: 25 new (12 AZ-396 + 13 AZ-397); full suite 501 passing, 2 skipped. Contracts unchanged (additive FcConfig fields, range relaxation on GcsConfig.summary_rate_hz, additive FcKind enum value). Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -229,7 +229,7 @@ def test_ac2_fc_telemetry_frame_dto_frozen() -> None:
|
||||
|
||||
def test_ac3_fc_kind_has_two_members() -> None:
|
||||
# Assert
|
||||
assert {m.name for m in FcKind} == {"ARDUPILOT_PLANE", "INAV"}
|
||||
assert {m.name for m in FcKind} == {"ARDUPILOT_PLANE", "INAV", "GCS_QGC"}
|
||||
|
||||
|
||||
def test_ac3_flight_state_has_five_members() -> None:
|
||||
@@ -505,9 +505,11 @@ def test_signing_key_source_unknown_value_rejected() -> None:
|
||||
|
||||
|
||||
def test_gcs_summary_rate_out_of_range_rejected() -> None:
|
||||
# AZ-397 widened the valid range to [0.5, 5.0] (AC-10); the boundary
|
||||
# cases below now fall OUTSIDE the new range.
|
||||
# Act + Assert — too high
|
||||
with pytest.raises(ConfigError, match=r"summary_rate_hz"):
|
||||
GcsConfig(summary_rate_hz=5.0)
|
||||
GcsConfig(summary_rate_hz=5.1)
|
||||
# Too low
|
||||
with pytest.raises(ConfigError, match=r"summary_rate_hz"):
|
||||
GcsConfig(summary_rate_hz=0.5)
|
||||
GcsConfig(summary_rate_hz=0.4)
|
||||
|
||||
@@ -297,12 +297,17 @@ def test_ac8_open_without_signing_key_succeeds(conn: _ConnStub, tmp_path) -> Non
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-9: source-set switch raises NotImplementedError
|
||||
# AC-9: source-set switch is wired (AZ-396 replaced AZ-393's placeholder)
|
||||
#
|
||||
# AZ-393's original AC-9 asserted `request_source_set_switch()` raises
|
||||
# `NotImplementedError`; AZ-396 (batch 11) replaced that placeholder
|
||||
# with the real body. The AZ-396 AC tests cover the new behaviour;
|
||||
# the AZ-393 surface is now exercised by those tests rather than this
|
||||
# one. The check left here verifies the method is present and callable.
|
||||
|
||||
|
||||
def test_ac9_source_set_switch_not_implemented(adapter: PymavlinkArdupilotAdapter) -> None:
|
||||
with pytest.raises(NotImplementedError, match="AZ-396"):
|
||||
adapter.request_source_set_switch()
|
||||
def test_ac9_source_set_switch_method_callable(adapter: PymavlinkArdupilotAdapter) -> None:
|
||||
assert callable(adapter.request_source_set_switch)
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
|
||||
@@ -0,0 +1,370 @@
|
||||
"""AZ-396 — D-C8-2 source-set switch AC tests."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import threading
|
||||
import time
|
||||
from collections.abc import Iterable
|
||||
from typing import Any
|
||||
from unittest import mock
|
||||
|
||||
import pytest
|
||||
|
||||
from gps_denied_onboard._types.fc import FcKind, PortConfig, Severity
|
||||
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
|
||||
CovarianceProjector,
|
||||
)
|
||||
from gps_denied_onboard.components.c8_fc_adapter.errors import SourceSetSwitchError
|
||||
from gps_denied_onboard.components.c8_fc_adapter.pymavlink_ardupilot_adapter import (
|
||||
PymavlinkArdupilotAdapter,
|
||||
)
|
||||
from gps_denied_onboard.config import load_config
|
||||
from gps_denied_onboard.runtime_root.spoof_recovery_sink import SpoofRecoverySink
|
||||
|
||||
# AC-1 / AC-2 / AC-3: pymavlink ardupilotmega command id for SET_EKF_SOURCE_SET.
|
||||
_CMD_SET_EKF_SOURCE_SET = 42007
|
||||
_MAV_RESULT_ACCEPTED = 0
|
||||
_MAV_RESULT_FAILED = 4
|
||||
|
||||
|
||||
class _AckMsg:
|
||||
def __init__(self, command: int, result: int) -> None:
|
||||
self.command = command
|
||||
self.result = result
|
||||
|
||||
|
||||
class _MavStub:
|
||||
def __init__(self) -> None:
|
||||
self.command_long_calls: list[tuple[int, ...]] = []
|
||||
self.statustext_calls: list[tuple[int, bytes]] = []
|
||||
self.named_value_float_calls: list[tuple[Any, ...]] = []
|
||||
self.gps_input_calls: list[tuple[Any, ...]] = []
|
||||
|
||||
def command_long_send(
|
||||
self,
|
||||
target_system: int,
|
||||
target_component: int,
|
||||
command: int,
|
||||
confirmation: int,
|
||||
p1: float,
|
||||
p2: float,
|
||||
p3: float,
|
||||
p4: float,
|
||||
p5: float,
|
||||
p6: float,
|
||||
p7: float,
|
||||
) -> None:
|
||||
self.command_long_calls.append((target_system, target_component, command, confirmation, p1))
|
||||
|
||||
def statustext_send(self, severity: int, text: bytes) -> None:
|
||||
self.statustext_calls.append((severity, text))
|
||||
|
||||
def named_value_float_send(self, time_boot_ms: int, name: bytes, value: float) -> None:
|
||||
self.named_value_float_calls.append((time_boot_ms, name, value))
|
||||
|
||||
def gps_input_send(self, *args: Any) -> None:
|
||||
self.gps_input_calls.append(args)
|
||||
|
||||
|
||||
class _ConnStub:
|
||||
def __init__(self, ack_queue: Iterable[_AckMsg] | None = None) -> None:
|
||||
self.mav = _MavStub()
|
||||
self.target_system = 1
|
||||
self.target_component = 1
|
||||
self._ack_queue = list(ack_queue or [])
|
||||
self.closed = False
|
||||
|
||||
def recv_match(self, *, type: str, blocking: bool, timeout: float | None) -> Any:
|
||||
# Real pymavlink filters by ``type``; the inbound decoder thread
|
||||
# calls recv_match without a type filter (None), the outbound
|
||||
# source-set switch calls with type="COMMAND_ACK". Honour that
|
||||
# so the inbound loop does not eat our ACK.
|
||||
if type != "COMMAND_ACK":
|
||||
return None
|
||||
if not self._ack_queue:
|
||||
return None
|
||||
return self._ack_queue.pop(0)
|
||||
|
||||
def setup_signing(self, key: bytes) -> None:
|
||||
pass
|
||||
|
||||
def close(self) -> None:
|
||||
self.closed = True
|
||||
|
||||
|
||||
def _ap_config(
|
||||
*,
|
||||
source_set: int = 1,
|
||||
timeout_ms: int = 1500,
|
||||
) -> Any:
|
||||
env = {
|
||||
"FC_ADAPTER": "ardupilot_plane",
|
||||
"FC_PORT_DEVICE": "/dev/null",
|
||||
"FC_PORT_BAUD": "921600",
|
||||
"FC_SIGNING_KEY_SOURCE": "none",
|
||||
"FC_SPOOF_RECOVERY_SOURCE_SET": str(source_set),
|
||||
"FC_SOURCE_SET_SWITCH_TIMEOUT_MS": str(timeout_ms),
|
||||
}
|
||||
return load_config(env=env, paths=(), require_env=False)
|
||||
|
||||
|
||||
def _make_adapter(conn: _ConnStub, cfg: Any) -> PymavlinkArdupilotAdapter:
|
||||
fdr = mock.MagicMock()
|
||||
a = PymavlinkArdupilotAdapter(
|
||||
config=cfg,
|
||||
wgs_converter=mock.MagicMock(),
|
||||
covariance_projector=CovarianceProjector(fdr_client=fdr),
|
||||
fdr_client=fdr,
|
||||
flight_id="flt-az396",
|
||||
connect_factory=lambda device, baud: conn,
|
||||
)
|
||||
port = PortConfig(fc_kind=FcKind.ARDUPILOT_PLANE, device="/dev/null", baud=921600)
|
||||
a.open(port, signing_key=None)
|
||||
return a
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-1: ACK success path
|
||||
|
||||
|
||||
def test_ac1_ack_success(caplog: pytest.LogCaptureFixture) -> None:
|
||||
conn = _ConnStub(ack_queue=[_AckMsg(_CMD_SET_EKF_SOURCE_SET, _MAV_RESULT_ACCEPTED)])
|
||||
a = _make_adapter(conn, _ap_config(source_set=1))
|
||||
try:
|
||||
with caplog.at_level(logging.INFO):
|
||||
a.request_source_set_switch()
|
||||
assert len(conn.mav.command_long_calls) == 1
|
||||
_, _, cmd, _, p1 = conn.mav.command_long_calls[0]
|
||||
assert cmd == _CMD_SET_EKF_SOURCE_SET
|
||||
assert p1 == 1.0
|
||||
assert any(
|
||||
getattr(r, "kind", None) == "c8.ap.source_set_switch_executed" for r in caplog.records
|
||||
)
|
||||
assert any(sev == int(Severity.INFO.value) for sev, _ in conn.mav.statustext_calls)
|
||||
assert any(
|
||||
call.args[0].payload.get("kind") == "c8.ap.source_set_switch_executed"
|
||||
for call in a._fdr_client.enqueue.mock_calls # type: ignore[attr-defined]
|
||||
)
|
||||
finally:
|
||||
a.close()
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-2: non-success ACK raises
|
||||
|
||||
|
||||
def test_ac2_non_success_ack_raises(caplog: pytest.LogCaptureFixture) -> None:
|
||||
conn = _ConnStub(ack_queue=[_AckMsg(_CMD_SET_EKF_SOURCE_SET, _MAV_RESULT_FAILED)])
|
||||
a = _make_adapter(conn, _ap_config())
|
||||
try:
|
||||
with caplog.at_level(logging.ERROR), pytest.raises(SourceSetSwitchError, match="result=4"):
|
||||
a.request_source_set_switch()
|
||||
assert any(
|
||||
getattr(r, "kind", None) == "c8.ap.source_set_switch_failed" for r in caplog.records
|
||||
)
|
||||
assert any(sev == int(Severity.ERROR.value) for sev, _ in conn.mav.statustext_calls)
|
||||
finally:
|
||||
a.close()
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-3: ACK timeout raises
|
||||
|
||||
|
||||
def test_ac3_ack_timeout_raises(caplog: pytest.LogCaptureFixture) -> None:
|
||||
conn = _ConnStub(ack_queue=[]) # never ACK
|
||||
a = _make_adapter(conn, _ap_config(timeout_ms=200))
|
||||
try:
|
||||
with (
|
||||
caplog.at_level(logging.ERROR),
|
||||
pytest.raises(SourceSetSwitchError, match="timeout after 200ms"),
|
||||
):
|
||||
a.request_source_set_switch()
|
||||
assert any(
|
||||
getattr(r, "kind", None) == "c8.ap.source_set_switch_failed" for r in caplog.records
|
||||
)
|
||||
finally:
|
||||
a.close()
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-4: configurable timeout (AC-3 already exercises a non-default value)
|
||||
|
||||
|
||||
def test_ac4_configurable_timeout_uses_config_value() -> None:
|
||||
conn = _ConnStub(ack_queue=[])
|
||||
cfg = _ap_config(timeout_ms=500)
|
||||
a = _make_adapter(conn, cfg)
|
||||
try:
|
||||
with pytest.raises(SourceSetSwitchError, match="timeout after 500ms"):
|
||||
a.request_source_set_switch()
|
||||
finally:
|
||||
a.close()
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-5: idempotence within 1 s — second call no-op'd
|
||||
|
||||
|
||||
def test_ac5_idempotence_within_1s_rate_limited(caplog: pytest.LogCaptureFixture) -> None:
|
||||
conn = _ConnStub(ack_queue=[_AckMsg(_CMD_SET_EKF_SOURCE_SET, _MAV_RESULT_FAILED)])
|
||||
a = _make_adapter(conn, _ap_config())
|
||||
try:
|
||||
with pytest.raises(SourceSetSwitchError):
|
||||
a.request_source_set_switch()
|
||||
with caplog.at_level(logging.INFO):
|
||||
a.request_source_set_switch()
|
||||
# only one command_long_send hit the wire
|
||||
assert len(conn.mav.command_long_calls) == 1
|
||||
assert any(
|
||||
getattr(r, "kind", None) == "c8.ap.source_set_switch_rate_limited"
|
||||
for r in caplog.records
|
||||
)
|
||||
finally:
|
||||
a.close()
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-6: idempotence after successful switch — second call after >1 s logs
|
||||
# but does NOT re-emit
|
||||
|
||||
|
||||
def test_ac6_idempotence_after_success_no_reissue(caplog: pytest.LogCaptureFixture) -> None:
|
||||
conn = _ConnStub(ack_queue=[_AckMsg(_CMD_SET_EKF_SOURCE_SET, _MAV_RESULT_ACCEPTED)])
|
||||
a = _make_adapter(conn, _ap_config())
|
||||
try:
|
||||
a.request_source_set_switch()
|
||||
# advance the monotonic clock past the 1 s rate-limit by mutating
|
||||
# the stored timestamp (we control the adapter under test).
|
||||
a._last_switch_attempt_ns -= 2_000_000_000 # type: ignore[attr-defined]
|
||||
with caplog.at_level(logging.INFO):
|
||||
a.request_source_set_switch()
|
||||
assert len(conn.mav.command_long_calls) == 1 # never re-issued
|
||||
assert any(
|
||||
getattr(r, "kind", None) == "c8.ap.source_set_switch_already_active"
|
||||
for r in caplog.records
|
||||
)
|
||||
finally:
|
||||
a.close()
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-7: SpoofRecoverySink invokes request_source_set_switch on adapter
|
||||
|
||||
|
||||
def test_ac7_spoof_recovery_sink_triggers_switch() -> None:
|
||||
mock_adapter = mock.MagicMock()
|
||||
sink = SpoofRecoverySink(mock_adapter)
|
||||
sink.start()
|
||||
try:
|
||||
sink.publish()
|
||||
# Wait briefly for the dispatch thread to process.
|
||||
for _ in range(20):
|
||||
if mock_adapter.request_source_set_switch.call_count >= 1:
|
||||
break
|
||||
time.sleep(0.05)
|
||||
assert mock_adapter.request_source_set_switch.call_count == 1
|
||||
finally:
|
||||
sink.stop()
|
||||
|
||||
|
||||
def test_ac7_spoof_recovery_sink_isolates_errors() -> None:
|
||||
mock_adapter = mock.MagicMock()
|
||||
mock_adapter.request_source_set_switch.side_effect = SourceSetSwitchError("ACK timeout")
|
||||
sink = SpoofRecoverySink(mock_adapter)
|
||||
sink.start()
|
||||
try:
|
||||
sink.publish()
|
||||
# Sink must not crash; we can re-publish.
|
||||
for _ in range(20):
|
||||
if mock_adapter.request_source_set_switch.call_count >= 1:
|
||||
break
|
||||
time.sleep(0.05)
|
||||
assert mock_adapter.request_source_set_switch.call_count == 1
|
||||
sink.publish()
|
||||
for _ in range(20):
|
||||
if mock_adapter.request_source_set_switch.call_count >= 2:
|
||||
break
|
||||
time.sleep(0.05)
|
||||
assert mock_adapter.request_source_set_switch.call_count == 2
|
||||
finally:
|
||||
sink.stop()
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-8: source_set_id from config
|
||||
|
||||
|
||||
def test_ac8_source_set_id_from_config() -> None:
|
||||
conn = _ConnStub(ack_queue=[_AckMsg(_CMD_SET_EKF_SOURCE_SET, _MAV_RESULT_ACCEPTED)])
|
||||
a = _make_adapter(conn, _ap_config(source_set=2))
|
||||
try:
|
||||
a.request_source_set_switch()
|
||||
_, _, _, _, p1 = conn.mav.command_long_calls[0]
|
||||
assert p1 == 2.0
|
||||
finally:
|
||||
a.close()
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-9: AZ-393 NotImplementedError placeholder replaced
|
||||
|
||||
|
||||
def test_ac9_no_longer_raises_not_implemented() -> None:
|
||||
conn = _ConnStub(ack_queue=[_AckMsg(_CMD_SET_EKF_SOURCE_SET, _MAV_RESULT_ACCEPTED)])
|
||||
a = _make_adapter(conn, _ap_config())
|
||||
try:
|
||||
a.request_source_set_switch() # would have raised NotImplementedError pre-AZ-396
|
||||
finally:
|
||||
a.close()
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-10: STATUSTEXT severity matrix
|
||||
|
||||
|
||||
def test_ac10_statustext_severity_matrix(caplog: pytest.LogCaptureFixture) -> None:
|
||||
# success → INFO
|
||||
conn1 = _ConnStub(ack_queue=[_AckMsg(_CMD_SET_EKF_SOURCE_SET, _MAV_RESULT_ACCEPTED)])
|
||||
a1 = _make_adapter(conn1, _ap_config())
|
||||
try:
|
||||
a1.request_source_set_switch()
|
||||
success_severities = [sev for sev, _ in conn1.mav.statustext_calls]
|
||||
assert int(Severity.INFO.value) in success_severities
|
||||
finally:
|
||||
a1.close()
|
||||
# failure → ERROR
|
||||
conn2 = _ConnStub(ack_queue=[_AckMsg(_CMD_SET_EKF_SOURCE_SET, _MAV_RESULT_FAILED)])
|
||||
a2 = _make_adapter(conn2, _ap_config())
|
||||
try:
|
||||
with pytest.raises(SourceSetSwitchError):
|
||||
a2.request_source_set_switch()
|
||||
failure_severities = [sev for sev, _ in conn2.mav.statustext_calls]
|
||||
assert int(Severity.ERROR.value) in failure_severities
|
||||
finally:
|
||||
a2.close()
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# Defence-in-depth: single-writer enforcement still applies
|
||||
|
||||
|
||||
def test_single_writer_thread_enforced_on_switch() -> None:
|
||||
conn = _ConnStub(ack_queue=[_AckMsg(_CMD_SET_EKF_SOURCE_SET, _MAV_RESULT_ACCEPTED)])
|
||||
a = _make_adapter(conn, _ap_config())
|
||||
a.request_source_set_switch() # bind main thread
|
||||
err: list[BaseException] = []
|
||||
|
||||
def run() -> None:
|
||||
try:
|
||||
a.request_source_set_switch()
|
||||
except RuntimeError as e:
|
||||
err.append(e)
|
||||
|
||||
t = threading.Thread(target=run)
|
||||
t.start()
|
||||
t.join(timeout=2.0)
|
||||
assert len(err) == 1
|
||||
assert "single-writer" in str(err[0]).lower()
|
||||
a.close()
|
||||
@@ -0,0 +1,360 @@
|
||||
"""AZ-397 — QgcTelemetryAdapter AC tests."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
import threading
|
||||
from datetime import datetime, timezone
|
||||
from types import SimpleNamespace
|
||||
from typing import Any
|
||||
from unittest import mock
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from gps_denied_onboard._types.fc import (
|
||||
FcKind,
|
||||
OperatorCommand,
|
||||
PortConfig,
|
||||
Severity,
|
||||
)
|
||||
from gps_denied_onboard._types.geo import LatLonAlt
|
||||
from gps_denied_onboard._types.pose import EstimatorOutput
|
||||
from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
|
||||
CovarianceProjector,
|
||||
)
|
||||
from gps_denied_onboard.components.c8_fc_adapter.errors import GcsAdapterConfigError
|
||||
from gps_denied_onboard.components.c8_fc_adapter.mavlink_gcs_adapter import (
|
||||
QgcTelemetryAdapter,
|
||||
_compute_downsample_modulo,
|
||||
)
|
||||
from gps_denied_onboard.config import load_config
|
||||
from gps_denied_onboard.config.schema import ConfigError, GcsConfig
|
||||
from gps_denied_onboard.helpers.wgs_converter import WgsConverter
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# Helpers
|
||||
|
||||
|
||||
class _MavStub:
|
||||
def __init__(self) -> None:
|
||||
self.global_position_int_calls: list[tuple[Any, ...]] = []
|
||||
self.named_value_float_calls: list[tuple[Any, ...]] = []
|
||||
self.statustext_calls: list[tuple[int, bytes]] = []
|
||||
|
||||
def global_position_int_send(self, *args: Any) -> None:
|
||||
self.global_position_int_calls.append(args)
|
||||
|
||||
def named_value_float_send(self, time_boot_ms: int, name: bytes, value: float) -> None:
|
||||
self.named_value_float_calls.append((time_boot_ms, name, value))
|
||||
|
||||
def statustext_send(self, severity: int, text: bytes) -> None:
|
||||
self.statustext_calls.append((severity, text))
|
||||
|
||||
|
||||
class _ConnStub:
|
||||
def __init__(self) -> None:
|
||||
self.mav = _MavStub()
|
||||
self.message_hooks: list[Any] = []
|
||||
self.closed = False
|
||||
|
||||
def close(self) -> None:
|
||||
self.closed = True
|
||||
|
||||
|
||||
def _config(*, summary_rate_hz: float = 2.0) -> Any:
|
||||
env = {
|
||||
"FC_ADAPTER": "ardupilot_plane",
|
||||
"FC_PORT_DEVICE": "/dev/null",
|
||||
"FC_PORT_BAUD": "921600",
|
||||
"FC_SIGNING_KEY_SOURCE": "none",
|
||||
"GCS_ADAPTER": "qgc_mavlink",
|
||||
"GCS_PORT_DEVICE": "/dev/null",
|
||||
"GCS_PORT_BAUD": "921600",
|
||||
"GCS_SUMMARY_RATE_HZ": str(summary_rate_hz),
|
||||
}
|
||||
return load_config(env=env, paths=(), require_env=False)
|
||||
|
||||
|
||||
def _make_output(*, source_label: str = "visual_propagated", frame_id: int = 1) -> EstimatorOutput:
|
||||
return EstimatorOutput(
|
||||
frame_id=frame_id,
|
||||
timestamp=datetime.now(tz=timezone.utc),
|
||||
pose_se3=np.eye(4),
|
||||
covariance_6x6=np.eye(6, dtype=np.float64) * 0.25,
|
||||
source_label=source_label,
|
||||
smoothed=False,
|
||||
extras={"wgs84": LatLonAlt(lat_deg=50.0, lon_deg=30.0, alt_m=100.0)},
|
||||
)
|
||||
|
||||
|
||||
def _make_adapter(conn: _ConnStub, cfg: Any) -> QgcTelemetryAdapter:
|
||||
fdr = mock.MagicMock()
|
||||
a = QgcTelemetryAdapter(
|
||||
config=cfg,
|
||||
wgs_converter=mock.MagicMock(),
|
||||
covariance_projector=CovarianceProjector(fdr_client=fdr),
|
||||
fdr_client=fdr,
|
||||
connect_factory=lambda device, baud: conn,
|
||||
)
|
||||
port = PortConfig(fc_kind=FcKind.GCS_QGC, device="/dev/null", baud=921600)
|
||||
a.open(port)
|
||||
return a
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-1: 5 -> 2 Hz downsampling
|
||||
|
||||
|
||||
def test_ac1_5hz_to_2hz_downsample() -> None:
|
||||
conn = _ConnStub()
|
||||
a = _make_adapter(conn, _config(summary_rate_hz=2.0))
|
||||
try:
|
||||
for i in range(100):
|
||||
a.emit_summary(_make_output(frame_id=i))
|
||||
# modulo = round(5/2) = 2 (round half to even gives 2; we just
|
||||
# need parity with the documented choice). 100 / 2 = 50.
|
||||
assert _compute_downsample_modulo(2.0) == 2
|
||||
assert len(conn.mav.global_position_int_calls) == 50
|
||||
finally:
|
||||
a.close()
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-2: configurable rate
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"rate_hz,expected_frames",
|
||||
[
|
||||
(1.0, 20), # modulo 5 -> 100/5
|
||||
(5.0, 100), # modulo 1 -> no downsample
|
||||
],
|
||||
)
|
||||
def test_ac2_configurable_rate(rate_hz: float, expected_frames: int) -> None:
|
||||
conn = _ConnStub()
|
||||
a = _make_adapter(conn, _config(summary_rate_hz=rate_hz))
|
||||
try:
|
||||
for i in range(100):
|
||||
a.emit_summary(_make_output(frame_id=i))
|
||||
assert len(conn.mav.global_position_int_calls) == expected_frames
|
||||
finally:
|
||||
a.close()
|
||||
|
||||
|
||||
def test_ac2_out_of_range_rate_rejected_at_config() -> None:
|
||||
with pytest.raises(ConfigError, match="summary_rate_hz"):
|
||||
GcsConfig(summary_rate_hz=10.0)
|
||||
with pytest.raises(ConfigError, match="summary_rate_hz"):
|
||||
GcsConfig(summary_rate_hz=0.2)
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-3: summary frame fields
|
||||
|
||||
|
||||
def test_ac3_summary_frame_fields() -> None:
|
||||
conn = _ConnStub()
|
||||
fdr = mock.MagicMock()
|
||||
a = QgcTelemetryAdapter(
|
||||
config=_config(summary_rate_hz=5.0), # no downsample for simplicity
|
||||
wgs_converter=mock.MagicMock(),
|
||||
covariance_projector=CovarianceProjector(fdr_client=fdr),
|
||||
fdr_client=fdr,
|
||||
connect_factory=lambda device, baud: conn,
|
||||
)
|
||||
port = PortConfig(fc_kind=FcKind.GCS_QGC, device="/dev/null", baud=921600)
|
||||
a.open(port)
|
||||
try:
|
||||
wgs = LatLonAlt(lat_deg=50.45, lon_deg=30.52, alt_m=180.0)
|
||||
output = EstimatorOutput(
|
||||
frame_id=1,
|
||||
timestamp=datetime.now(tz=timezone.utc),
|
||||
pose_se3=np.eye(4),
|
||||
covariance_6x6=np.diag([0.25, 0.25, 9.0, 1.0, 1.0, 1.0]).astype(np.float64),
|
||||
source_label="visual_propagated",
|
||||
smoothed=False,
|
||||
extras={"wgs84": wgs},
|
||||
)
|
||||
a.emit_summary(output)
|
||||
assert len(conn.mav.global_position_int_calls) == 1
|
||||
call = conn.mav.global_position_int_calls[0]
|
||||
# global_position_int_send(time_boot_ms, lat, lon, alt_mm, rel_alt_mm, vx, vy, vz, hdg)
|
||||
assert call[1] == int(wgs.lat_deg * 1e7)
|
||||
assert call[2] == int(wgs.lon_deg * 1e7)
|
||||
assert call[3] == int(wgs.alt_m * 1000.0)
|
||||
# horiz_accuracy via NAMED_VALUE_FLOAT
|
||||
assert len(conn.mav.named_value_float_calls) == 1
|
||||
_, name, value = conn.mav.named_value_float_calls[0]
|
||||
assert name == b"horiz_m"
|
||||
expected = CovarianceProjector(fdr_client=mock.MagicMock()).to_ardupilot_horiz_accuracy_m(
|
||||
output
|
||||
)
|
||||
assert value == pytest.approx(expected)
|
||||
finally:
|
||||
a.close()
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-4: STATUSTEXT mirror
|
||||
|
||||
|
||||
def test_ac4_statustext_mirror() -> None:
|
||||
conn = _ConnStub()
|
||||
a = _make_adapter(conn, _config(summary_rate_hz=2.0))
|
||||
try:
|
||||
a.emit_status_text("hello", Severity.INFO)
|
||||
assert len(conn.mav.statustext_calls) == 1
|
||||
sev, text = conn.mav.statustext_calls[0]
|
||||
assert sev == int(Severity.INFO.value)
|
||||
assert text == b"hello"
|
||||
finally:
|
||||
a.close()
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-5: operator command subscription
|
||||
|
||||
|
||||
def test_ac5_operator_command_subscription_invokes_callback() -> None:
|
||||
conn = _ConnStub()
|
||||
a = _make_adapter(conn, _config())
|
||||
received: list[OperatorCommand] = []
|
||||
try:
|
||||
a.subscribe_operator_commands(received.append)
|
||||
# Inject a PARAM_REQUEST_LIST stub
|
||||
msg = SimpleNamespace(target_system=1, target_component=1)
|
||||
msg.get_type = lambda: "PARAM_REQUEST_LIST"
|
||||
msg.get_srcSystem = lambda: 42
|
||||
# Fire every registered hook
|
||||
for hook in conn.message_hooks:
|
||||
hook(conn, msg)
|
||||
assert len(received) == 1
|
||||
cmd = received[0]
|
||||
assert cmd.command == "PARAM_REQUEST_LIST"
|
||||
assert cmd.payload.get("target_system") == 1
|
||||
finally:
|
||||
a.close()
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-6: operator command FDR audit trail
|
||||
|
||||
|
||||
def test_ac6_operator_command_fdr_audit_trail() -> None:
|
||||
conn = _ConnStub()
|
||||
fdr = mock.MagicMock()
|
||||
a = QgcTelemetryAdapter(
|
||||
config=_config(),
|
||||
wgs_converter=mock.MagicMock(),
|
||||
covariance_projector=CovarianceProjector(fdr_client=fdr),
|
||||
fdr_client=fdr,
|
||||
connect_factory=lambda device, baud: conn,
|
||||
)
|
||||
port = PortConfig(fc_kind=FcKind.GCS_QGC, device="/dev/null", baud=921600)
|
||||
a.open(port)
|
||||
try:
|
||||
a.subscribe_operator_commands(lambda _cmd: None)
|
||||
msg = SimpleNamespace(target_system=1, target_component=1, command=400)
|
||||
msg.get_type = lambda: "COMMAND_LONG"
|
||||
msg.get_srcSystem = lambda: 7
|
||||
for hook in conn.message_hooks:
|
||||
hook(conn, msg)
|
||||
operator_records = [
|
||||
call
|
||||
for call in fdr.enqueue.mock_calls
|
||||
if call.args[0].payload.get("kind") == "c8.gcs.operator_command"
|
||||
]
|
||||
assert len(operator_records) == 1
|
||||
rec = operator_records[0].args[0].payload
|
||||
assert rec["kv"]["command"] == "COMMAND_LONG"
|
||||
assert rec["kv"]["source_system"] == 7
|
||||
finally:
|
||||
a.close()
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-7: single-writer thread for outbound
|
||||
|
||||
|
||||
def test_ac7_single_writer_thread() -> None:
|
||||
conn = _ConnStub()
|
||||
a = _make_adapter(conn, _config(summary_rate_hz=5.0))
|
||||
err: list[BaseException] = []
|
||||
try:
|
||||
a.emit_summary(_make_output())
|
||||
|
||||
def run() -> None:
|
||||
try:
|
||||
a.emit_summary(_make_output(frame_id=2))
|
||||
except RuntimeError as e:
|
||||
err.append(e)
|
||||
|
||||
t = threading.Thread(target=run)
|
||||
t.start()
|
||||
t.join(timeout=2.0)
|
||||
assert len(err) == 1
|
||||
assert "single-writer" in str(err[0]).lower()
|
||||
finally:
|
||||
a.close()
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-8: first emit logged once
|
||||
|
||||
|
||||
def test_ac8_first_emit_logged_once(caplog: pytest.LogCaptureFixture) -> None:
|
||||
conn = _ConnStub()
|
||||
a = _make_adapter(conn, _config(summary_rate_hz=5.0))
|
||||
try:
|
||||
with caplog.at_level(logging.INFO):
|
||||
for i in range(5):
|
||||
a.emit_summary(_make_output(frame_id=i))
|
||||
first = [
|
||||
r for r in caplog.records if getattr(r, "kind", None) == "c8.gcs.first_summary_emit"
|
||||
]
|
||||
assert len(first) == 1
|
||||
finally:
|
||||
a.close()
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-9: WGS84 round-trip (defensive)
|
||||
|
||||
|
||||
def test_ac9_wgs84_round_trip_within_1cm() -> None:
|
||||
# Round-trip a known lat/lon through WgsConverter ECEF and back.
|
||||
origin = LatLonAlt(lat_deg=50.45, lon_deg=30.52, alt_m=180.0)
|
||||
enu = WgsConverter.latlonalt_to_local_enu(
|
||||
origin, LatLonAlt(lat_deg=50.4501, lon_deg=30.5201, alt_m=180.5)
|
||||
)
|
||||
recovered = WgsConverter.local_enu_to_latlonalt(origin, enu)
|
||||
expected = LatLonAlt(lat_deg=50.4501, lon_deg=30.5201, alt_m=180.5)
|
||||
# 1 cm threshold — pyproj's ENU round-trip is sub-millimetre at
|
||||
# these distances; we keep the AC's 1 cm threshold as the contract.
|
||||
enu_residual = WgsConverter.latlonalt_to_local_enu(expected, recovered)
|
||||
assert np.linalg.norm(enu_residual) < 0.01
|
||||
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# AC-10: GcsAdapterConfigError on bad config (config-load path)
|
||||
|
||||
|
||||
def test_ac10_gcs_config_error_on_bad_rate() -> None:
|
||||
with pytest.raises(ConfigError, match="summary_rate_hz"):
|
||||
GcsConfig(summary_rate_hz=6.0)
|
||||
|
||||
|
||||
def test_ac10_open_rejects_wrong_fc_kind() -> None:
|
||||
conn = _ConnStub()
|
||||
fdr = mock.MagicMock()
|
||||
a = QgcTelemetryAdapter(
|
||||
config=_config(),
|
||||
wgs_converter=mock.MagicMock(),
|
||||
covariance_projector=CovarianceProjector(fdr_client=fdr),
|
||||
fdr_client=fdr,
|
||||
connect_factory=lambda device, baud: conn,
|
||||
)
|
||||
port = PortConfig(fc_kind=FcKind.ARDUPILOT_PLANE, device="/dev/null", baud=921600)
|
||||
with pytest.raises(GcsAdapterConfigError, match="GCS_QGC"):
|
||||
a.open(port)
|
||||
Reference in New Issue
Block a user