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
}
|