motian_mavbridge/tests/test_commands.py

import math
import unittest

from motian_bridge import Bridge
from motian_bridge.dialects.v20 import VKFly
from motian_bridge.telemetry import Telemetry
from test_bridge import FakeMqtt, FakeSerial, command, config


def heartbeat(system=1, component=1):
    mav = VKFly.MAVLink(None, srcSystem=system, srcComponent=component)
    return mav.heartbeat_encode(
        VKFly.MAV_TYPE_QUADROTOR,
        VKFly.MAV_AUTOPILOT_GENERIC,
        0,
        VKFly.VKFLY_CUSTOM_MODE_POSHOLD,
        VKFly.MAV_STATE_ACTIVE,
    ).pack(mav)


def decode(frame):
    parser = VKFly.MAVLink(None)
    result = None
    for byte in frame:
        message = parser.parse_char(bytes((byte,)))
        if message is not None:
            result = message
    return result


class CommandHandlerTests(unittest.TestCase):
    def setUp(self):
        self.cfg = config()
        self.mqtt = FakeMqtt()
        self.serial = FakeSerial()
        self.bridge = Bridge(self.cfg, self.mqtt, lambda *args, **kwargs: self.serial)
        self.bridge._serial = self.serial
        self.bridge._on_connect(self.mqtt, None, None, 0, None)
        self.bridge._telemetry.feed(heartbeat(42, 1), now=1)

    def deliver(self, data_type, data):
        self.bridge._on_message(self.mqtt, None, type("Message", (), {
            "topic": self.cfg.obtain_topic,
            "retain": False,
            "payload": command(data_type, data),
        })())

    def last(self):
        return decode(self.serial.written[-1])

    def test_get_all_params(self):
        self.deliver(281, None)
        message = self.last()
        self.assertEqual(message.get_type(), "PARAM_REQUEST_LIST")
        self.assertEqual((message.target_system, message.target_component), (42, 1))

    def test_direct_parameter_mappings(self):
        cases = [
            ("battery_low_threshold", 30, "VCAP1_LOW_VAL", VKFly.MAV_PARAM_TYPE_UINT8, 30),
            ("battery_critical_threshold", 20, "VCAP2_LOW_VAL", VKFly.MAV_PARAM_TYPE_UINT8, 20),
            ("height_limit", 120, "ALT_LIM_UP1", VKFly.MAV_PARAM_TYPE_UINT16, 120),
            ("distance_limit", 5000, "MAX_HOR_DIST", VKFly.MAV_PARAM_TYPE_UINT32, 5000),
            ("return_height", 80, "RTL_ALT_M", VKFly.MAV_PARAM_TYPE_REAL32, 80),
            ("return_speed", 8, "MC_XY_CRUISE", VKFly.MAV_PARAM_TYPE_REAL32, 8),
            ("obstacle_avoidance_distance", 5, "OBAVOID_DIST",
             VKFly.MAV_PARAM_TYPE_REAL32, 5),
            ("obstacle_avoidance_enable", 1, "OBAVOID_ACT",
             VKFly.MAV_PARAM_TYPE_UINT8, 1),
            ("obstacle_avoidance_switch", 0, "OBAVOID_ACT",
             VKFly.MAV_PARAM_TYPE_UINT8, 0),
        ]
        for name, value, expected_id, expected_type, expected_value in cases:
            with self.subTest(name=name):
                self.serial.written.clear()
                self.deliver(282, {"name": name, "type": 1, "value": value})
                message = self.last()
                self.assertEqual(message.get_type(), "PARAM_SET")
                self.assertEqual(message.param_id, expected_id)
                self.assertEqual(message.param_type, expected_type)
                self.assertEqual(message.param_value, expected_value)

    def test_string_parameter_mappings(self):
        self.deliver(282, {"name": "return_type", "type": 2, "value": "MissionReverse"})
        self.assertEqual(self.last().param_id, "RTL_EXEC_MODE")
        self.assertEqual(self.last().param_value, 2)
        self.deliver(282, {"name": "precision_land_mode", "type": 2,
                           "value": "AprilTag"})
        self.assertEqual(self.last().param_id, "RTL_PLD_EN")
        self.assertEqual(self.last().param_value, 1)

    def test_todo_and_third_threshold_do_not_write(self):
        for name in ("battery_emergency_threshold", "geofence_action", "rtk_switch",
                     "visual_sensor_enable", "unknown"):
            with self.subTest(name=name):
                before = len(self.serial.written)
                self.deliver(282, {"name": name, "type": 1, "value": 1})
                self.assertEqual(len(self.serial.written), before)

    def test_low_battery_action_preserves_other_bits(self):
        self.bridge._telemetry.params["FS_CONF_A"] = float(0xAB00)
        self.deliver(282, {"name": "low_battery_action", "type": 2,
                           "value": "ReturnHome"})
        message = self.last()
        self.assertEqual(message.param_id, "FS_CONF_A")
        self.assertEqual(message.param_type, VKFly.MAV_PARAM_TYPE_UINT32)
        self.assertEqual(message.param_value, 0xAB22)

    def test_low_battery_action_requires_parameter_download(self):
        before = len(self.serial.written)
        self.deliver(282, {"name": "low_battery_action", "type": 2,
                           "value": "ReturnHome"})
        self.assertEqual(len(self.serial.written), before)

    def test_param_value_is_cached(self):
        mav = VKFly.MAVLink(None, srcSystem=42, srcComponent=1)
        frame = mav.param_value_encode(
            b"FS_CONF_A", 4660, VKFly.MAV_PARAM_TYPE_UINT32, 1, 0
        ).pack(mav)
        self.bridge._telemetry.feed(frame, now=2)
        self.assertEqual(self.bridge.mavlink_param("FS_CONF_A"), 4660)

    def test_2050_false_arms_and_true_is_todo(self):
        self.deliver(2050, False)
        message = self.last()
        self.assertEqual(message.get_type(), "COMMAND_LONG")
        self.assertEqual(message.command, VKFly.MAV_CMD_COMPONENT_ARM_DISARM)
        self.assertEqual(message.param1, 1)
        before = len(self.serial.written)
        self.deliver(2050, True)
        self.assertEqual(len(self.serial.written), before)

    def test_takeoff_and_land(self):
        self.deliver(2054, None)
        takeoff = self.last()
        self.assertEqual(takeoff.command, VKFly.MAV_CMD_NAV_TAKEOFF)
        self.assertTrue(math.isnan(takeoff.param7))
        self.deliver(2055, None)
        land = self.last()
        self.assertEqual(land.command, VKFly.MAV_CMD_NAV_LAND)

    def test_reposition_command_int(self):
        self.deliver(2070, {
            "latitude": 33.3180863,
            "longitude": 120.0978796,
            "altitude": 61.52,
            "speed": 5.0,
            "heading": 30,
        })
        message = self.last()
        self.assertEqual(message.get_type(), "COMMAND_INT")
        self.assertEqual(message.command, VKFly.MAV_CMD_DO_REPOSITION)
        self.assertEqual(message.frame, VKFly.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT)
        self.assertEqual(message.param2, VKFly.MAV_DO_REPOSITION_FLAGS_CHANGE_MODE)
        self.assertEqual(message.x, 333180863)
        self.assertEqual(message.y, 1200978796)
        self.assertAlmostEqual(message.z, 61.52, places=4)

    def test_commands_wait_for_heartbeat(self):
        bridge = Bridge(self.cfg, self.mqtt, lambda *args, **kwargs: self.serial)
        bridge._serial = self.serial
        bridge._on_connect(self.mqtt, None, None, 0, None)
        before = len(self.serial.written)
        bridge._on_message(self.mqtt, None, type("Message", (), {
            "topic": self.cfg.obtain_topic,
            "retain": False,
            "payload": command(2054, None),
        })())
        self.assertEqual(len(self.serial.written), before)


class ParameterSyncTests(unittest.TestCase):
    def setUp(self):
        self.cfg = config(param_request_timeout=1.0, param_request_max_attempts=2)
        self.mqtt = FakeMqtt()
        self.serial = FakeSerial()
        self.bridge = Bridge(self.cfg, self.mqtt, lambda *args, **kwargs: self.serial)
        self.bridge._serial = self.serial
        self.bridge._on_connect(self.mqtt, None, None, 0, None)
        self.bridge._telemetry.feed(heartbeat(42, 1), now=0)

    def request_lists(self):
        messages = [decode(frame) for frame in self.serial.written]
        return [message for message in messages if message.get_type() == "PARAM_REQUEST_LIST"]

    def test_auto_request_once_targets_locked_controller(self):
        self.bridge._sync_parameters(now=0)
        frames = self.request_lists()
        self.assertEqual(len(frames), 1)
        self.assertEqual((frames[0].target_system, frames[0].target_component), (42, 1))
        self.bridge._sync_parameters(now=0.1)
        self.assertEqual(len(self.serial.written), 1)

    def test_disabled_config_never_requests(self):
        bridge = Bridge(config(request_params_on_start=False), self.mqtt,
                        lambda *args, **kwargs: self.serial)
        bridge._serial = self.serial
        bridge._telemetry.feed(heartbeat(42, 1), now=0)
        bridge._sync_parameters(now=0)
        self.assertEqual(self.serial.written, [])

    def test_retry_until_max_attempts_then_give_up(self):
        self.bridge._sync_parameters(now=0)
        mav = VKFly.MAVLink(None, srcSystem=42, srcComponent=1)
        self.bridge._telemetry.feed(mav.param_value_encode(
            b"P", 1.0, VKFly.MAV_PARAM_TYPE_REAL32, 3, 0).pack(mav), now=0.1)
        self.bridge._sync_parameters(now=0.5)   # timeout not reached yet
        self.assertEqual(len(self.serial.written), 1)
        self.bridge._sync_parameters(now=1.0)   # second attempt
        self.assertEqual(len(self.serial.written), 2)
        self.bridge._sync_parameters(now=2.0)   # attempts exhausted
        self.assertEqual(len(self.serial.written), 2)

    def test_complete_table_stops_retry(self):
        self.bridge._sync_parameters(now=0)
        mav = VKFly.MAVLink(None, srcSystem=42, srcComponent=1)
        self.bridge._telemetry.feed(mav.param_value_encode(
            b"P", 1.0, VKFly.MAV_PARAM_TYPE_REAL32, 1, 0).pack(mav), now=0.1)
        self.assertTrue(self.bridge._telemetry.aircraft.params.complete())
        self.bridge._sync_parameters(now=99.0)
        self.assertEqual(len(self.serial.written), 1)

    def test_new_aircraft_requests_again(self):
        self.bridge._sync_parameters(now=0)
        self.assertEqual(len(self.serial.written), 1)
        self.bridge._telemetry = Telemetry(self.cfg)
        self.bridge._telemetry.feed(heartbeat(7, 1), now=0)
        self.bridge._sync_parameters(now=0)
        self.assertEqual(len(self.serial.written), 2)
        self.assertEqual(decode(self.serial.written[-1]).target_system, 7)


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