新增多设备控制

Deploy_Tienkung/common/stdin_keyboard_control.py

@@ -8,6 +8,7 @@ import select
 import termios
 import tty
 import os
+import time
 import yaml
 from typing import Optional
 from .joystick import ControlFlag
@@ -50,6 +51,8 @@ class KeyboardController:
         self.running = False
         self.input_thread = None
         self.original_terminal_settings = None
+        self.last_input_time = 0.0
+        self.last_fsm_command_time = 0.0
         
         # 加载配置文件
         self._load_config()
@@ -166,6 +169,7 @@ class KeyboardController:
     
     def _process_key(self, key):
         """处理按键输入"""
+        handled = True
         if key == 'w':
             self._on_w_key()
         elif key == 's':
@@ -207,7 +211,10 @@ class KeyboardController:
             self._handle_arrow_key()
         else:
             # 忽略其他按键
-            pass
+            handled = False
+
+        if handled:
+            self.last_input_time = time.time()
     
     def _handle_arrow_key(self):
         """处理方向键序列"""
@@ -296,17 +303,20 @@ class KeyboardController:
         """处理z键 - 切换到ZERO状态"""
         with self.data_mutex:
             self.keyboard_flag.fsm_state_command = "gotoZERO"
+            self.last_fsm_command_time = time.time()
             print("Command: gotoZERO")
     
     def _on_v_key(self):
         """处理v键 - 切换到BEYONGDMIMIC状态"""
         with self.data_mutex:
             self.keyboard_flag.fsm_state_command = "gotoBEYONDMIMIC"
+            self.last_fsm_command_time = time.time()
             print("Command: gotoBEYONDMIMIC")
     def _on_c_key(self):
         """处理c键 - 切换到STOP状态"""
         with self.data_mutex:
             self.keyboard_flag.fsm_state_command = "gotoSTOP"
+            self.last_fsm_command_time = time.time()
             print("Command: gotoSTOP")
             
             
@@ -346,6 +356,7 @@ class KeyboardController:
         """处理m键 - 切换到WALKAMP状态"""
         with self.data_mutex:
             self.keyboard_flag.fsm_state_command = "gotoWALKAMP"
+            self.last_fsm_command_time = time.time()
             print("Command: gotoWALKAMP")
 
     def _on_p_key(self):
@@ -355,6 +366,7 @@ class KeyboardController:
             self.keyboard_flag.y_speed_command = 0.0
             self.keyboard_flag.yaw_speed_command = 0.0
             self.keyboard_flag.fsm_state_command = "gotoMYPOLICY"
+            self.last_fsm_command_time = time.time()
             print("Command: gotoMYPOLICY (movement commands reset to zero)")
 
     def _on_n_key(self):
@@ -364,6 +376,7 @@ class KeyboardController:
             self.keyboard_flag.y_speed_command = 0.0
             self.keyboard_flag.yaw_speed_command = 0.0
             self.keyboard_flag.fsm_state_command = "gotoXSIMRUN"
+            self.last_fsm_command_time = time.time()
             print("Command: gotoXSIMRUN (movement commands reset to zero)")
     
     def _handle_ctrl_c(self):
@@ -418,6 +431,12 @@ class KeyboardController:
             flag_copy = KeyboardFlag()
             flag_copy.__dict__.update(self.keyboard_flag.__dict__)
             return flag_copy
+
+    def get_last_input_time(self) -> float:
+        return self.last_input_time
+
+    def get_last_fsm_command_time(self) -> float:
+        return self.last_fsm_command_time
             
     def init(self) -> int:
         """初始化键盘控制器"""