#!/usr/bin/env python3
|
"""分析每个前进档位的实际速度"""
|
import math
|
|
log_file = "python/tools/calibration_20251204_171502.log"
|
|
# 解析所有$CAL数据
|
data_points = []
|
with open(log_file, 'r', encoding='utf-8', errors='ignore') as f:
|
for line in f:
|
if line.startswith('$CAL'):
|
parts = line.strip().split(',')
|
if len(parts) >= 12:
|
data_points.append({
|
'seq': int(parts[1]),
|
'time_ms': int(parts[2]),
|
'state': parts[3],
|
'throttle': int(parts[4]),
|
'steering': int(parts[5]),
|
'x': float(parts[6]),
|
'y': float(parts[7])
|
})
|
|
print("=" * 80)
|
print("前进测试各档位平均速度分析")
|
print("=" * 80)
|
|
# 定义需要分析的cruise状态
|
cruise_states = [
|
'FWD_VLOW_C',
|
'FWD_LOW_C',
|
'FWD_MID_C',
|
'FWD_HIGH_C',
|
'FWD_VHIGH_C',
|
'FWD_MAX_C'
|
]
|
|
for state in cruise_states:
|
# 提取该状态的所有数据点
|
state_data = [d for d in data_points if d['state'] == state]
|
|
if len(state_data) < 2:
|
print(f"\n{state:15s}: 数据不足")
|
continue
|
|
# 获取起止点
|
start = state_data[0]
|
end = state_data[-1]
|
|
# 计算总距离
|
dx = end['x'] - start['x']
|
dy = end['y'] - start['y']
|
distance = math.sqrt(dx*dx + dy*dy)
|
|
# 计算总时间
|
time_s = (end['time_ms'] - start['time_ms']) / 1000.0
|
|
# 计算平均速度
|
avg_speed = distance / time_s if time_s > 0 else 0
|
|
# 获取PWM值（应该是恒定的）
|
pwm = start['throttle']
|
|
# 检查是否有GPS跳变
|
max_jump = 0
|
for i in range(1, len(state_data)):
|
dx_step = state_data[i]['x'] - state_data[i-1]['x']
|
dy_step = state_data[i]['y'] - state_data[i-1]['y']
|
jump = math.sqrt(dx_step*dx_step + dy_step*dy_step)
|
if jump > max_jump:
|
max_jump = jump
|
|
# 标记数据质量
|
quality = "✓" if max_jump < 1.0 else "⚠ GPS跳变"
|
|
print(f"\n{state:15s} (PWM={pwm}):")
|
print(f" 起点: ({start['x']:6.2f}, {start['y']:6.2f}) m")
|
print(f" 终点: ({end['x']:6.2f}, {end['y']:6.2f}) m")
|
print(f" 总距离: {distance:6.2f} m")
|
print(f" 总时间: {time_s:5.1f} s")
|
print(f" 平均速度: {avg_speed:5.3f} m/s {quality}")
|
print(f" 最大单步跳变: {max_jump:5.2f} m")
|
|
print("\n" + "=" * 80)
|
print("数据汇总（仅可信数据）")
|
print("=" * 80)
|
print(f"{'PWM':>6s} {'状态':>15s} {'速度(m/s)':>10s} {'质量':>8s}")
|
print("-" * 80)
|
|
for state in cruise_states:
|
state_data = [d for d in data_points if d['state'] == state]
|
if len(state_data) < 2:
|
continue
|
|
start = state_data[0]
|
end = state_data[-1]
|
|
dx = end['x'] - start['x']
|
dy = end['y'] - start['y']
|
distance = math.sqrt(dx*dx + dy*dy)
|
time_s = (end['time_ms'] - start['time_ms']) / 1000.0
|
avg_speed = distance / time_s if time_s > 0 else 0
|
pwm = start['throttle']
|
|
# 检查最大跳变
|
max_jump = 0
|
for i in range(1, len(state_data)):
|
dx_step = state_data[i]['x'] - state_data[i-1]['x']
|
dy_step = state_data[i]['y'] - state_data[i-1]['y']
|
jump = math.sqrt(dx_step*dx_step + dy_step*dy_step)
|
if jump > max_jump:
|
max_jump = jump
|
|
quality = "可信" if max_jump < 1.0 else "跳变"
|
marker = " ✓" if max_jump < 1.0 else " ✗"
|
|
print(f"{pwm:6d} {state:>15s} {avg_speed:10.3f} {quality:>8s}{marker}")
|
|
print("=" * 80)
|
