| | |
|---|
| | | 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 (但模拟器这里主要是为了闭环，不需要完美匹配Bias，只要方向对) |
|---|
| | | 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 |
|---|
| | | |
|---|
| | | |
|---|
| | | |
|---|
| | |
|---|
| | | 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], |
|---|
| | |
|---|
| | | _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]) |
|---|
| | |
|---|
| | | 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)}") |
|---|
