From 3c2021441490ae9b93f7a2ef0f379909b589edd9 Mon Sep 17 00:00:00 2001
From: yincheng.zhong <634916154@qq.com>
Date: 星期六, 13 十二月 2025 18:53:16 +0800
Subject: [PATCH] 应该是拍视频的版本，外包在此基础上加了MQTT部分。
---
python/tools/calibration_analyzer.py | 59 +++++++++++++++++++++++++----------------------------------
1 files changed, 25 insertions(+), 34 deletions(-)
diff --git a/python/tools/calibration_analyzer.py b/python/tools/calibration_analyzer.py
index f652b2a..ebbd6a6 100644
--- a/python/tools/calibration_analyzer.py
+++ b/python/tools/calibration_analyzer.py
@@ -34,13 +34,13 @@
# 娣诲姞椤圭洰璺緞
sys.path.append(str(Path(__file__).parent.parent))
-from hitl.geo import geo_to_ecef, ecef_to_enu
+# ENU鍧愭爣宸插湪鍗曠墖鏈虹璁＄畻锛屼笉鍐嶉渶瑕乬eo杞崲鍑芥暟
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}")
@@ -50,9 +50,12 @@
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])
@@ -64,11 +67,13 @@
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:
@@ -77,9 +82,6 @@
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):
@@ -101,29 +103,16 @@
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鍧愭爣鍜岄�熷害...")
+ """璁＄畻閫熷害锛圗NU鍧愭爣宸插湪鍗曠墖鏈虹璁＄畻瀹屾垚锛�"""
+ 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 # 绉�
@@ -451,10 +440,12 @@
'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()}
},
--
Gitblit v1.10.0