過去在處理 GNSS 資料時，多是使用高階語言 (ex. spilt function)，或是一些開源程式，甚至 GNSS 設備既有的 API，在自己的程式內就可以快速取用各種數據。最近一些原因，不得不面對 NMEA 語句的基礎處理。本篇記錄開發成果，但程式確實還有許多優化空間，有空再整理。
Serial Port Connection
我是透過 Thread 平行處理 GNSS NMEA 資料的接收， Main 函數會 create 跟 init 這個 Thread ，並傳入一個共享記憶體位址 arg 放置 GNSS 資料，與 Main 即時共享。
Thread 的第一步要開啟 Serial Port ，讀取資料放進 buffer ，這部分我是參考下面網站的 open_serial_port 跟 read_port 兩個函數：
https://www.pololu.com/docs/0J73/15.5
int open_serial_port(const char * device, uint32_t baud_rate)
{
int fd = open(device, O_RDONLY| O_NOCTTY); // modified
if (fd == -1)
{
perror(device);
return -1;
}
// Flush away any bytes previously read or written.
int result = tcflush(fd, TCIOFLUSH);
if (result)
{
perror("tcflush failed"); // just a warning, not a fatal error
}
// Get the current configuration of the serial port.
struct termios options;
result = tcgetattr(fd, &options);
if (result)
{
perror("tcgetattr failed");
close(fd);
return -1;
}
// Turn off any options that might interfere with our ability to send and
// receive raw binary bytes.
options.c_iflag &= ~(INLCR | IGNCR | ICRNL | IXON | IXOFF);
options.c_oflag &= ~(ONLCR | OCRNL);
options.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
// Set up timeouts: Calls to read() will return as soon as there is
// at least one byte available or when 100 ms has passed.
options.c_cc[VTIME] = 1;
options.c_cc[VMIN] = 0;
// This code only supports certain standard baud rates. Supporting
// non-standard baud rates should be possible but takes more work.
switch (baud_rate)
{
case 4800: cfsetospeed(&options, B4800); break;
case 9600: cfsetospeed(&options, B9600); break;
case 19200: cfsetospeed(&options, B19200); break;
case 38400: cfsetospeed(&options, B38400); break;
case 115200: cfsetospeed(&options, B115200); break;
default:
fprintf(stderr, "warning: baud rate %u is not supported, using 9600.\n",
baud_rate);
cfsetospeed(&options, B9600);
break;
}
cfsetispeed(&options, cfgetospeed(&options));
result = tcsetattr(fd, TCSANOW, &options);
if (result)
{
perror("tcsetattr failed");
close(fd);
return -1;
}
return fd;
}ssize_t read_port(int fd, uint8_t * buffer, size_t size)
{
size_t received = 0;
while (received < size)
{
ssize_t r = read(fd, buffer + received, size - received);
if (r < 0)
{
perror("failed to read from port");
return -1;
}
if (r == 0)
{
// Timeout
break;
}
received += r;
}
return received;
}我只有把 O_RDWR 改為 O_RDONLY ，因為我只需要讀，不需要寫入資料給 Serial Port 。但未來如果要做 VRS 的 RTK Server 連線，則會需要寫入 RTCM 的動作。另外 VTIME 跟 VMIN 是兩個有趣的參數，可以自行 google 如何設定。
這邊使用自訂的 GNSS 結構接取傳入的記憶體位址，然後宣告 device 跟 baudrate ，並作為引數呼叫 open_serial_port ，取得 open device 指向的 file name (幾乎所有的資料交換/傳輸機制，其實本質上都是一個 file)。
接著用 while loop 呼叫 read_port 持續對該 file 讀取資料，並放到宣告的 buffer 內。也因此在每次 loop 開始的時候需要用 bzero 清空 buffer。
完整程式碼：
// Thread
void* threadFunc(void* arg) {
// 宣告裝置跟儲存位置
MYGNSS* gnss = (MYGNSS*) arg;
const char * device = "/dev/ttyUSB0";
uint32_t baudrate = 115200;
char buff[1024];
// 開啟裝置
int fdgnss = open_serial_port(device, baudrate);
while (1) {
// 清空 buffer 並讀取資料，再用 strtok 切割語句
bzero(buff, 1024);
ssize_t received = read_port(fdgnss, buff, 1024);
char * token = strtok(buff, "\n");
while(token) {
// 找出語句中逗號跟星號的位置，並計算 checksum
int comma[20] = {0};
int count_comma = 0;
int starkey = 0;
int XOR = 0;
for(int i = 0; i < strlen(token); i++){
if(token[i] == '*'){
starkey = i;
break;
} else if(token[i] == ','){
comma[count_comma] = i;
count_comma += 1;
}
if(token[i] != '$'){
XOR ^= token[i];
}
}
// 如果語句非空，則判斷語句類型並取出目標數值
char target[20];
if(comma[1] - comma[0] > 1 ){
if(strcmp(_substr(target, token, 0, 6), "$GNGGA") == 0){
_substr(target, token, starkey + 1, 2);
int cs_num = (int)strtol(target, NULL, 16);
if(XOR == cs_num && comma[2] - comma[1] > 1) {
gnss->lat = (double)strtod(_substr(target, token, comma[1] + 1, 2), NULL) + (float)strtod(_substr(target, token, comma[1] + 3, 10),NULL) / 60;
gnss->lon = (double)strtod(_substr(target, token, comma[3] + 1, 3), NULL) + (float)strtod(_substr(target, token, comma[3] + 4, 10),NULL) / 60;
}
}
}
token = strtok(NULL, "\n");
if(token == NULL){
break;
}
}
}
}GNSS NMEA Buffer Process
因為 read_port 一次讀取的 buffer 包含多個 NMEA 語句， strtok 的作用是將 buffer 的內容用 “\n” 符號切割，並 “逐次” 回傳。所以 while 末尾加上：
token = strtok(NULL, "\n") 用 NULL 作為引數，strtok 才會將下一筆切割出的字串再放入 token。遞迴直到回傳的 token 是 NULL，代表本次 buffer 內容已經被完全處理完。 strtok 的介紹：
https://www.runoob.com/cprogramming/c-function-strtok.html
在開發測試的過程中，曾經發生 Windows Visual Studio 編譯運行都沒有問題，但在 Linux 環境下發生 Segmentation fault 。我先用 cmake Debug build ，然後用 gdb 運行我的程式 debug ，發現問題是出在 strtok 。
cmake -G Ninja -DCMAKE_BUILD_TYPE=Debug ..
cmake --build build/
gdb -q -ex run --args ./bin/XXX原因是 strtok 的迭代機制，不能處理 const char* 這種靜態記憶體位址，如果要處理透過引數傳入的 char 時，就會碰到這個問題。解決辦法也很簡單，就宣告一個 local 的動態位址 char [] 複製這筆資料，再給 strtok 處理。參考資料：
https://blog.csdn.net/qq_31985307/article/details/116106327
https://stackoverflow.com/questions/8957829/strtok-segmentation-fault
GNSS NMEA Sentence Spilt
逐一對 strtok 分割完的 NMEA 語句 (token) 做判讀 ，我是用比較笨的方法，loop 整條語句，先找出逗號跟星號的位置，並存放在陣列內。如果有兩個逗號位置相連，代表有欄位為空沒有值，一般就是 GNSS 尚未 fix 。 NMEA 介紹可以參考：
substr 是一個好用的函數，可以依據起始位置與長度，取出字串中對應的部分，放到傳入的第一個引數位址。 C 語言沒有，但可以透過 strncpy 實作：
char* _substr( char * dest, const char * src, unsigned int start, unsigned int cnt) {
strncpy(dest, src + start, cnt);
dest[cnt] = 0 ;
return dest;
}參考來源：
https://job.achi.idv.tw/2013/12/02/in-the-c-language-substr/
*WordPress JSON 碰到 “substr” 發生錯誤 「更新時發生錯誤。無效的 JSON 回應。」，導致文章無法儲存，所以本文中的 “substr” 全部用 “_substr”。
每一條 NMEA 語句用 substr 從位置 0 起算，取出長度為 6 的部分字串，再用 strcmp 做字串比對，找到我要的 $GNGGA 語句。根據 NMEA protocol 同樣用 substr 搭配對應的逗號位置，就可以找到經緯度數值。
取出經緯度數值的字串後，再用 strtod 轉成 double 就可以做數值運算。以緯度為例，需要注意其定義是 ddmm.mmmmm ，所以要以 “度 deg” 為單位的話，要先取出前兩個字元，再把剩餘 “分 minute” 的部分除以 60 加回去。
gnss->lat = (double)strtod(_substr(target, token, comma[1] + 1, 2), NULL) + (float)strtod(_substr(target, token, comma[1] + 3, 10),NULL) / 60;
gnss->lon = (double)strtod(_substr(target, token, comma[3] + 1, 3), NULL) + (float)strtod(_substr(target, token, comma[3] + 4, 10),NULL) / 60;NMEA Checksum
NMEA checksum 的定義是：不包括語句起始的 “$” 字符，對每一個字元做 XOR ，直到語句內的 “*” 字符(不包括)。
int XOR ^= token[i];取出語句內星號後面兩個字元，就是該語句 Checksum 的正確值。透過 strtol long int 將其轉換成 16 進位，並與 XOR 的結果數值相等時，表示語句傳輸沒有錯誤。因此對 NMEA 語句取值，除了判斷欄位是否為空以外，還會比對 checksum 。
_substr(t, token, starkey + 1, 2);
int cs_num = (int)strtol(t, NULL, 16);
if(XOR == cs_num && comma[2] - comma[1] > 1) {
...至此就是一套獲取 NMEA 數據的完整流程。
C++ 版本
文末附上 C++ 版本，概念邏輯是一樣的，只是 C++ 有不同的函數可以使用，同樣也是一份倉促的程式碼，但功能是 OK 的：
#include <iostream>
#include <iomanip>
#include <math.h>
#include <string.h>
#include <sstream>
using namespace std;
string NmeaProcess(string test){
int comma[20] = {0};
int lat_d; float lat_m; double lat_s; double lat_ld;
int gps_gsv_message_count; int gps_gsv_message_number; int gps_total_sats_in_view;
int gps_prn_number[3][4] = {0};
int gps_elevation[3][4] = {0};
int gps_azimuth[3][4] = {0};
int gps_snr[3][4] = {0};
int count = 0;
for(int i = 0; i < test.size(); i++){
if(test[i] == ','){
comma[count] = i;
count += 1;
}
}
if(comma[1] - comma[0] > 1 ){
if(test._substr(0, 6) == "$GPGGA"){
if(comma[2] - comma[1] > 1 ){
lat_d = stoi(test._substr(comma[1] + 1, 2));
lat_m = stof(test._substr(comma[1] + 3, 2));
lat_s = stod(test._substr(comma[1] + 5, 5)) * 60;
lat_ld = stod(test._substr(comma[1] + 1, 9)) / 100;
}
int XOR = 0;
for (int i = 0; i < test.length(); i++) {
if (test[i] == '*') break;
if (test[i] != '$'){
XOR ^= test[i];
}
}
stringstream stream;
stream << hex << XOR;
string cs_estim( stream.str() );
string cs = test._substr(comma[13] + 2, 2);
if(cs_estim.compare(cs) == 0) {
printf("equal\n");
}
}
if(test._substr(0, 6) == "$GPGSV"){
if(comma[2] - comma[1] > 1 ){
gps_gsv_message_count = stoi(test._substr(comma[0] + 1, 1));
gps_gsv_message_number = stoi(test._substr(comma[1] + 1, 1));
gps_total_sats_in_view = stoi(test._substr(comma[2] + 1, 2));
for(int x=0; x < 4; x++) {
gps_prn_number[gps_gsv_message_number-1][x] = stoi(test._substr(comma[3+x*4] + 1, 2));
gps_elevation[gps_gsv_message_number-1][x] = stoi(test._substr(comma[4+x*4] + 1, 2));
gps_azimuth[gps_gsv_message_number-1][x] = stoi(test._substr(comma[5+x*4] + 1, 3));
gps_snr[gps_gsv_message_number-1][x] = stoi(test._substr(comma[6+x*4] + 1, 2));
}
}
int XOR = 0;
for (int i = 0; i < test.length(); i++) {
if (test[i] == '*') break;
if (test[i] != '$'){
XOR ^= test[i];
}
}
stringstream stream;
stream << hex << XOR;
string cs_estim( stream.str() );
string cs = test._substr(comma[18] + 4, 2);
if(cs_estim.compare(cs) == 0) {
printf("equal\n");
}
}
}
string tmp1 ("Lat: ");
if(test._substr(0, 6) == "$GPGGA"){
cout << lat_d << "\n"; cout << lat_m << "\n"; cout<< setprecision(8) << lat_s << "\n";
tmp1 += (to_string(lat_d)); tmp1 += ("d "); tmp1 += (to_string(lat_m)); tmp1 += ("' "); tmp1 += (to_string(lat_s)); tmp1 += ("\" "); tmp1 += test[comma[2] + 1];
string tmp2 ("Lat: "); tmp2 += (to_string(lat_ld)); tmp2 += ("d "); tmp2 += test[comma[2] + 1];
cout<< tmp1 << "\n"; cout << tmp2 << "\n";
string nmea_json = "{""type"": ""Feature"",""geometry"": {""type"": ""Point"",""coordinates"": [";
nmea_json += (to_string(lat_ld));
nmea_json += "]},""properties"": { ""name"": ""Dinagat Islands""}}";
cout << nmea_json << "\n";
}
return tmp1;
}
在〈GNSS NMEA 處理 -【C 語言學習筆記】〉中有 2 則留言
留言功能已關閉。