GNRMC、GNVTG、GNGGA格式消息解析

大家好，欢迎来到IT知识分享网。

import math import numpy as np from pathlib import Path from pyproj import CRS, Transformer from datetime import datetime, timedelta # def dms_to_decimal(degrees, minutes, seconds): # decimal_degrees = degrees + minutes/60 + seconds/3600 # return decimal_degrees def gngga_to_decimal(latitude_dddmm, longitude_dddmm, radians=False): latitude_decimal = latitude_dddmm // 100 + (latitude_dddmm % 100) / 60 longitude_decimal = longitude_dddmm // 100 + (longitude_dddmm % 100) / 60 if radians: # 将度数转换为弧度 latitude_decimal = math.radians(latitude_decimal) longitude_decimal = math.radians(longitude_decimal) return latitude_decimal, longitude_decimal # def wgs84_to_cartesian_lib(latitude, longitude, altitude): # wgs84 = CRS.from_epsg(4326) # enu = CRS.from_epsg(4978) # transformer = Transformer.from_crs(wgs84, enu, always_xy=True) # x, y, z = transformer.transform(longitude, latitude, altitude) # return x, y, z def wgs84_to_cartesian_math(latitude_rad, longitude_rad, altitude): # WGS84椭球体参数 a = .0 # 长半轴 f = 1 / 298. # 扁率 f = 1-b/a 298.0 e2 = 2 * f - f  2 # 离心率的平方 e = (a2 - b2)^0.5/a = 1- b^2/a^2 # 计算曲率半径 N = a / math.sqrt(1 - e2 * math.sin(latitude_rad)  2) # 计算三维坐标点 https://www.cnblogs.com/charlee44/p/15202473.html x = (N + altitude) * math.cos(latitude_rad) * math.cos(longitude_rad) y = (N + altitude) * math.cos(latitude_rad) * math.sin(longitude_rad) z = (N * (1 - e2) + altitude) * math.sin(latitude_rad) return np.array([[x, y, z]]) def ecef_to_enu(ref_latitude, ref_longitude): R = np.array([[-1*math.sin(ref_longitude), math.cos(ref_longitude), 0], [-1*math.sin(ref_latitude)*math.cos(ref_longitude), -1*math.sin(ref_latitude)*math.sin(ref_longitude), math.cos(ref_latitude)], [ math.cos(ref_latitude)*math.cos(ref_longitude), math.cos(ref_latitude)*math.sin(ref_longitude), math.sin(ref_latitude)]]) return R if __name__ == '__main__': log_path = Path("log/LOG00024.TXT") log_txt_list = log_path.open('r').readlines() save_name = f"{ 
     log_path.stem}_filter.txt" save_path = log_path.with_name(save_name) time_list = [] GNRMC_list = [] GNVTG_list = [] GNGGA_list = [] trajectory_list = [] for log in log_txt_list: if 'GNRMC' in log: GNRMC_list.append(log) if 'GNVTG' in log: GNVTG_list.append(log) if 'GNGGA' in log: GNGGA_list.append(log) print(len(GNRMC_list)) print(len(GNVTG_list)) print(len(GNGGA_list)) cnt = 0 for gnrmc, gnvtg, gngga in zip(GNRMC_list, GNVTG_list, GNGGA_list): gnrmc_fields = gnrmc.split(',') gngga_fields = gngga.split(',') if gnrmc_fields[12] == 'R' and gngga_fields[6] == '4': # 速度 gnvtg_fields = gnvtg.split(',') speed = float(gnvtg_fields[7]) # print(speed) # 日期和时间 utc_time = gnrmc_fields[1] utc_date = gnrmc_fields[9] format = f"20{ 
     utc_date[4:6]}-{ 
     utc_date[2:4]}-{ 
     utc_date[0:2]} { 
     utc_time[0:2]}:{ 
     utc_time[2:4]}:{ 
     utc_time[4:]}0" utc_date_time = datetime.fromisoformat(format) beijing_date_time = utc_date_time + timedelta(hours=8) timestamp = beijing_date_time.strftime('%Y%m%d%H%M%S%f') time_list.append(beijing_date_time) # print(timestamp) # break # 经纬度高程 latitude = float(gngga_fields[2]) longitude = float(gngga_fields[4]) altitude = float(gngga_fields[9]) latitude_rad, longitude_rad = gngga_to_decimal(latitude, longitude, radians=True) current_point = wgs84_to_cartesian_math(latitude_rad, longitude_rad, altitude) if cnt == 0: rotation = ecef_to_enu(latitude_rad, longitude_rad) start_point = current_point enu_point = np.dot(rotation, (current_point-start_point).T).T x, y, z = enu_point[0] # 检查轨迹连续性 if len(time_list) >= 2: if time_list[-1] == time_list[-2]+timedelta(milliseconds=100): pass else: print(time_list[-1]) print(time_list[-2]) print(time_list[-2]+timedelta(milliseconds=100)) print() with open(save_path, 'a') as f: f.write("\nbreak\n") else: pass # 保存 info = f"{ 
     timestamp},{ 
     x},{ 
     y},{ 
     z},{ 
     speed}\n" with open(save_path, 'a') as f: f.write(info) cnt += 1 else: pass 

测试数据

$GNRMC,023255.10,A,3030.,N,11425.,E,0.100,,,,,R,V*0F $GNVTG,,T,,M,0.100,N,0.186,K,D*36 $GNGGA,023255.10,3030.,N,11425.,E,4,12,1.17,31.586,M,-10.499,M,1.1,0659*7D $GNRMC,023255.20,A,3030.,N,11425.,E,0.082,,,,,R,V*09 $GNVTG,,T,,M,0.082,N,0.152,K,D*34 $GNGGA,023255.20,3030.,N,11425.,E,4,12,1.17,31.586,M,-10.499,M,1.2,0659*73 $GNRMC,023255.30,A,3030.,N,11425.,E,0.047,,,,,R,V*0B $GNVTG,,T,,M,0.047,N,0.086,K,D*35 $GNGGA,023255.30,3030.,N,11425.,E,4,12,1.17,31.585,M,-10.499,M,1.3,0659*7A $GNRMC,023255.40,A,3030.,N,11425.,E,0.032,,,,,R,V*0C $GNVTG,,T,,M,0.032,N,0.059,K,D*35 $GNGGA,023255.40,3030.,N,11425.,E,4,12,1.17,31.584,M,-10.499,M,0.4,0659*78 $GNRMC,023255.50,A,3030.,N,11425.,E,0.054,,,,,R,V*0A $GNVTG,,T,,M,0.054,N,0.100,K,D*38 $GNGGA,023255.50,3030.,N,11425.,E,4,12,1.17,31.581,M,-10.499,M,0.5,0659*7A $GNRMC,023255.60,A,3030.,N,11425.,E,0.039,,,,,R,V*0D $GNVTG,,T,,M,0.039,N,0.071,K,D*34 $GNGGA,023255.60,3030.,N,11425.,E,4,12,1.17,31.575,M,-10.499,M,0.6,0659*7E $GNRMC,023255.70,A,3030.,N,11425.,E,0.090,,,,,R,V*00 $GNVTG,,T,,M,0.090,N,0.167,K,D*31 $GNGGA,023255.70,3030.,N,11425.,E,4,12,1.17,31.580,M,-10.499,M,0.7,0659*7B $GNRMC,023255.80,A,3030.,N,11425.,E,0.078,,,,,R,V*07 $GNVTG,,T,,M,0.078,N,0.144,K,D*36 $GNGGA,023255.80,3030.,N,11425.,E,4,12,1.17,31.582,M,-10.499,M,0.8,0659*77 $GNRMC,023255.90,A,3030.,N,11425.,E,0.051,,,,,R,V*09 $GNVTG,,T,,M,0.051,N,0.095,K,D*30 $GNGGA,023255.90,3030.,N,11425.,E,4,12,1.17,31.582,M,-10.499,M,0.9,0659*73 $GNRMC,023256.00,A,3030.,N,11425.,E,0.022,,,,,R,V*09 $GNVTG,,T,,M,0.022,N,0.041,K,D*3D $GNGGA,023256.00,3030.,N,11425.,E,4,12,1.17,31.583,M,-10.499,M,1.0,0659*7E $GNRMC,023256.10,A,3030.,N,11425.,E,0.012,,,,,R,V*09 $GNVTG,,T,,M,0.012,N,0.022,K,D*3B $GNGGA,023256.10,3030.,N,11425.,E,4,12,1.17,31.576,M,-10.499,M,1.1,0659*76