#!/usr/bin/env python3
|
"""
|
调试航向控制的符号定义
|
根据实际IMU定义：GyroZ正值 = 顺时针(CW) = 右转 = 罗盘角增加
|
"""
|
import math
|
|
# 从log中提取的数据
|
pos_x, pos_y = 1.89, -2.81 # ENU坐标
|
tgt_x, tgt_y = 5.40, -7.17
|
current_hdg_compass = 308.13 # 罗盘角度
|
turn_output = 0.80 # log中显示的turn值（正值）
|
|
print("=" * 60)
|
print("实际测试数据分析")
|
print("=" * 60)
|
print(f"当前位置(ENU): ({pos_x:.2f}, {pos_y:.2f})")
|
print(f"目标位置(ENU): ({tgt_x:.2f}, {tgt_y:.2f})")
|
print(f"当前航向(罗盘): {current_hdg_compass:.1f}°")
|
print(f"Log显示turn: {turn_output:.2f} (正值)")
|
print()
|
|
# 1. 计算目标航向
|
dx = tgt_x - pos_x # 东方向
|
dy = tgt_y - pos_y # 北方向
|
dist = math.sqrt(dx**2 + dy**2)
|
|
target_math_rad = math.atan2(dy, dx)
|
target_math_deg = math.degrees(target_math_rad)
|
|
# 转换为罗盘角度
|
target_compass_deg = 90.0 - target_math_deg
|
if target_compass_deg < 0:
|
target_compass_deg += 360
|
if target_compass_deg >= 360:
|
target_compass_deg -= 360
|
|
print("=" * 60)
|
print("步骤1: 计算目标航向")
|
print("=" * 60)
|
print(f"位移向量: dx={dx:.2f}m(东), dy={dy:.2f}m(北), dist={dist:.2f}m")
|
print(f"目标航向(数学坐标系): {target_math_deg:.1f}° = {target_math_rad:.3f} rad")
|
print(f"目标航向(罗盘坐标系): {target_compass_deg:.1f}°")
|
print()
|
|
# 2. 模拟motion_control.c中的转换
|
# mc_compass_deg_to_math_rad: math_deg = 90.0f - heading_deg
|
current_math_deg = 90.0 - current_hdg_compass
|
current_math_rad = math.radians(current_math_deg)
|
|
# 归一化到-π~π
|
def wrap_angle(angle_rad):
|
while angle_rad > math.pi:
|
angle_rad -= 2 * math.pi
|
while angle_rad < -math.pi:
|
angle_rad += 2 * math.pi
|
return angle_rad
|
|
current_math_rad = wrap_angle(current_math_rad)
|
target_math_rad_wrapped = wrap_angle(target_math_rad)
|
|
print("=" * 60)
|
print("步骤2: 罗盘角→数学坐标系（motion_control.c第114行）")
|
print("=" * 60)
|
print(f"math_deg = 90.0 - heading_deg = 90.0 - {current_hdg_compass:.1f} = {current_math_deg:.1f}°")
|
print(f"current_math_rad = {current_math_rad:.3f} rad = {math.degrees(current_math_rad):.1f}°")
|
print(f"target_math_rad = {target_math_rad_wrapped:.3f} rad = {math.degrees(target_math_rad_wrapped):.1f}°")
|
print()
|
|
# 3. 计算航向误差（数学坐标系）
|
heading_err_rad = target_math_rad_wrapped - current_math_rad
|
heading_err_rad = wrap_angle(heading_err_rad)
|
heading_err_deg = math.degrees(heading_err_rad)
|
|
print("=" * 60)
|
print("步骤3: 计算航向误差（motion_control.c第43行）")
|
print("=" * 60)
|
print(f"heading_err = target - current")
|
print(f" = {target_math_rad_wrapped:.3f} - {current_math_rad:.3f}")
|
print(f" = {heading_err_rad:.3f} rad")
|
print(f" = {heading_err_deg:.1f}°")
|
if heading_err_rad > 0:
|
print("误差为正 → 数学坐标系中需要逆时针转(CCW)")
|
print(" → 罗盘坐标系中需要顺时针转(从西北转向东南)")
|
else:
|
print("误差为负 → 数学坐标系中需要顺时针转(CW)")
|
print(" → 罗盘坐标系中需要逆时针转")
|
print()
|
|
# 4. 计算yaw_rate_cmd（假设Kp=1，简化）
|
Kp = 1.0
|
max_turn_rate = 0.8
|
yaw_rate_cmd = Kp * heading_err_rad
|
yaw_rate_cmd = max(-max_turn_rate, min(max_turn_rate, yaw_rate_cmd))
|
|
print("=" * 60)
|
print("步骤4: 计算yaw_rate_cmd（motion_control.c第247行）")
|
print("=" * 60)
|
print(f"yaw_rate_cmd = Kp × heading_err")
|
print(f" = {Kp:.1f} × {heading_err_rad:.3f}")
|
print(f" = {yaw_rate_cmd:.3f} rad/s")
|
print(f"限制到±{max_turn_rate}: yaw_rate_cmd = {yaw_rate_cmd:.3f} rad/s")
|
print()
|
|
# 5. motion_control.c第548行：取反
|
turn_rate_before_invert = yaw_rate_cmd
|
turn_rate_after_invert = -yaw_rate_cmd
|
|
print("=" * 60)
|
print("步骤5: 第548行取反（motion_control.c）")
|
print("=" * 60)
|
print(f"_out->turn_rate = -yaw_rate_cmd")
|
print(f" = -{yaw_rate_cmd:.3f}")
|
print(f" = {turn_rate_after_invert:.3f} rad/s")
|
print()
|
|
# 6. 对比log中的值
|
print("=" * 60)
|
print("步骤6: 对比实际log输出")
|
print("=" * 60)
|
print(f"计算的turn_rate: {turn_rate_after_invert:.3f} rad/s")
|
print(f"Log显示turn: {turn_output:.2f} rad/s")
|
print()
|
|
if (turn_rate_after_invert > 0 and turn_output > 0) or (turn_rate_after_invert < 0 and turn_output < 0):
|
print("✓ 符号一致")
|
else:
|
print("✗ 符号不一致！需要检查代码")
|
print()
|
|
# 7. PWM映射分析
|
print("=" * 60)
|
print("步骤7: PWM映射（motion_control_task.c第469-487行）")
|
print("=" * 60)
|
print("注释说：正值=左转，负值=右转")
|
print(f"turn_rate = {turn_rate_after_invert:.3f} rad/s")
|
|
if turn_rate_after_invert > 0:
|
print("→ 正值 → 应该左转（逆时针，罗盘角减少）")
|
print("→ PWM < 1500")
|
elif turn_rate_after_invert < 0:
|
print("→ 负值 → 应该右转（顺时针，罗盘角增加）")
|
print("→ PWM > 1500")
|
print()
|
|
# 8. 实际车辆行为
|
print("=" * 60)
|
print("步骤8: 实际车辆行为")
|
print("=" * 60)
|
print(f"Log显示航向从308°增加到344° → 顺时针转（右转）")
|
print(f"目标航向141°")
|
print(f"正确的转向：308° → 逆时针 → 141°（转167°）")
|
print(f"实际转向：顺时针转")
|
print()
|
print("结论：转向方向反了！")
|
print()
|
|
# 9. 问题诊断
|
print("=" * 60)
|
print("问题诊断")
|
print("=" * 60)
|
print("可能的原因：")
|
print("1. 第548行的取反导致符号错误")
|
print("2. PWM映射的符号定义与注释不符")
|
print("3. 航向误差计算有问题")
|
print()
|
print("建议：")
|
print("1. 去掉motion_control.c第548行的取反")
|
print("2. 或者修改motion_control_task.c的PWM映射符号")
|
print("3. 或者检查mc_compass_deg_to_math_rad转换是否正确")
|
