| | |
|---|
| | | |
|---|
| | | # 添加项目路径 |
|---|
| | | sys.path.append(str(Path(__file__).parent.parent)) |
|---|
| | | from hitl.geo import geo_to_ecef, ecef_to_enu |
|---|
| | | # ENU坐标已在单片机端计算，不再需要geo转换函数 |
|---|
| | | |
|---|
| | | |
|---|
| | | class CalibrationData: |
|---|
| | | """校准数据点""" |
|---|
| | | def __init__(self, line: str): |
|---|
| | | # $CAL,seq,time_ms,state,throttle_pwm,steering_pwm,lat,lon,alt,hdg,pitch,roll,gx,gy,gz,ax,ay,az |
|---|
| | | # $CAL,seq,time_ms,state,throttle_pwm,steering_pwm,enu_x,enu_y,enu_z,hdg,pitch,roll,gx,gy,gz,ax,ay,az |
|---|
| | | parts = line.strip().split(',') |
|---|
| | | if len(parts) < 18: |
|---|
| | | raise ValueError(f"Invalid CAL line: {line}") |
|---|
| | |
|---|
| | | self.state = parts[3] |
|---|
| | | self.throttle_pwm = int(parts[4]) |
|---|
| | | self.steering_pwm = int(parts[5]) |
|---|
| | | self.lat = float(parts[6]) |
|---|
| | | self.lon = float(parts[7]) |
|---|
| | | self.alt = float(parts[8]) |
|---|
| | | |
|---|
| | | # 直接读取ENU坐标（不再需要从经纬度转换） |
|---|
| | | self.enu_x = float(parts[6]) # 东向坐标 (m) |
|---|
| | | self.enu_y = float(parts[7]) # 北向坐标 (m) |
|---|
| | | self.enu_z = float(parts[8]) # 天向坐标 (m) |
|---|
| | | |
|---|
| | | self.hdg = float(parts[9]) |
|---|
| | | self.pitch = float(parts[10]) |
|---|
| | | self.roll = float(parts[11]) |
|---|
| | |
|---|
| | | self.az = float(parts[17]) / 1000.0 |
|---|
| | | |
|---|
| | | # 待计算字段 |
|---|
| | | self.enu_x = 0.0 |
|---|
| | | self.enu_y = 0.0 |
|---|
| | | self.enu_z = 0.0 |
|---|
| | | self.velocity = 0.0 # 前向速度 m/s |
|---|
| | | self.yaw_rate = 0.0 # 角速度 rad/s |
|---|
| | | |
|---|
| | | # 兼容性字段（已废弃） |
|---|
| | | self.lat = 0.0 |
|---|
| | | self.lon = 0.0 |
|---|
| | | self.alt = self.enu_z |
|---|
| | | |
|---|
| | | |
|---|
| | | class CalibrationAnalyzer: |
|---|
| | |
|---|
| | | def __init__(self, log_file: str): |
|---|
| | | self.log_file = log_file |
|---|
| | | self.data: List[CalibrationData] = [] |
|---|
| | | self.origin_lat = None |
|---|
| | | self.origin_lon = None |
|---|
| | | self.origin_alt = None |
|---|
| | | self.states_data: Dict[str, List[CalibrationData]] = defaultdict(list) |
|---|
| | | |
|---|
| | | def parse_log(self): |
|---|
| | |
|---|
| | | |
|---|
| | | print(f"成功解析 {len(self.data)} 条数据点") |
|---|
| | | |
|---|
| | | # 使用第一个点作为ENU原点 |
|---|
| | | self.origin_lat = self.data[0].lat |
|---|
| | | self.origin_lon = self.data[0].lon |
|---|
| | | self.origin_alt = self.data[0].alt |
|---|
| | | print(f"ENU原点: lat={self.origin_lat:.6f}, lon={self.origin_lon:.6f}, alt={self.origin_alt:.2f}") |
|---|
| | | # ENU坐标已在单片机端计算完成，这里不需要原点转换 |
|---|
| | | print(f"ENU坐标范围: X=[{min(d.enu_x for d in self.data):.2f}, {max(d.enu_x for d in self.data):.2f}], " + |
|---|
| | | f"Y=[{min(d.enu_y for d in self.data):.2f}, {max(d.enu_y for d in self.data):.2f}] (m)") |
|---|
| | | |
|---|
| | | def compute_enu_and_velocity(self): |
|---|
| | | """计算ENU坐标和速度""" |
|---|
| | | print("计算ENU坐标和速度...") |
|---|
| | | """计算速度（ENU坐标已在单片机端计算完成）""" |
|---|
| | | print("计算速度和角速度...") |
|---|
| | | |
|---|
| | | origin_ecef = geo_to_ecef(self.origin_lat, self.origin_lon, self.origin_alt) |
|---|
| | | origin_lat_rad = np.deg2rad(self.origin_lat) |
|---|
| | | origin_lon_rad = np.deg2rad(self.origin_lon) |
|---|
| | | |
|---|
| | | # ENU坐标已在单片机端计算完成，这里只需要计算速度和角速度 |
|---|
| | | for i, data in enumerate(self.data): |
|---|
| | | # 计算ENU坐标 |
|---|
| | | ecef = geo_to_ecef(data.lat, data.lon, data.alt) |
|---|
| | | dx = ecef[0] - origin_ecef[0] |
|---|
| | | dy = ecef[1] - origin_ecef[1] |
|---|
| | | dz = ecef[2] - origin_ecef[2] |
|---|
| | | enu = ecef_to_enu(dx, dy, dz, origin_lat_rad, origin_lon_rad) |
|---|
| | | data.enu_x, data.enu_y, data.enu_z = enu |
|---|
| | | |
|---|
| | | # 计算速度（相邻点差分） |
|---|
| | | if i > 0: |
|---|
| | | dt = (data.time_ms - self.data[i-1].time_ms) / 1000.0 # 秒 |
|---|
| | |
|---|
| | | 'log_file': str(self.log_file), |
|---|
| | | 'total_samples': len(self.data), |
|---|
| | | 'duration_seconds': (self.data[-1].time_ms - self.data[0].time_ms) / 1000.0, |
|---|
| | | 'origin': { |
|---|
| | | 'latitude': self.origin_lat, |
|---|
| | | 'longitude': self.origin_lon, |
|---|
| | | 'altitude': self.origin_alt |
|---|
| | | 'enu_range': { |
|---|
| | | 'x_min': min(d.enu_x for d in self.data), |
|---|
| | | 'x_max': max(d.enu_x for d in self.data), |
|---|
| | | 'y_min': min(d.enu_y for d in self.data), |
|---|
| | | 'y_max': max(d.enu_y for d in self.data), |
|---|
| | | 'note': 'ENU coordinates computed onboard, relative to first RTK fix' |
|---|
| | | }, |
|---|
| | | 'states': {state: len(dlist) for state, dlist in self.states_data.items()} |
|---|
| | | }, |
|---|
