From 567085ead3f6adaabd884f16ab4b17c62e8f0403 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期日, 21 十二月 2025 22:28:09 +0800
Subject: [PATCH] OTA升级功能调通，准备增加boot的代码
---
python/hitl/protocols.py | 54 +++++++++++++++++++++++++++++++++++++++++++++++++-----
1 files changed, 49 insertions(+), 5 deletions(-)
diff --git a/python/hitl/protocols.py b/python/hitl/protocols.py
index a542018..5bb8fbd 100644
--- a/python/hitl/protocols.py
+++ b/python/hitl/protocols.py
@@ -13,8 +13,52 @@
from datetime import datetime, timezone
from typing import Callable, Optional, Tuple
def _pwm_to_velocity(value: int, center: int = 1500, span: int = 500, max_speed: float = 1.5) -> float:
- ratio = _clamp((value - center) / span, -1.0, 1.0)
- return ratio * max_speed
+ """
+ 灏� PWM 淇″彿杞崲涓虹嚎閫熷害鎴栬閫熷害銆�
+ 鍩轰簬 motion_config.h 涓殑妯″瀷鍙傛暟锛�
+ Forward: v = K_fwd * (1500 - pwm) + Bias (浣嗘ā鎷熷櫒杩欓噷涓昏鏄负浜嗛棴鐜紝涓嶉渶瑕佸畬缇庡尮閰岯ias锛屽彧瑕佹柟鍚戝)
+ 1000 PWM = 0.56 m/s
+ 1500 PWM = 0.0 m/s
+ 2000 PWM = -0.2 m/s (backward)
+ Turn: w = K_turn * (1500 - pwm)
+ 1000 PWM = 63 deg/s (Left)
+ 2000 PWM = -61.5 deg/s (Right)
+ """
+ if value < 1000: value = 1000
+ if value > 2000: value = 2000
+
+ # 浣跨敤鍙傛暟
+ # MC_CFG_FORWARD_K = 0.00196
+ # MC_CFG_FORWARD_BIAS = -0.077
+ # MC_CFG_STEERING_K_LEFT = 0.00206
+ # MC_CFG_STEERING_K_RIGHT = 0.00201
+
+ # 绠�鍗曠殑绾挎�ф槧灏勭敤浜庢ā鎷燂紝蹇界暐姝诲尯鍜屽亸缃殑缁嗚妭锛屼繚璇佸ぇ鑷磋寖鍥存纭嵆鍙�
+ # 鐪熷疄閫熷害 = K * (1500 - pwm) + Bias
+
+ delta = 1500 - value
+ # 妯℃嫙鍣ㄩ渶瑕佽繑鍥為�熷害鍊硷紝鑰屼笉鏄缃�熷害銆�
+ # 杩欓噷鐨� max_speed 鍙傛暟涓嶅啀鐩存帴浣滀负姣斾緥绯绘暟锛岃�屾槸鐢ㄤ簬鎸囩ず鏄嚎閫熷害杩樻槸瑙掗�熷害鐨勭缉鏀�
+
+ if max_speed < 10.0: # 绾块�熷害妯″紡 (m/s)
+ # 鍋囪鏄� Forward PWM
+ # 1000 -> 500 diff -> 0.56 m/s => 0.00112 m/s/us
+ # 2000 -> -500 diff -> -0.56 m/s (瀹為檯杞﹀彲鑳芥槸0.2锛屼絾妯℃嫙鍣ㄥ彲浠ュ绉�)
+ # 浣跨敤骞冲潎 K = 0.0015
+ k_lin = 0.0015
+ if abs(delta) < 40: return 0.0 # 姝诲尯
+ return delta * k_lin
+ else: # 瑙掗�熷害妯″紡 (deg/s -> rad/s)
+ # 鍋囪鏄� Steering PWM
+ # 1000 -> 500 diff -> 63 deg/s => 0.126 deg/s/us
+ # 2000 -> -500 diff -> -61.5 deg/s
+ # K_ang ~ 0.12 deg/s/us
+ k_ang_deg = 0.12
+ if abs(delta) < 10: return 0.0 # 姝诲尯
+ deg_s = delta * k_ang_deg
+ # 杞崲涓� rad/s ? 涓嬮潰璋冪敤鏄� math.radians(_pwm_to_velocity(steering_pwm, max_speed=90.0))
+ # 鎵�浠ヨ繖閲屽彧闇�瑕佽繑鍥� deg/s锛岃皟鐢ㄨ�呬細杞� rad/s
+ return deg_s
@@ -68,7 +112,7 @@
east_vel: float,
north_vel: float,
up_vel: float,
- heading_deg: float,
+ heading_rad: float,
pitch_deg: float,
roll_deg: float,
accel_bias: Tuple[float, float, float],
@@ -86,7 +130,7 @@
_write_float(buf, -up_vel) # IM23A 鍚戜笅姝ｏ紝MCU 鍐呴儴杞崲鍥炲悜涓�
_write_float(buf, roll_deg)
_write_float(buf, pitch_deg)
- _write_float(buf, heading_deg)
+ _write_float(buf, heading_rad) # 鑸悜瑙掞細寮у害鍊硷紝鑼冨洿-3.14鍒�3.14锛�0搴︽鍖楁柟鍚�
_write_float(buf, 0.02) # 瀹氫綅绮惧害锛屽彲鏍规嵁闇�瑕佽皟鏁�
_write_float(buf, accel_bias[0])
_write_float(buf, accel_bias[1])
@@ -138,7 +182,7 @@
if len(payload) == 4:
steering_pwm, throttle_pwm = struct.unpack("<HH", payload)
# 閲嶆柊鏄犲皠锛氳浆鍚� PWM 鈫� yaw rate锛涙补闂� PWM 鈫� 绾块�熷害
- forward = _pwm_to_velocity(throttle_pwm)
+ forward = _pwm_to_velocity(throttle_pwm, max_speed=1.0)
turn = math.radians(_pwm_to_velocity(steering_pwm, max_speed=90.0))
return forward, turn
raise ValueError(f"涓嶆敮鎸佺殑鎺у埗璐熻浇闀垮害: {len(payload)}")
--
Gitblit v1.10.0