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()