#!/usr/bin/env python3
|
"""
|
检查IM23A航向角的定义
|
通过分析位置变化方向和航向角的关系，推断正确的坐标系
|
"""
|
import sys
|
import math
|
|
def parse_cal_line(line):
|
"""解析$CAL数据行"""
|
if not line.startswith('$CAL'):
|
return None
|
parts = line.strip().split(',')
|
if len(parts) < 10:
|
return None
|
try:
|
return {
|
'seq': int(parts[1]),
|
'time': int(parts[2]),
|
'state': parts[3],
|
'x': float(parts[6]), # ENU X (东)
|
'y': float(parts[7]), # ENU Y (北)
|
'z': float(parts[8]), # ENU Z (天)
|
'heading': float(parts[9]), # 航向角(弧度)
|
}
|
except (ValueError, IndexError):
|
return None
|
|
def main():
|
if len(sys.argv) < 2:
|
print("用法: python check_heading_def.py <log_file>")
|
sys.exit(1)
|
|
logfile = sys.argv[1]
|
|
data_points = []
|
with open(logfile, 'r', encoding='utf-8', errors='ignore') as f:
|
for line in f:
|
point = parse_cal_line(line)
|
if point:
|
data_points.append(point)
|
|
print(f"共读取 {len(data_points)} 条数据")
|
|
# 找几个有明显位置变化的片段
|
print("\n=== 分析位置变化和航向角的关系 ===\n")
|
|
for i in range(50, min(len(data_points)-20, 200), 20):
|
p1 = data_points[i]
|
p2 = data_points[i+10]
|
|
dx = p2['x'] - p1['x']
|
dy = p2['y'] - p1['y']
|
dist = math.sqrt(dx**2 + dy**2)
|
|
if dist > 0.5: # 只看位移大于0.5m的
|
# 计算位置变化的方向（数学坐标系：东=0°，北=90°）
|
move_angle_math = math.atan2(dy, dx) # 弧度
|
move_angle_deg = math.degrees(move_angle_math)
|
|
# 转换为罗盘角度（北=0°，东=90°）
|
move_angle_compass_deg = 90.0 - move_angle_deg
|
if move_angle_compass_deg < 0:
|
move_angle_compass_deg += 360
|
if move_angle_compass_deg >= 360:
|
move_angle_compass_deg -= 360
|
|
# IM23A的航向角
|
heading_rad = p2['heading']
|
heading_deg = math.degrees(heading_rad)
|
|
# 转换为0-360范围
|
heading_compass = heading_deg
|
if heading_compass < 0:
|
heading_compass += 360
|
|
print(f"序列 {p1['seq']}-{p2['seq']} ({p1['state']})")
|
print(f" 位移: dx={dx:6.2f}m, dy={dy:6.2f}m, dist={dist:5.2f}m")
|
print(f" 移动方向(数学): {move_angle_deg:6.1f}°")
|
print(f" 移动方向(罗盘): {move_angle_compass_deg:6.1f}°")
|
print(f" IM23A航向(原始): {heading_rad:6.2f} rad = {heading_deg:6.1f}°")
|
print(f" IM23A航向(0-360): {heading_compass:6.1f}°")
|
print(f" 差值(罗盘-IM23A): {move_angle_compass_deg - heading_compass:6.1f}°")
|
print()
|
|
if __name__ == '__main__':
|
main()
|
