motian_mavbridge/tests/test_telemetry.py

import json
import math
import time
import unittest

from motian_bridge import Bridge, Config
from motian_bridge.dialects.v20 import VKFly
from motian_bridge.telemetry import Telemetry


def cfg(**kwargs):
    fields = dict(system_id="UAV000001", uav_type=7, serial_port="loop://",
                  serial_baudrate=57600, mqtt_host="localhost")
    fields.update(kwargs)
    return Config(**fields)


def sender(system=1, component=1):
    return VKFly.MAVLink(None, srcSystem=system, srcComponent=component)


def heartbeat(mav, armed=False, mode=4):
    return mav.heartbeat_encode(
        VKFly.MAV_TYPE_QUADROTOR, VKFly.MAV_AUTOPILOT_GENERIC,
        VKFly.MAV_MODE_FLAG_SAFETY_ARMED if armed else 0,
        mode, VKFly.MAV_STATE_ACTIVE).pack(mav)


def bms(mav, id_=0, voltage=22800, current=350, percent=76):
    return mav.vk_bms_status_encode(
        1000, voltage, current, 28, percent, id_, 3, 6,
        [0] * 30, 42, 95).pack(mav)


def position(mav):
    return mav.global_position_int_encode(
        1000, 312304000, 1214737000, 135500, 120500,
        520, 110, -30, 8730).pack(mav)


class ParserTests(unittest.TestCase):
    def test_split_frames_crc_and_v1_heartbeat(self):
        telemetry = Telemetry(cfg())
        mav = sender()
        frame = heartbeat(mav, True)
        self.assertEqual(telemetry.feed(frame[:7], now=10), [])
        self.assertEqual(telemetry.feed(frame[7:] + position(mav), now=10),
                         [(274, True)])
        data = dict(telemetry.tick(now=10))[258]
        self.assertAlmostEqual(data["Latitude"], 31.2304)
        self.assertAlmostEqual(data["Altitude"], 120.5)
        self.assertEqual(data["VelocityZ"], -0.3)
        self.assertEqual(data["FlightMode"], "POSHOLD")

        corrupted = bytearray(position(mav))
        corrupted[-1] ^= 0xFF
        telemetry.feed(corrupted, now=11)
        # Corrupted position must not replace the good observation.
        self.assertEqual(telemetry.latest["GLOBAL_POSITION_INT"][1], 10)
        self.assertEqual(telemetry.feed(heartbeat(mav, False), now=12), [(274, False)])

        mav_v1 = sender()
        frame_v1 = mav_v1.heartbeat_encode(
            VKFly.MAV_TYPE_QUADROTOR, VKFly.MAV_AUTOPILOT_GENERIC, 128, 12,
            VKFly.MAV_STATE_ACTIVE).pack(mav_v1, force_mavlink1=True)
        self.assertEqual(frame_v1[0], 0xFE)
        self.assertEqual(Telemetry(cfg()).feed(frame_v1, now=1), [(274, True)])

    def test_flight_units_freshness_and_partial_state(self):
        telemetry = Telemetry(cfg())
        mav = sender()
        telemetry.feed(heartbeat(mav), now=0)
        initial = dict(telemetry.tick(now=0))[258]
        self.assertNotIn("Latitude", initial)
        self.assertEqual(initial["Armed"], False)
        telemetry.feed(mav.attitude_encode(
            1000, -0.5, 1.2, -math.pi / 2, 0, 0, 0).pack(mav), now=0.5)
        telemetry.feed(position(mav), now=0.5)
        telemetry.feed(mav.vfr_hud_encode(
            12.4, 13.1, 87, 50, 135.5, 0.8).pack(mav), now=0.5)
        telemetry.feed(mav.home_position_encode(
            312280000, 1214700000, 15000, 0, 0, 0, [1, 0, 0, 0],
            0, 0, 0).pack(mav), now=0.5)
        telemetry.feed(mav.gps_raw_int_encode(
            1000000, 3, 312304000, 1214737000, 135500,
            80, 110, 1300, 8700, 18).pack(mav), now=0.5)
        self.assertEqual(telemetry.tick(now=0.9), [])
        state = dict(telemetry.tick(now=1))[258]
        self.assertAlmostEqual(state["Roll"], math.degrees(-0.5), places=4)
        self.assertAlmostEqual(state["Pitch"], math.degrees(1.2), places=4)
        self.assertAlmostEqual(state["Yaw"], 270, places=4)
        self.assertAlmostEqual(state["GroundSpeed"], 13.1, places=5)
        self.assertEqual(state["HomeAltitude"], 15)
        self.assertGreater(state["SpaceDistToHome"], state["DistanceToHome"])
        self.assertEqual(state["Gps"]["SatelliteCount"], 18)
        self.assertEqual(state["Gps"]["LevelDivisor"], 0.8)
        self.assertNotIn("Temperature", state)
        self.assertNotIn("FlyTime", state)
        self.assertNotIn("IsMotorsOn", state)

        # Heartbeat is fresh, but all other observations have expired.
        telemetry.feed(heartbeat(mav), now=5.6)
        stale = dict(telemetry.tick(now=5.6))[258]
        self.assertNotIn("Latitude", stale)
        self.assertNotIn("Gps", stale)
        self.assertNotIn("HomeLatitude", stale)
        self.assertEqual(telemetry.tick(now=11), [])
        self.assertEqual(telemetry.feed(heartbeat(mav), now=11), [])
        self.assertEqual(dict(telemetry.tick(now=11))[258]["FlightMode"], "POSHOLD")

    def test_lock_to_first_flight_controller_and_reset(self):
        telemetry = Telemetry(cfg())
        gcs = sender(10, 190)
        foreign = sender(2, 1)
        selected = sender(1, 1)
        telemetry.feed(heartbeat(gcs), now=0)
        self.assertIsNone(telemetry.source)
        telemetry.feed(heartbeat(selected, True), now=0)
        telemetry.feed(heartbeat(foreign, False), now=0)
        telemetry.feed(position(foreign), now=0)
        self.assertEqual(telemetry.source, (1, 1))
        self.assertEqual(dict(telemetry.tick(now=0))[258]["Armed"], True)
        self.assertNotIn("Latitude", dict(telemetry.tick(now=1))[258])
        replacement = Telemetry(cfg())
        self.assertEqual(replacement.feed(heartbeat(foreign), now=2), [(274, False)])
        self.assertEqual(replacement.source, (2, 1))

    def test_bms_only_two_packs_rate_limit_and_stale(self):
        telemetry = Telemetry(cfg())
        mav = sender()
        telemetry.feed(heartbeat(mav), now=0)
        telemetry.feed(bms(mav, 0), now=0)
        single = dict(telemetry.tick(now=0))[257]
        self.assertEqual(single["Battery_VoltageLeft"], 22.8)
        self.assertEqual(single["Battery_Current"], 3.5)
        self.assertEqual(single["Battery_Temperature"], 28)
        self.assertEqual(single["cycleIndex"], 42)
        self.assertEqual(single["flags"], 3)
        telemetry.feed(bms(mav, 1, voltage=23000, current=-120, percent=50), now=0.2)
        self.assertEqual(telemetry.tick(now=0.2), [])
        dual = dict(telemetry.tick(now=1))[257]
        self.assertEqual(dual["Battery_VoltageRight"], 23)
        self.assertEqual(dual["Battery_CurrentRight"], -1.2)
        self.assertNotIn("Battery_Voltage", dual)
        self.assertNotIn("Battery_Remaining", dual)
        telemetry.feed(bms(mav, 7), now=2)
        self.assertNotIn(257, dict(telemetry.tick(now=2)))
        telemetry.feed(bms(mav, 1, percent=90), now=6)
        expired = dict(telemetry.tick(now=6))[257]
        self.assertNotIn("Battery_VoltageLeft", expired)
        self.assertEqual(expired["Battery_RemainingRight"], 90)

    def test_statustext_immediate_and_packs_without_heartbeat_ignored(self):
        telemetry = Telemetry(cfg())
        mav = sender()
        self.assertEqual(telemetry.feed(bms(mav), now=0), [])
        self.assertEqual(telemetry.tick(now=0), [])
        telemetry.feed(heartbeat(mav), now=1)
        status = mav.statustext_encode(4, "电池低".encode("utf-8")).pack(mav)
        self.assertEqual(telemetry.feed(status, now=1),
                         [(260, {"severity": 4, "msg": "电池低"})])

    def test_nonfinite_floats_are_omitted_from_json(self):
        telemetry = Telemetry(cfg())
        mav = sender()
        telemetry.feed(heartbeat(mav), now=0)
        telemetry.feed(mav.attitude_encode(
            1000, float("nan"), 0.25, float("inf"), 0, 0, 0).pack(mav), now=0)
        telemetry.feed(mav.vfr_hud_encode(
            float("nan"), 5, 0, 0, 0, float("inf")).pack(mav), now=0)
        report = dict(telemetry.tick(now=0))[258]
        self.assertNotIn("Roll", report)
        self.assertNotIn("Yaw", report)
        self.assertNotIn("AirSpeed", report)
        self.assertNotIn("ClimbRate", report)
        self.assertAlmostEqual(report["Pitch"], math.degrees(0.25), places=4)
        self.assertEqual(report["GroundSpeed"], 5)
        json.dumps(report, allow_nan=False)


class BridgeTelemetryTests(unittest.TestCase):
    def test_bridge_reports_and_raw_switch(self):
        from test_bridge import FakeMqtt, FakeSerial

        for raw in (True, False):
            with self.subTest(raw=raw):
                conf = cfg(publish_raw_mavlink=raw)
                mqtt = FakeMqtt()
                fake_serial = FakeSerial()
                mav = sender()
                fake_serial.to_read.append(heartbeat(mav, True) + position(mav) + bms(mav))
                bridge = Bridge(conf, mqtt, lambda *args, **kwargs: fake_serial)
                bridge.start()
                bridge._on_connect(mqtt, None, None, 0, None)
                try:
                    deadline = time.monotonic() + 2
                    while len(mqtt.published) < (4 if raw else 3) and time.monotonic() < deadline:
                        time.sleep(0.01)
                    by_type = {json.loads(payload)["DataType"]: (topic, json.loads(payload)["Data"])
                               for topic, payload, _, _ in mqtt.published}
                    self.assertEqual(526 in by_type, raw)
                    self.assertEqual(by_type[274], (conf.receive_topic, True))
                    self.assertEqual(by_type[257][0], conf.receive_topic)
                    self.assertEqual(by_type[258][1]["Latitude"], 31.2304)
                    self.assertEqual(by_type[258][0], conf.receive_topic)
                    # Turning off upstream raw frames does not affect #526 downlink.
                    bridge._on_message(mqtt, None, type("Command", (), {
                        "topic": conf.obtain_topic,
                        "retain": False,
                        "payload": json.dumps({
                            "DataType": 526, "UAVType": conf.uav_type,
                            "SystemID": conf.system_id, "TimeStamp": "2026-09-29T00:00:00+08:00",
                            "Data": "01FF",
                        }).encode(),
                    })())
                    self.assertEqual(fake_serial.written[-1], b"\x01\xff")
                finally:
                    bridge.stop()

    def test_serial_reconnect_rebinds_flight_controller(self):
        from test_bridge import FakeMqtt, FakeSerial

        class DisconnectedSerial(FakeSerial):
            def read(self, size):
                if self.to_read:
                    return self.to_read.pop(0)
                raise OSError("cable disconnected")

        first = DisconnectedSerial()
        first.to_read.append(heartbeat(sender(1), True))
        second = FakeSerial()
        second.to_read.append(heartbeat(sender(2), False))
        opened = []

        def factory(*args, **kwargs):
            opened.append(True)
            return first if len(opened) == 1 else second

        conf = cfg(publish_raw_mavlink=False, reconnect_seconds=0.01)
        mqtt = FakeMqtt()
        bridge = Bridge(conf, mqtt, factory)
        bridge._on_connect(mqtt, None, None, 0, None)
        bridge.start()
        try:
            deadline = time.monotonic() + 2
            while bridge._telemetry.source != (2, 1) and time.monotonic() < deadline:
                time.sleep(0.01)
            self.assertTrue(first.closed)
            self.assertEqual(bridge._telemetry.source, (2, 1))
            armed_events = [json.loads(payload)["Data"] for topic, payload, _, _ in mqtt.published
                            if json.loads(payload)["DataType"] == 274]
            self.assertEqual(armed_events, [True, False])
        finally:
            bridge.stop()


if __name__ == "__main__":
    unittest.main()