解析RoyalTek RBT-2100 蓝牙GPS接收器数据包

数据包为标准的 NMEA-0183 协议，解析了‌** $GPGSA** ‌，‌**$ GPGSV，** $GPGGA，$ GPRMC四种报文，包括定位信息和卫星可视信息。
cpp 复制代码
#define WIN32_LEAN_AND_MEAN
#include <windows.h>
#include <iostream>
#include <string>
#include <vector>
#include <sstream>
#include <cmath>
#include <chrono>
#include <iomanip>
#include <cstdlib>
#include <format> // C++20 格式化输出

using namespace std;

// ============================================================
//  GPS 数据结构
// ============================================================
struct GpsData {
	double latitude = 0.0;   // 十进制度 (北正南负)
	double longitude = 0.0;   // 十进制度 (东正西负)
	double altitude = 0.0;   // 海拔 (m)
	std::string altitudeUnit;
	float heightDifference = 0.0f;
	std::string heightDifferenceUnit;
	int    satellites = 0;     // 使用卫星数
	int    fixQuality = 0;     // 0=无效, 1=GPS, 2=DGPS
	std::string utcTime;
	float HDOP = 0.0f;
	std::string differentialStationNo;
	std::string differentialDataAge;

	//RMC
	float velocity = 0.0;//knot
	float magneticDeclination = 0; // ?磁偏角
	std::string magneticDeclinationDirection;
	float heading = 0.0f;//°以正北为 0 度，顺时针计算
	std::string utcDate; //格式为 ddmmyy (日月年)。
	std::string posMode; //模式，A = 自动，D = 差分，E = 估测，N = 数据无效（3.0协议内容）

	bool   valid = false;
};

struct SateInfo
{
	int PRN;//(卫星编号)
	int Elevation;// ?仰角
	int Azimuth;// ??方位角
	int SNR;// ?信噪比
};

struct GSVData {
	int total = 0;
	int current = 0;
	int satellite = 0;
	std::vector<struct SateInfo> sateinfo{};
};

struct GSAData {
	string posMode;
	int posType = 0;
	std::vector<int> PRN;
	float PDOP = 0.0f;
	float HDOP = 0.0f;
	float VDOP = 0.0f;
};

// ============================================================
//  NMEA 工具函数
// ============================================================

// 按逗号分割字符串
static std::vector<std::string> SplitFields(const std::string& s, char delim) {
	std::vector<std::string> tokens;
	std::istringstream iss(s);
	std::string token;
	while (std::getline(iss, token, delim))
		tokens.push_back(token);
	return tokens;
}

// 验证 NMEA 校验和 ($....*HH)
static bool VerifyChecksum(const std::string& sentence) {
	if (sentence.empty() || sentence[0] != '$')
		return false;

	size_t starPos = sentence.find('*');
	if (starPos == std::string::npos || starPos + 2 >= sentence.size())
		return false;

	unsigned char calc = 0;
	for (size_t i = 1; i < starPos; ++i)
		calc ^= static_cast<unsigned char>(sentence[i]);

	unsigned char given = 0;
	std::string hexStr = sentence.substr(starPos + 1, 2);
	// 手动解析十六进制，避免 locale 问题
	for (char c : hexStr) {
		given <<= 4;
		if (c >= '0' && c <= '9')      given |= (c - '0');
		else if (c >= 'A' && c <= 'F') given |= (c - 'A' + 10);
		else if (c >= 'a' && c <= 'f') given |= (c - 'a' + 10);
		else return false;
	}
	return calc == given;
}

// NMEA 坐标 → 十进制度
// 输入格式: "DDMM.MMMMM" 或 "DDDMM.MMMMM", hemisphere: N/S/E/W
static double NmeaToDecimalDegree(const std::string& raw, char hemisphere) {
	if (raw.empty()) return 0.0;

	size_t dotPos = raw.find('.');
	if (dotPos == std::string::npos || dotPos < 3)
		return 0.0;

	size_t minStart = dotPos - 2;  // "分"从小数点前2位开始
	double degrees = std::stod(raw.substr(0, minStart));
	double minutes = std::stod(raw.substr(minStart));
	double result = degrees + minutes / 60.0;

	if (hemisphere == 'S' || hemisphere == 'W')
		result = -result;

	return result;
}

// ============================================================
//  NMEA 语句解析
// ============================================================

// 解析 $GPGGA / $GNGGA / 
//$GPGGA,044744.00,3122.4658,N,12025.2791,E,1,10,3.00,12.575,M,7.100,M,00,0000*5F
//字段0至字段13
static GpsData ParseGGA(const std::string& sentence) {
	GpsData data;
	auto f = SplitFields(sentence, ',');
	// 字段: $xxGGA,time,lat,N/S,lon,E/W,fix,sat,hdop,alt,M,...
	if (f.size() < 10) return data;

	try {
		data.utcTime = f[1];
		data.latitude = NmeaToDecimalDegree(f[2], f[3].empty() ? 'N' : f[3][0]);
		data.longitude = NmeaToDecimalDegree(f[4], f[5].empty() ? 'E' : f[5][0]);
		data.fixQuality = std::stoi(f[6]);
		data.satellites = std::stoi(f[7]);
		data.HDOP = std::stof(f[8]);
		if (!f[9].empty())
			data.altitude = std::stod(f[9]);
		data.altitudeUnit = f[10];
		data.valid = (data.fixQuality > 0);

		data.heightDifference = std::stof(f[11]);
		data.heightDifferenceUnit = f[12];
		data.differentialStationNo = f[13];
		data.differentialDataAge = f[14];
	}
	catch (...) {
		data.valid = false;
	}
	return data;
}

// 解析 $GPRMC / $GNRMC / $BDRMC
// $GPRMC,044838.00,A,3122.4658,N,12025.2799,E,0.257,261.7,180921,0.0,E,A*3B

static GpsData ParseRMC(const std::string& sentence) {
	GpsData data;
	auto f = SplitFields(sentence, ',');
	// 字段: $xxRMC,time,A/V,lat,N/S,lon,E/W,speed,course,date,...
	if (f.size() < 7) return data;

	try {
		data.utcTime = f[1];
		bool statusOK = (!f[2].empty() && f[2][0] == 'A');
		data.latitude = NmeaToDecimalDegree(f[3], f[4].empty() ? 'N' : f[4][0]);
		data.longitude = NmeaToDecimalDegree(f[5], f[6].empty() ? 'E' : f[6][0]);
		data.valid = statusOK;
		data.velocity = std::stof(f[7]);
		data.heading = std::stof(f[8]);
		data.utcDate = f[9];
		data.magneticDeclination = f[10].empty() ? 0.0f : std::stof(f[10]);
		data.magneticDeclinationDirection = f[11];
		data.posMode = f[12];
	}
	catch (...) {
		data.valid = false;
	}
	return data;
}

GSAData ParseGSA(const std::string& sentence)
{
	//$GPGSA,A,1,,,,,,,,,,,,,,,
	GSAData data;
	auto f = SplitFields(sentence, ',');
	if (f.size() < 18) return data;
	data.posMode = f[1];
	data.posType = std::stoi(f[2]);
	for (int i = 3; i < 15; ++i)
	{
		if (!f[i].empty())
		{
			data.PRN.push_back(stoi(f[i]));
		}
	}
	data.PDOP = f[15].empty() ? 0 : std::stof(f[15]);
	data.HDOP = f[16].empty() ? 0 : std::stof(f[16]);
	data.VDOP = f[17].empty() ? 0 : std::stof(f[17]);
	return data;
}

GSVData ParseGSV(const std::string& sentence)
{
	GSVData data;
	auto f = SplitFields(sentence, ',');
	if (f.size() < 7) return data;

	data.total = std::stoi(f[1]);
	data.current = std::stoi(f[2]);
	data.satellite = std::stoi(f[3]);
	auto count = (f.size() - 4) / 4;
	for (size_t i = 0; i < count; i++)
	{
		SateInfo info;
		info.PRN = std::stoi(f[4 + i * 4]);
		info.Elevation = std::stoi(f[4 + i * 4 + 1]);
		info.Azimuth = std::stoi(f[4 + i * 4 + 2]);
		info.SNR = f[4 + i * 4 + 3].empty() ? 0 : std::stoi(f[4 + i * 4 + 3]);

		data.sateinfo.push_back(info);
	}
	return data;
}

// ============================================================
//  Win32 串口封装类
// ============================================================
class Win32SerialPort {
public:
	Win32SerialPort() = default;
	~Win32SerialPort() { Close(); }

	// 禁止拷贝
	Win32SerialPort(const Win32SerialPort&) = delete;
	Win32SerialPort& operator=(const Win32SerialPort&) = delete;

	/// 打开串口并配置参数
	/// @param portName  如 "COM5" 或 "\\\\.\\COM10"（COM≥10 必须用此格式）
	/// @param baudRate  波特率，蓝牙GPS通常为4800
	bool Open(const std::string& portName, DWORD baudRate = 4800) {
		// COM10及以上需要使用 \\.\COMxx 格式
		std::string fullPath = portName;
		if (portName.substr(0, 4) == "COM") {
			int num = std::atoi(portName.c_str() + 3);
			if (num >= 10)
				fullPath = "\\\\.\\" + portName;
		}

		hPort_ = CreateFileA(fullPath.c_str(), GENERIC_READ,
			0,              // 不共享
			nullptr,        // 默认安全属性
			OPEN_EXISTING,
			0,              // 非重叠模式（同步读写）
			nullptr
		);

		if (hPort_ == INVALID_HANDLE_VALUE) {
			DWORD err = GetLastError();
			std::cerr << "[ERROR] CreateFile 失败, GetLastError=" << err << "\n";
			return false;
		}

		// ---------- 配置 DCB ----------
		DCB dcb{};
		dcb.DCBlength = sizeof(DCB);
		if (!GetCommState(hPort_, &dcb)) {
			std::cerr << "[ERROR] GetCommState 失败\n";
			CloseHandle(hPort_);
			hPort_ = INVALID_HANDLE_VALUE;
			return false;
		}

		dcb.BaudRate = baudRate;
		dcb.ByteSize = 8;
		dcb.Parity = NOPARITY;
		dcb.StopBits = ONESTOPBIT;
		dcb.fBinary = TRUE;
		dcb.fParity = FALSE;
		dcb.fOutxCtsFlow = FALSE;
		dcb.fOutxDsrFlow = FALSE;
		dcb.fDtrControl = DTR_CONTROL_ENABLE;
		dcb.fRtsControl = RTS_CONTROL_ENABLE;
		dcb.fOutX = FALSE;
		dcb.fInX = FALSE;
		dcb.fNull = FALSE;
		dcb.fAbortOnError = FALSE;

		if (!SetCommState(hPort_, &dcb)) {
			std::cerr << "[ERROR] SetCommState 失败\n";
			CloseHandle(hPort_);
			hPort_ = INVALID_HANDLE_VALUE;
			return false;
		}
		// ---------- 超时设置 ----------
		// ReadIntervalTimeout > 0 且其余为0 → 每次ReadFile最多等该毫秒数
		COMMTIMEOUTS timeouts{};
		timeouts.ReadIntervalTimeout = 50;   // 字符间最大间隔(ms)
		timeouts.ReadTotalTimeoutMultiplier = 0;
		timeouts.ReadTotalTimeoutConstant = 0;
		timeouts.WriteTotalTimeoutMultiplier = 0;
		timeouts.WriteTotalTimeoutConstant = 0;

		if (!SetCommTimeouts(hPort_, &timeouts)) {
			std::cerr << "[ERROR] SetCommTimeouts 失败\n";
			CloseHandle(hPort_);
			hPort_ = INVALID_HANDLE_VALUE;
			return false;
		}

		// 清空收发缓冲区
		PurgeComm(hPort_, PURGE_RXCLEAR | PURGE_TXCLEAR);

		std::cout << "[INFO] 已打开 " << fullPath
			<< " @ " << baudRate << " bps\n";
		return true;
	}

	/// 从串口读取一个字节，返回实际读取字节数（0表示超时/无数据）
	int ReadByte(char& ch) {
		DWORD bytesRead = 0;
		if (!ReadFile(hPort_, &ch, 1, &bytesRead, nullptr))
			return -1;  // 错误
		return static_cast<int>(bytesRead);
	}

	/// 读取一行 NMEA 数据（以 \n 结尾），带超时保护
	/// @return true=成功读到完整行, false=超时或错误
	bool ReadLine(std::string& line, DWORD timeoutMs = 5000) {
		line.clear();
		auto deadline = std::chrono::steady_clock::now()
			+ std::chrono::milliseconds(timeoutMs);

		while (std::chrono::steady_clock::now() < deadline) {
			char ch;
			int n = ReadByte(ch);
			if (n < 0) return false;       // 读取出错
			if (n == 0) continue;          // 超时，继续等待

			if (ch == '\n')
				return !line.empty();      // 收到完整行
			if (ch != '\r')
				line += ch;
		}
		return false;  // 总超时
	}

	void Close() {
		if (hPort_ != INVALID_HANDLE_VALUE) {
			CloseHandle(hPort_);
			hPort_ = INVALID_HANDLE_VALUE;
			std::cout << "\n串口已关闭。\n";
		}
	}

	bool IsOpen() const { return hPort_ != INVALID_HANDLE_VALUE; }

private:
	HANDLE hPort_ = INVALID_HANDLE_VALUE;
};

// ============================================================
//  主程序入口
// ============================================================
int main(int argc, char* argv[]) {
	std::string portName = (argc > 1) ? argv[1] : "COM3";
	DWORD baudRate = (argc > 2) ? static_cast<DWORD>(std::stoul(argv[2])) : 9600;

	Win32SerialPort serial;
	if (!serial.Open(portName, baudRate)) {
		std::cerr << "\n用法: gps_reader.exe <COM端口> [波特率]\n";
		std::cerr << "示例: gps_reader.exe COM5 4800\n";
		std::cerr << "\n请确认:\n";
		std::cerr << "  1. 蓝牙GPS已配对\n";
		std::cerr << "  2. 已在「蓝牙设置→COM端口」中分配了端口\n";
		std::cerr << "  3. 没有其他程序占用该端口\n";
		return 1;
	}

	std::cout << "等待 GPS 数据... (Ctrl+C 退出)\n\n";
	std::string line;
	while (serial.ReadLine(line))
	{
		// 跳过非NMEA行
		if (line.empty() || line[0] != '$')
			continue;

		// 校验和验证
		if (!VerifyChecksum(line))
			continue;

		// 提取语句类型标识 (第4~6字符): GGA / RMC / GLL ...
		// 兼容 $GPxxx, $GNxxx, $BDxxx, $GLxxx
		if (line.size() < 7) continue;
		std::string type = line.substr(0, line.find_first_of(','));
		line = line.substr(0, line.find_last_of('*'));

		std::cout << std::fixed << std::setprecision(4);
		if (type == "$GPGGA") {
			//continue;
			GpsData gps = ParseGGA(line);
			if (!gps.valid)
				continue;
			// 格式化输出			
			auto utc = gps.utcTime;
			int hours = std::stoi(utc.substr(0, 2)) + 8;
			int minutes = std::stoi(utc.substr(2, 2));
			int seconds = std::stoi(utc.substr(4, 2));
			string utcTime = std::format("{:02d}:{:02d}:{:02d}", hours, minutes, seconds);
			std::cout << "[定位信息] " << gps.valid
				<< " UTC=" << utcTime
				<< " 纬度=" << gps.latitude
				<< " 经度=" << gps.longitude
				<< " 海拔=" << gps.altitude << gps.altitudeUnit;
			if (gps.satellites > 0)
				std::cout << " 卫星=" << gps.satellites << endl;
		}
		else if (type == "$GPRMC") {
			GpsData gps = ParseRMC(line);
			if (!gps.valid)
				continue;
			// 格式化输出

			auto utc = gps.utcTime;
			int hours = std::stoi(utc.substr(0, 2)) + 8;
			int minutes = std::stoi(utc.substr(2, 2));
			int seconds = std::stoi(utc.substr(4, 2));
			string utcTime = std::format("{:2d}:{:2d}:{:2d}", hours, minutes, seconds);
			string utcDate = gps.utcDate.empty() ? "" : gps.utcDate.substr(4, 2) + "-" + gps.utcDate.substr(2, 2) + "-" + gps.utcDate.substr(0, 2);
			std::cout << "[最小定位信息] " << gps.valid
				<< "  UTC=" << utcDate << " " << utcTime
				<< "  纬度=" << gps.latitude
				<< "  经度=" << gps.longitude
				<< "  速度=" << gps.velocity
				<< "  航向=" << gps.heading
				<< "  模式=" << gps.posMode << endl;
		}
		else if (type == "$GPGSA") {
			auto gps = ParseGSA(line);
			// 格式化输出
			std::cout << "[卫星状态及精度] "
				<< " 定位模式=" << gps.posMode << " 定位类型=" << gps.posType << " 定位卫星列表=";
			for (int i = 0; i < gps.PRN.size(); ++i)
				cout << " " << gps.PRN[i];
			cout << " PDOP=" << gps.PDOP << " HDOP=" << gps.HDOP << " VDOP=" << gps.VDOP << endl;
		}
		else if (type == "$GPGSV") {
			auto gps = ParseGSV(line);
			// 格式化输出
			std::cout << "[可视卫星状态] "
				<< " 报文序号=" << gps.current << " / " << gps.total
				<< " 可视卫星数量=" << gps.satellite << endl;
			for (const auto& sate : gps.sateinfo)
			{
				std::cout << "卫星编号=" << sate.PRN
					<< " 仰角=" << sate.Elevation
					<< " 方位角=" << sate.Azimuth
					<< " 信噪比=" << sate.SNR << endl;
			}
		}
		else
			// 其语句暂不处理
			std::cout << "type: " << type << "  " + line << std::endl;
	}	
	return 0;
}