import math import unittest from motian_bridge import Bridge from motian_bridge.dialects.v20 import VKFly 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) if __name__ == "__main__": unittest.main()