meiqi/tienkung-szu
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

新增多设备控制

Browse Source
This commit is contained in:
meiqi
2026-03-30 14:22:53 +08:00
parent 2988d4174b
commit 32702e25f3
7 changed files with 234 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
Deploy_Tienkung/udp_loopback/udp_fsm_controller.py
View File

@@ -72,6 +72,7 @@ class UDPFSMController:
self.udp_flag = UDPFSMFlag()
self.udp_flag.height_cmd = self.initial_lift
self.last_seq_id = -1
self.last_fsm_command_time = 0.0
def start(self) -> None:
self.rx_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
@@ -132,6 +133,14 @@ class UDPFSMController:
flag_copy.__dict__.update(self.udp_flag.__dict__)
return flag_copy
def get_last_input_time(self) -> float:
with self.data_mutex:
return self.motion_frame.last_rx_time
def get_last_fsm_command_time(self) -> float:
with self.data_mutex:
return self.last_fsm_command_time
def init(self) -> int:
print("UDP FSM controller initialized")
return 0
@@ -145,14 +154,19 @@ class UDPFSMController:
event_code = packet.event_code
if event_code == "pose_home":
self.motion_frame.mode_tag = "pose_home"
self.last_fsm_command_time = packet.sent_at
elif event_code == "pose_hold":
self.motion_frame.mode_tag = "pose_hold"
self.last_fsm_command_time = packet.sent_at
elif event_code == "mode_stride":
self.motion_frame.mode_tag = "mode_stride"
self.last_fsm_command_time = packet.sent_at
elif event_code == "mode_dash":
self.motion_frame.mode_tag = "mode_dash"
self.last_fsm_command_time = packet.sent_at
elif event_code == "mode_xrun":
self.motion_frame.mode_tag = "mode_xrun"
self.last_fsm_command_time = packet.sent_at
elif event_code == "surge_up":
self.motion_frame.surge_goal = min(
self.max_surge, self.motion_frame.surge_goal + self.surge_step

41
Deploy_Tienkung/udp_loopback/udp_keyboard_sender.py
View File

@@ -64,6 +64,9 @@ class UDPKeyboardSender:
print(" Left/Right -> lift +/-")
print(" Up/Down -> surge +/-")
print(" r -> trim_reset")
print(" 4 -> clear x speed")
print(" 5 -> clear y speed")
print(" 6 -> clear yaw speed")
print(" x -> session_quit")
def start(self) -> None:
@@ -106,19 +109,22 @@ class UDPKeyboardSender:
def _process_key(self, key: str) -> None:
event_map = {
"w": ("surge_up", "w"),
"s": ("surge_down", "s"),
"a": ("sway_left", "a"),
"d": ("sway_right", "d"),
"q": ("spin_left", "q"),
"e": ("spin_right", "e"),
"z": ("pose_home", "z"),
"c": ("pose_hold", "c"),
"m": ("mode_stride", "m"),
"p": ("mode_dash", "p"),
"n": ("mode_xrun", "n"),
"r": ("trim_reset", "r"),
"x": ("session_quit", "x"),
"w": ("surge_up", "w", 1.0),
"s": ("surge_down", "s", 1.0),
"a": ("sway_left", "a", 1.0),
"d": ("sway_right", "d", 1.0),
"q": ("spin_left", "q", 1.0),
"e": ("spin_right", "e", 1.0),
"z": ("pose_home", "z", 1.0),
"c": ("pose_hold", "c", 1.0),
"m": ("mode_stride", "m", 1.0),
"p": ("mode_dash", "p", 1.0),
"n": ("mode_xrun", "n", 1.0),
"r": ("trim_reset", "r", 1.0),
"4": ("set_surge", "4", 0.0),
"5": ("set_sway", "5", 0.0),
"6": ("set_spin", "6", 0.0),
"x": ("session_quit", "x", 1.0),
}
if key == "\x03":
@@ -130,8 +136,8 @@ class UDPKeyboardSender:
return
if key in event_map:
event_code, key_name = event_map[key]
self._send_event(event_code, key_name)
event_code, key_name, drive_value = event_map[key]
self._send_event(event_code, key_name, drive_value=drive_value)
def _handle_arrow_key(self) -> None:
if not select.select([sys.stdin], [], [], 0.1)[0]:
@@ -152,13 +158,16 @@ class UDPKeyboardSender:
event_code, key_name = arrow_map[key3]
self._send_event(event_code, key_name)
def _send_event(self, event_code: str, key_name: str) -> None:
def _send_event(
self, event_code: str, key_name: str, drive_value: float = 1.0
) -> None:
if self.socket is None:
return
envelope = InputEnvelope(
seq_id=self.seq_id,
event_code=event_code,
key_name=key_name,
drive_value=drive_value,
source_tag=self.source_tag,
)
self.seq_id += 1