借助网络配置,控制Modbus工具的日志显示,避免卡死界面
智能大石头 authored at 2021-12-11 22:09:19
12.35 KiB
XCoder
using System;
using System.Collections.Generic;
using System.Globalization;
using NewLife;

namespace NewLife.XCoder.Tools.Gps;

/// <summary>一条定位结果</summary>
/// <param name="Latitude">纬度(十进制度,南纬为负)</param>
/// <param name="Longitude">经度(十进制度,西经为负)</param>
/// <param name="Time">UTC 时间,取不到时为空</param>
/// <param name="SpeedKnots">速度(节),取不到时为空</param>
/// <param name="Course">航向(度),取不到时为空</param>
/// <param name="Satellites">卫星数,取不到时为空</param>
/// <param name="Altitude">海拔(米),取不到时为空</param>
/// <param name="Source">来源语句类型,如 GPRMC / GPGGA</param>
public sealed record GpsFix(
    Double Latitude,
    Double Longitude,
    DateTime? Time = null,
    Double? SpeedKnots = null,
    Double? Course = null,
    Int32? Satellites = null,
    Double? Altitude = null,
    String Source = "");

/// <summary>GPS 定位数据处理</summary>
/// <remarks>
/// 覆盖三类常见输入:
/// 1. NMEA 语句($GPRMC / $GPGGA 及其 GN/BD/GL 变体),先校验校验和再解析;
/// 2. 嵌入式常见的 8 字符十六进制单精度浮点(小端序),可与十进制度互转;
/// 3. 轨迹两点球面距离(Haversine),用于里程估算。
/// 解析失败一律返回 null / 空清单,不抛异常,避免脏数据把界面打挂。
/// </remarks>
public static class GpsParser
{
    /// <summary>把 NMEA 的度分格式(ddmm.mmmm / dddmm.mmmm)转换为十进制度</summary>
    /// <param name="value">度分字符串,如 "3112.3456"</param>
    /// <param name="hemisphere">半球字母 N/S/E/W,大小写不敏感</param>
    /// <returns>十进制度;格式非法返回 null</returns>
    public static Double? ParseCoordinate(String? value, String? hemisphere)
    {
        if (value.IsNullOrWhiteSpace()) return null;

        var text = value!.Trim();
        var dot = text.IndexOf('.');
        if (dot < 0) return null;

        // 小数点前至少要有「度 + 两位分」,即 ddmm(.mmmm)
        if (dot < 3) return null;
        if (!Double.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out var raw)) return null;

        var degrees = Math.Floor(raw / 100);
        var minutes = raw - degrees * 100;

        // 分必须落在 [0,60),否则说明数据错了
        if (minutes < 0 || minutes >= 60) return null;

        var result = degrees + minutes / 60;

        var h = (hemisphere ?? "").Trim().ToUpperInvariant();
        if (h is "S" or "W") result = -result;

        return result;
    }

    /// <summary>校验 NMEA 语句校验和($ 与 * 之间逐字节异或)</summary>
    /// <param name="line">整行语句,如 "$GPRMC,...,*68"</param>
    /// <returns>校验通过或语句没有校验和字段时返回 true</returns>
    public static Boolean CheckSum(String? line)
    {
        if (line.IsNullOrWhiteSpace()) return false;

        var text = line!.Trim();
        if (text.Length < 4 || text[0] != '$') return false;

        var star = text.IndexOf('*');
        if (star < 0) return true;     // 没有校验和字段时按无校验处理
        if (star + 3 > text.Length) return false;

        var sum = 0;
        for (var i = 1; i < star; i++) sum ^= text[i];

        if (!Int32.TryParse(text.Substring(star + 1, 2), NumberStyles.HexNumber, CultureInfo.InvariantCulture, out var expect)) return false;

        return sum == expect;
    }

    /// <summary>解析单条 NMEA 语句</summary>
    /// <param name="line">语句原文(可含前后空白)</param>
    /// <returns>定位结果;非定位语句、校验失败、定位状态无效时返回 null</returns>
    public static GpsFix? ParseNmea(String? line)
    {
        if (line.IsNullOrWhiteSpace()) return null;
        if (!CheckSum(line)) return null;

        var text = line!.Trim();
        var star = text.IndexOf('*');
        if (star > 0) text = text[..star];

        var fields = text.Split(',');
        if (fields.Length < 7) return null;

        var type = fields[0].TrimStart('$');
        if (type.Length < 5) return null;

        // 只认最后三个字符,兼容 GP/GN/BD/GL/GA 等不同星系前缀
        var kind = type[^3..].ToUpperInvariant();

        return kind switch
        {
            "RMC" => ParseRmc(fields, type),
            "GGA" => ParseGga(fields, type),
            _ => null,
        };
    }

    private static GpsFix? ParseRmc(String[] fields, String type)
    {
        // $--RMC,hhmmss.ss,A,ddmm.mmmm,N,dddmm.mmmm,E,speed,course,ddmmyy,...
        if (fields.Length < 8) return null;

        // 状态字段 A=有效,V=无效
        if (fields[2].Length > 0 && !fields[2].Equals("A", StringComparison.OrdinalIgnoreCase)) return null;

        var lat = ParseCoordinate(fields[3], fields[4]);
        var lng = ParseCoordinate(fields[5], fields[6]);
        if (lat == null || lng == null) return null;

        return new GpsFix(lat.Value, lng.Value, ParseUtcTime(fields[1]))
        {
            SpeedKnots = ToDouble(fields[7]),
            Course = ToDouble(fields.Length > 8 ? fields[8] : null),
            Source = type,
        };
    }

    private static GpsFix? ParseGga(String[] fields, String type)
    {
        // $--GGA,hhmmss.ss,ddmm.mmmm,N,dddmm.mmmm,E,fix,sats,hdop,alt,M,...
        if (fields.Length < 10) return null;

        // 定位质量 0=未定位
        if (ToDouble(fields[6]) is not { } quality || quality <= 0) return null;

        var lat = ParseCoordinate(fields[2], fields[3]);
        var lng = ParseCoordinate(fields[4], fields[5]);
        if (lat == null || lng == null) return null;

        var sats = ToDouble(fields[7]);

        return new GpsFix(lat.Value, lng.Value, ParseUtcTime(fields[1]))
        {
            Satellites = sats != null ? (Int32)sats.Value : null,
            Altitude = ToDouble(fields[9]),
            Source = type,
        };
    }

    /// <summary>解析多行文本中的全部有效定位(每行一条 NMEA)</summary>
    /// <param name="text">含多条语句的文本,允许夹杂注释与空行</param>
    /// <returns>有效定位清单,保持原顺序</returns>
    public static IReadOnlyList<GpsFix> ParseNmeaBatch(String? text)
    {
        var list = new List<GpsFix>();
        if (text.IsNullOrWhiteSpace()) return list;

        foreach (var raw in text!.Split('\n'))
        {
            var line = raw.Trim().TrimEnd('\r');
            if (line.Length == 0 || line[0] != '$') continue;

            // 一行里可能粘连了两条语句,按 $ 再切一次
            var index = line.IndexOf('$', 1);
            if (index > 0)
            {
                var first = ParseNmea(line[..index]);
                if (first != null) list.Add(first);
                line = line[index..];
            }

            var fix = ParseNmea(line);
            if (fix != null) list.Add(fix);
        }

        return list;
    }

    /// <summary>把 8 字符十六进制解析为单精度浮点(小端序,与嵌入式设备一致)</summary>
    /// <param name="hex">十六进制串,如 "C3F54840"(不足 8 位抛异常)</param>
    /// <returns>解析出的浮点值</returns>
    /// <exception cref="ArgumentException">长度不足或含非法字符</exception>
    public static Single HexToSingle(String? hex)
    {
        var bytes = ParseHexBytes(hex, 4);
        return BitConverter.ToSingle(bytes, 0);
    }

    /// <summary>把单精度浮点转成 8 字符十六进制(小端序)</summary>
    /// <param name="value">浮点值</param>
    /// <returns>8 字符大写十六进制</returns>
    public static String SingleToHex(Single value)
    {
        var bytes = BitConverter.GetBytes(value);
        var chars = new Char[8];
        for (var i = 0; i < 4; i++)
        {
            var b = bytes[i];
            chars[i * 2] = ToChar(b >> 4);
            chars[i * 2 + 1] = ToChar(b & 0x0F);
        }

        return new String(chars);
    }

    /// <summary>解析 16 字符合并坐标(前 8 位纬度,后 8 位经度)</summary>
    /// <param name="hex">16 字符十六进制</param>
    /// <param name="latitude">纬度</param>
    /// <param name="longitude">经度</param>
    /// <returns>是否解析成功</returns>
    public static Boolean TryParseCombined(String? hex, out Single latitude, out Single longitude)
    {
        latitude = 0;
        longitude = 0;
        if (hex == null) return false;

        var text = hex.Trim().Replace(" ", "");
        if (text.Length < 16) return false;

        try
        {
            latitude = HexToSingle(text[..8]);
            longitude = HexToSingle(text[8..16]);

            return true;
        }
        catch (ArgumentException)
        {
            return false;
        }
    }

    /// <summary>计算两点球面距离(米),Haversine 公式</summary>
    /// <param name="lat1">起点纬度</param>
    /// <param name="lng1">起点经度</param>
    /// <param name="lat2">终点纬度</param>
    /// <param name="lng2">终点经度</param>
    /// <returns>距离(米)</returns>
    public static Double Distance(Double lat1, Double lng1, Double lat2, Double lng2)
    {
        const Double R = 6371008.8;   // 地球平均半径

        var radLat1 = lat1 * Math.PI / 180;
        var radLat2 = lat2 * Math.PI / 180;
        var dLat = radLat2 - radLat1;
        var dLng = (lng2 - lng1) * Math.PI / 180;

        var a = Math.Sin(dLat / 2) * Math.Sin(dLat / 2) +
                Math.Cos(radLat1) * Math.Cos(radLat2) * Math.Sin(dLng / 2) * Math.Sin(dLng / 2);

        return 2 * R * Math.Asin(Math.Min(1, Math.Sqrt(a)));
    }

    /// <summary>累计轨迹里程(米)</summary>
    /// <param name="points">按时间顺序排列的定位点</param>
    /// <returns>总距离(米),不足两点返回 0</returns>
    public static Double TotalDistance(IReadOnlyList<GpsFix>? points)
    {
        if (points == null || points.Count < 2) return 0;

        var total = 0d;
        for (var i = 1; i < points.Count; i++)
            total += Distance(points[i - 1].Latitude, points[i - 1].Longitude, points[i].Latitude, points[i].Longitude);

        return total;
    }

    #region 辅助
    private static Byte[] ParseHexBytes(String? hex, Int32 count)
    {
        if (hex.IsNullOrWhiteSpace()) throw new ArgumentException("十六进制内容为空", nameof(hex));

        var text = hex!.Trim().Replace(" ", "");
        if (text.Length < count * 2) throw new ArgumentException($"十六进制长度不足 {count * 2} 位:{text}", nameof(hex));

        var bytes = new Byte[count];
        for (var i = 0; i < count; i++)
        {
            var hi = ParseNibble(text[i * 2], hex);
            var lo = ParseNibble(text[i * 2 + 1], hex);
            bytes[i] = (Byte)((hi << 4) | lo);
        }

        return bytes;
    }

    private static Int32 ParseNibble(Char c, String source)
    {
        if (c >= '0' && c <= '9') return c - '0';
        if (c >= 'a' && c <= 'f') return c - 'a' + 10;
        if (c >= 'A' && c <= 'F') return c - 'A' + 10;

        throw new ArgumentException($"非法十六进制字符 [{c}]:{source}", nameof(source));
    }

    private static Char ToChar(Int32 value) => (Char)(value < 10 ? '0' + value : 'A' + value - 10);

    private static Double? ToDouble(String? text)
    {
        if (text.IsNullOrWhiteSpace()) return null;
        if (!Double.TryParse(text!.Trim(), NumberStyles.Float, CultureInfo.InvariantCulture, out var value)) return null;

        return value;
    }

    private static DateTime? ParseUtcTime(String? text)
    {
        if (text.IsNullOrWhiteSpace()) return null;

        var value = text!.Trim();
        if (value.Length < 6) return null;

        var hour = value[..2].ToInt(-1);
        var minute = value.Substring(2, 2).ToInt(-1);
        var second = value.Substring(4, 2).ToInt(-1);
        if (hour < 0 || minute < 0 || second < 0) return null;

        var ms = 0;
        var dot = value.IndexOf('.');
        if (dot > 0)
        {
            var frac = value[(dot + 1)..];
            if (frac.Length > 3) frac = frac[..3];
            ms = (frac + "000")[..3].ToInt(0);
        }

        try
        {
            return new DateTime(DateTime.UtcNow.Year, DateTime.UtcNow.Month, DateTime.UtcNow.Day, hour, minute, second, ms, DateTimeKind.Utc);
        }
        catch (ArgumentOutOfRangeException)
        {
            return null;
        }
    }
    #endregion
}