Попал в руки OpenCPU модуль L218. Кроме всего прочего, он имеет на борту GNSS. Соответственно возникла потребность в разборе NMEA фрейма. Поставляемое SDK заточено под «C code», и т.к править кучу Makefile-в было лениво, попытки использовать готовые проекты провалились.
Тогда пришлось написать свою реализацию разбора NMEA сообщений. На эту не самую умную и интересную задачу, пришлось потратить свободный вечер. Будет приятно, если кому-то понадобиться решить похожую задачу и этот пример с этим поможет.
Имеющийся модуль посылал 5 типов сообщений: GGA, GSA, GSV, VTG, RMC.
Подробное описание можно почитать в спецификации. Например здесь.
Преступим непосредственно к разбору
Создатим структуру для храния формата разбора и типа фрейма:
typedef struct {
char header[15];
eGnssNmeaMsgType type;
char format[NMEA_MESSAGE_MAX_LEN];
}sNmeaParcer;Наполним данными:
sNmeaParcer nmeaParcer[NMEA_MESSAGE_MAX] = {
{"GGA", GGA__, "%2d%2d%2d.%*3d,%f,%c,%f,%c,%d,%d,%f,%f,%c"},
{"GSA", GSA__, "%c,%i,%*i,%*i,%*i,%*i,%*i,%*i,%*i,%*i,%*i,%*i,%*i,%*i,%f,%f,%f"},
{"GSV", GSV__, "%3d,%3d,%3d,%3d,%f,%f,%i"},
{"VTG", VTG__, "%f,%c,%*d,%*c,%f"},
{"RMC", RMC__, "%2d%2d%2d.%*3d,%*c,%f,%c,%f,%c,%f,%f,%2d%2d%2d"}
};И создадим тестовые сообщения (нам же надо это как-то проверить):
char nmea_buffer[NMEA_MESSAGE_MAX][NMEA_MESSAGE_MAX_LEN] = {
{"$GNGGA,094650.000,5548.6477,N,03750.0997,E,1,11,1.00,274.4,M,14.3,M,,*76"},
{"$GPGSA,A,3,03,17,19,12,14,01,32,11,22,,,,1.35,1.00,0.91*07"},
{"$BDGSV,1,1,04,09,41,070,28,12,36,240,,11,31,308,31,17,10,072,*64"},
{"$GNVTG,190.77,T,,M,15.8,N,0.00,K,A*2B"},
{"$GNRMC,093005.000,A,5548.6223,N,03750.1092,E,0.4,0.6,291117,0,0,A*7C"}
};Разбор состоит из одной единственной функции:
bool gpsParceNmeaRaw(char *pData) {
bool result = false;
uint8_t msgType = 0;
char data[NMEA_MESSAGE_MAX_LEN] = {0};
uint16_t len = 0;
//
int dd = 0;
float ss = 0;
char mode_c = 'A'; // automatic
int mode_int = 1; // no fix
float vdop, hdop, pdop;
struct tm timeStruct;
float lat, lon, height;
char lat_sign, lon_sign, height_units;
int precision, satellites;
int message_counter = 0;
int message_current = 0;
int total_sv_view = 0;
int sv_prn_number = 0;
float elevation_degrees = 0;
float azimuth_degrees = 0;
int snr_db_value = 0;
//
float track_made_good = 0;
char fixet_text_T = 0;
//
float speed = 0;
float course_over_ground = 0;
char *pChar = 0;
len = strlen(pData);
if(len == 0) {
return false;
}
uint16_t index = 0;
uint16_t repIndex = 0;
while(index < len) {
if(repIndex > 0) {
if(pData[index] == ',') {
if(data[repIndex-1] == ',') {
data[repIndex++] = '0';
}
}
}
data[repIndex++] = pData[index];
index++;
}
pData = data;
for(msgType = 0; msgType < NMEA_MESSAGE_MAX; msgType++) {
if(strstr (pData, (char*)nmeaParcer[msgType].header) != NULL) {
printf("Header found [%s]rn", nmeaParcer[msgType].header);
pChar = strchr(pData, ',');
if(pChar != NULL) {
switch(nmeaParcer[msgType].type) {
case GGA__: // $GNGGA,094650.000,5548.6477,N,03750.0997,E,1,11,1.00,274.4,M,14.3,M,,*76
sscanf(pChar+1, nmeaParcer[msgType].format,
&timeStruct.tm_hour, &timeStruct.tm_min, &timeStruct.tm_sec,
&lat, &lat_sign, &lon, &lon_sign, &precision, &satellites, &hdop, &height, &height_units);
dd = (int)((float)(lat)/100); //37
ss = (float)(lat) - (dd * 100); // 50,1122
lat = dd + (ss/60); //31.6227773333333333
if(lat != 0) {
if (lat_sign == 'S') {
lat = (0-lat);
}
}
dd = (int)((float)(lon)/100); //37
ss = (float)(lon) - (dd * 100); // 50,1122
lon = dd + (ss/60); //31.6227773333333333
if(lon != 0) {
if (lon_sign=='S') {
lon = (0-lon);
}
}
printf("NMEA: parcing raw: %d,%d,%d,%f,%c,%f,%c,%d,%d,%f,%f,%drn",
timeStruct.tm_hour, timeStruct.tm_min, timeStruct.tm_sec,
lat, lat_sign, lon, lon_sign, precision, satellites, hdop, height, height_units);
return true;
break;
case GSA__ : // $GPGSA,A,3,03,17,19,12,14,01,32,11,22,,,,1.35,1.00,0.91*07
sscanf(pChar+1, nmeaParcer[msgType].format, &mode_c, &mode_int,
&pdop, &hdop, &vdop);
printf("NMEA: parcing raw: %c,%i,%f,%f,%frn", mode_c, mode_int, pdop, hdop, vdop);
return true;
break;
case GSV__ : // $BDGSV,1,1,04,09,41,070,28,12,36,240,,11,31,308,31,17,10,072,*64
sscanf(pChar+1, nmeaParcer[msgType].format,
&message_counter, &message_current, &total_sv_view, &sv_prn_number,
&elevation_degrees, &azimuth_degrees, &snr_db_value);
printf("NMEA: parcing raw: %i,%i,%i,%i,%f,%f,%irn", message_counter, message_current, total_sv_view, sv_prn_number, elevation_degrees, azimuth_degrees, snr_db_value);
return true;
break;
case VTG__ : // $GNVTG,190.77,T,,M,15.12,N,0.00,K,A*2B
sscanf(pChar+1, nmeaParcer[msgType].format, &track_made_good, &fixet_text_T, &speed); //!!!
printf("NMEA: parcing raw: %f,%c,%frn", track_made_good, fixet_text_T, speed);
return true;
break;
case RMC__: // $GNRMC,093005.000,A,5548.6223,N,03750.1092,E,0.4,0.46,291117,0,0,A*7C
sscanf(pChar+1, nmeaParcer[msgType].format,
&timeStruct.tm_hour, &timeStruct.tm_min, &timeStruct.tm_sec,
&lat, &lat_sign, &lon, &lon_sign, &speed, &course_over_ground,
&timeStruct.tm_mday, &timeStruct.tm_mon, &timeStruct.tm_year);
printf("NMEA: parcing raw: %d.%d.%d %f %frn",
timeStruct.tm_mday, timeStruct.tm_mon, timeStruct.tm_year,
speed, course_over_ground);
return true;
break;
case UNKNOWN__ :
break;
default : break;
}
}
}
}
return result;
}Работает так:
int main(void) {
int i = 0;
char nmea_buffer[NMEA_MESSAGE_MAX][NMEA_MESSAGE_MAX_LEN] = {
{"$GNGGA,094650.000,5548.6477,N,03750.0997,E,1,11,1.00,274.4,M,14.3,M,,*76"},
{"$GPGSA,A,3,03,17,19,12,14,01,32,11,22,,,,1.35,1.00,0.91*07"},
{"$BDGSV,1,1,04,09,41,070,28,12,36,240,,11,31,308,31,17,10,072,*64"},
{"$GNVTG,190.77,T,,M,15.8,N,0.00,K,A*2B"},
{"$GNRMC,093005.000,A,5548.6223,N,03750.1092,E,0.4,0.6,291117,0,0,A*7C"}
};
printf("NMEA: startParcerrn");
while(i<NMEA_MESSAGE_MAX) {
printf("NMEA: message -%srn", nmea_buffer[i]);
gpsParceNmeaRaw((char*)nmea_buffer[i]);
i++;
}
printf("NMEA: ENDrn");
return EXIT_SUCCESS;
}В процессе оказалось, sscanf не умеет пропускать параметры с неопределенным типом. Что стало небольшой проблемой, т.к. многие gnss модули целиком пропускают параметр, в случае его невалидности.
Например: 1,,,5
Пришлось данные копировать в промежуточный буфер — char data[NMEA_MESSAGE_MAX_LEN]
И принудительно вставлять нули.
Автор: Khomin
