GNRMC、GNVTG、GNGGA格式消息解析

DongHappyyy

已于 2024-05-23 18:54:59 修改

阅读量2.2k

点赞数 1

文章标签： python RTK GNGGA GNRMC

于 2024-05-23 16:44:20 首次发布

本文链接：https://blog.csdn.net/weixin_39715012/article/details/139151857

版权

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 = 6378137.0           # 长半轴
    f = 1 / 298.257223563   # 扁率  f = 1-b/a  298.25722293287096133065466877236
    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.9246016,N,11425.8607692,E,0.100,,230524,,,R,V*0F
$GNVTG,,T,,M,0.100,N,0.186,K,D*36
$GNGGA,023255.10,3030.9246016,N,11425.8607692,E,4,12,1.17,31.586,M,-10.499,M,1.1,0659*7D
$GNRMC,023255.20,A,3030.9246012,N,11425.8607698,E,0.082,,230524,,,R,V*09
$GNVTG,,T,,M,0.082,N,0.152,K,D*34
$GNGGA,023255.20,3030.9246012,N,11425.8607698,E,4,12,1.17,31.586,M,-10.499,M,1.2,0659*73
$GNRMC,023255.30,A,3030.9246018,N,11425.8607700,E,0.047,,230524,,,R,V*0B
$GNVTG,,T,,M,0.047,N,0.086,K,D*35
$GNGGA,023255.30,3030.9246018,N,11425.8607700,E,4,12,1.17,31.585,M,-10.499,M,1.3,0659*7A
$GNRMC,023255.40,A,3030.9246014,N,11425.8607678,E,0.032,,230524,,,R,V*0C
$GNVTG,,T,,M,0.032,N,0.059,K,D*35
$GNGGA,023255.40,3030.9246014,N,11425.8607678,E,4,12,1.17,31.584,M,-10.499,M,0.4,0659*78
$GNRMC,023255.50,A,3030.9246009,N,11425.8607705,E,0.054,,230524,,,R,V*0A
$GNVTG,,T,,M,0.054,N,0.100,K,D*38
$GNGGA,023255.50,3030.9246009,N,11425.8607705,E,4,12,1.17,31.581,M,-10.499,M,0.5,0659*7A
$GNRMC,023255.60,A,3030.9246022,N,11425.8607721,E,0.039,,230524,,,R,V*0D
$GNVTG,,T,,M,0.039,N,0.071,K,D*34
$GNGGA,023255.60,3030.9246022,N,11425.8607721,E,4,12,1.17,31.575,M,-10.499,M,0.6,0659*7E
$GNRMC,023255.70,A,3030.9246015,N,11425.8607719,E,0.090,,230524,,,R,V*00
$GNVTG,,T,,M,0.090,N,0.167,K,D*31
$GNGGA,023255.70,3030.9246015,N,11425.8607719,E,4,12,1.17,31.580,M,-10.499,M,0.7,0659*7B
$GNRMC,023255.80,A,3030.9246012,N,11425.8607699,E,0.078,,230524,,,R,V*07
$GNVTG,,T,,M,0.078,N,0.144,K,D*36
$GNGGA,023255.80,3030.9246012,N,11425.8607699,E,4,12,1.17,31.582,M,-10.499,M,0.8,0659*77
$GNRMC,023255.90,A,3030.9246017,N,11425.8607698,E,0.051,,230524,,,R,V*09
$GNVTG,,T,,M,0.051,N,0.095,K,D*30
$GNGGA,023255.90,3030.9246017,N,11425.8607698,E,4,12,1.17,31.582,M,-10.499,M,0.9,0659*73
$GNRMC,023256.00,A,3030.9246017,N,11425.8607696,E,0.022,,230524,,,R,V*09
$GNVTG,,T,,M,0.022,N,0.041,K,D*3D
$GNGGA,023256.00,3030.9246017,N,11425.8607696,E,4,12,1.17,31.583,M,-10.499,M,1.0,0659*7E
$GNRMC,023256.10,A,3030.9246022,N,11425.8607728,E,0.012,,230524,,,R,V*09
$GNVTG,,T,,M,0.012,N,0.022,K,D*3B
$GNGGA,023256.10,3030.9246022,N,11425.8607728,E,4,12,1.17,31.576,M,-10.499,M,1.1,0659*76