#!/usr/bin/env python3
|
"""检查GPS数据是否有异常跳变"""
|
import sys
|
import math
|
|
log_file = sys.argv[1] if len(sys.argv) > 1 else "calibration_20251204_171502.log"
|
|
# 读取所有巡航状态数据
|
cruise_states = ['FWD_VLOW_C', 'FWD_LOW_C', 'FWD_MID_C', 'FWD_HIGH_C', 'FWD_VHIGH_C', 'FWD_MAX_C']
|
|
for state_name in cruise_states:
|
data = []
|
with open(log_file, 'r', encoding='utf-8', errors='ignore') as f:
|
for line in f:
|
if state_name in line and line.startswith('$CAL'):
|
parts = line.strip().split(',')
|
if len(parts) >= 12:
|
data.append({
|
'time': int(parts[2]),
|
'pwm': int(parts[4]),
|
'x': float(parts[6]),
|
'y': float(parts[7]),
|
'hdg': float(parts[9])
|
})
|
|
if len(data) < 10:
|
continue
|
|
print(f"\n========== {state_name} (PWM {data[0]['pwm']}) ==========")
|
|
# 检查跳变
|
max_jump = 0
|
max_speed = 0
|
for i in range(1, len(data)):
|
dx = data[i]['x'] - data[i-1]['x']
|
dy = data[i]['y'] - data[i-1]['y']
|
dt = (data[i]['time'] - data[i-1]['time']) / 1000.0
|
|
dist = math.sqrt(dx*dx + dy*dy)
|
if dt > 0:
|
speed = dist / dt
|
if speed > max_speed:
|
max_speed = speed
|
if dist > max_jump:
|
max_jump = dist
|
|
# 计算总体平均速度
|
total_dx = data[-1]['x'] - data[0]['x']
|
total_dy = data[-1]['y'] - data[0]['y']
|
total_dist = math.sqrt(total_dx*total_dx + total_dy*total_dy)
|
total_time = (data[-1]['time'] - data[0]['time']) / 1000.0
|
avg_speed = total_dist / total_time if total_time > 0 else 0
|
|
print(f"总体: 距离{total_dist:.2f}m, 时间{total_time:.1f}s, 平均速度{avg_speed:.3f} m/s")
|
print(f"单步最大跳变: {max_jump:.2f}m, 最大瞬时速度: {max_speed:.3f} m/s")
|
|
if max_jump > 2.0 or max_speed > 10.0:
|
print(f"⚠️ 警告：存在GPS跳变！数据不可信！")
|
elif avg_speed > 3.0:
|
print(f"⚠️ 警告：平均速度异常高！")
|
