Си: парсинг GSV GPS nmea сообщение

gpsПо просьбам трудящихся опишу как доработать парсер из заметки AVR: подключаем GPS приёмник для работы с GSV сообщениями.  Исходный код программы приведен в статье и прикреплен в виде архива.

Описание формата GSV сообщения

GSV сообщение содержит детальную информация о спутниках. Так как длина одного NMEA сообщения не может превышать 80 символов, то в одном сообщении может быть информация только о 4-х спутниках. Поэтому, если видимых спутников больше чем 4, то информация выводится по частям в нескольких сообщениях.

Для каждого спутника в GSV сообщении содержится следующая информация:

  • SNR (Signal to Noise Ration) — соотношение сигнал/шум для принимаемого со спутника навигационного сигнала.
  • угол возвышения, в градусах
  • азимут в градусах

Пример GSV сообщения:

$GPGSV,4,1,15,01,06,067,38,08,24,095,42,09,33,100,46,11,10,048,40*72

где:
$ — признак начала сообщения
GP — данные от системы GPS
GSV — признак того, что это GSV сообщение
4 – количество сообщений
1 – номер сообщения
15 – количество видимых спутников
01 – номер спутника
06 – угол возвышения, в градусах (для спутника №1)
067 – азимут в градусах (для спутника №1)
38 – SNR, уровень сигнала (для спутника №1)
08 – номер спутника
24 – угол возвышения, в градусах (для спутника №8)
095 – азимут в градусах (для спутника №8)
42 – SNR, уровень сигнала (для спутника №8)
09 – номер спутника
33 – угол возвышения, в градусах (для спутника №9)
100 – азимут в градусах (для спутника №9)
46 – SNR, уровень сигнала (для спутника №9)
09 – номер спутника
33 – угол возвышения, в градусах (для спутника №9)
100 – азимут в градусах (для спутника №9)
46 – SNR, уровень сигнала (для спутника №9)
11 – номер спутника
10 – угол возвышения, в градусах (для спутника №11)
048 – азимут в градусах (для спутника №11)
40 – SNR, уровень сигнала (для спутника №11)
* — признак конца сообщения
72 — контрольная сумма

Пример программы для парсинга GSV сообщений

#define BUF_SIZE 81    //размер буфера
char buf[BUF_SIZE];    //буфер для приема сообщений
unsigned char buf_cnt; //кол-во принятых символов
 
#define ARRAY_SIZE( array )  (sizeof(array)/sizeof(array[0]))
 
//структура описывающая информацию об одном GPS/ГЛОНАСС спутнике.
typedef struct _sat {
   uint8_t num;        //номер спутника
   uint8_t  elevation; //Высота, градусы, (90° - максимум)
   uint16_t azimuth;   //Азимут (0-360°)
   uint8_t  snr;       //Отношение сигнал/шум
} sat_t;
 
#define MAX_SAT  36   //максимально возможное кол-во GPS/ГЛОНАСС спутников
 
//структура для хранения информации о всех видимых GPS/ГЛОНАСС спутниках
typedef struct  _gsv_data  {
  sat_t satellites[ MAX_SAT ]; //массив для хранения информации о GPS/ГЛОНАСС спутниках
  uint8_t valid;               //признак достоверности данных
  uint8_t cnt;                 //кол-во спутников
} gsv_data_t;
 
 
gsv_data_t gps_gsv_data; //данные по gps
gsv_data_t glonass_gsv_data; //данные по глонасс
 
 
//структура, описывающая nmea сообщение
typedef struct _nmea_message {
  char * id; //идентификатор сообщения
  char (*parser)( char *buf, void *data ); //парсер сообщения
  void *data; //результат парсинга сообщения
} nmea_message_t;
 
//массив обработчиков, данные от GPS и ГЛОНАСС будет разбирать один и тот же парсер, но результат хранится в разных структурах
nmea_message_t nmea_messages[] =
{
 {
   .id = "GPGSV",
   .parser = nmea_gsv_parser,
   .data = &gps_gsv_data, //результат парсинга данных от GPS
 },
 {
   .id = "GLGSV",
   .parser = nmea_gsv_parser,
   .data = &glonass_gsv_data, //результат парсинга данных от ГЛОНАСС
 },
};
 
 
 
int main( void  )
{
  buf_cnt = 0;
  int c;
  while( (c = getc( stdin )) != EOF ) {
    if( c == '\r' || c == '\n' ) {  //если конец строки, то парсим данные в буфере
      buf[buf_cnt] = '\0';
      nmea_parser( buf );
      buf_cnt = 0;
 
    } 
    else if( buf_cnt < BUF_SIZE ) {
      buf[buf_cnt] = c;
      buf_cnt++;
    }      
  }
 
  int i;
  //в main выводим информацию о спутниках.
 
  //по системе GPS
  if( gps_gsv_data.valid ) { 
    for( i=0; i < gps_gsv_data.cnt; i++ ) {
      printf( "GPS  N=%u  elevation=%u azimuth=%u snr=%u\r\n", 
	  gps_gsv_data.satellites[i].num,
	  gps_gsv_data.satellites[i].elevation,                    
	  gps_gsv_data.satellites[i].azimuth,         
	  gps_gsv_data.satellites[i].snr );
    } 
  }
 
  //по системе глонасс
  if( glonass_gsv_data.valid ) {
    for( i=0; i < glonass_gsv_data.cnt; i++ ) {
      printf( "GLONASS N=%u  elevation=%u azimuth=%u snr=%u\r\n", 
	  glonass_gsv_data.satellites[i].num,
	  glonass_gsv_data.satellites[i].elevation,                    
	  glonass_gsv_data.satellites[i].azimuth,         
	  glonass_gsv_data.satellites[i].snr );
    } 
  }
  return 0;
}

Функция для парсинга nmea сообщений

unsigned char nmea_parser( char* buf )
{
  unsigned char i;
 
  //первый символ должен быть = $
  if( *buf++ != '$' ) {
    return -1;
  }
 
  //проверка контрольной суммы
  if( nmea_check_crc( buf )  ) {
    return -1; //CRC fail
  }
 
  for( i = 0; i< ARRAY_SIZE( nmea_messages ); i++ ) {
    if( strncmp( buf, nmea_messages[i].id, strlen( nmea_messages[i].id ) ) == 0 ) {
      if( nmea_messages[i].parser ) {
        buf = nmea_next_field( buf );
        return nmea_messages[i].parser( buf, nmea_messages[i].data );
      }
      return -1;
    }
  }
 
  return -1;
}

Функция для парсинга gsv сообщения

char nmea_gsv_parser( char *buf, void *data )
{
  gsv_data_t* gsv = (gsv_data_t*)data;
 
  uint8_t msg_cnt;
  uint8_t msg_num;
 
 
  //получаем кол-во сообщений
  msg_cnt = atoi( buf );
  buf = nmea_next_field( buf );
 
  //получаем номер текущего  сообщения
  msg_num = atoi( buf );
  buf = nmea_next_field( buf );
 
  if( msg_num == 1 ) {
    //если первое сообщение, то обнуляем все данные
    memset( data, 0, sizeof( gsv_data_t ) );
  }
 
  //пропускаем поле с данными о кол-ве спутников
  buf = nmea_next_field( buf );
 
  //пока есть информация о спутниках, парсим её
  do {
    gsv->satellites[ gsv->cnt].num = atoi( buf );
    buf = nmea_next_field( buf ); 
 
    gsv->satellites[ gsv->cnt].elevation = atoi( buf );
    buf = nmea_next_field( buf ); 
 
    gsv->satellites[ gsv->cnt ].azimuth = atoi( buf );
    buf = nmea_next_field( buf ); 
 
    gsv->satellites[ gsv->cnt ].snr = atoi( buf );
    buf = nmea_next_field( buf ); 
 
    gsv->cnt++;
 
  } while( buf != NULL );
 
 
  if( msg_num == msg_cnt ) {
    gsv->valid = 1; //если распарсили последнее сообщение, то выставляем признак готовности данных
  }
 
  return 0;
}

Функция для поиска следующего поля nmea сообщения

// если следующее поле не найдено, то она возвращает NULL
char *nmea_next_field( char* buf)
{
  while( *buf++ != ',' ) {    
   if( *buf == '*' || *buf == '\0' ) {
     return NULL;
   }
  }
  return buf;
}

Функция для преобразования из текстового hex формата в бинарные данные

unsigned char hex2bin( char c )
{
  if( ( c >= 'A') && ( c <= 'F') ) {     
    return c - 'A' + 0xA;       
  }      
  else if( ( c >= 'a') && ( c <= 'f') ) {
    return c - 'a' + 0xA;    
  }
  else {
    return c - '0';    
  }
}

Функция для проверки контрольной суммы nmea сообщения.

char nmea_check_crc( char *buf )
{
  unsigned char crc, calc_crc = 0;
 
  while( *buf != '*' ) {
    if( *buf == '\0' ) {
      return -1;
    }    
    calc_crc ^= *buf++;
  }
 
  crc = hex2bin( *(buf+2) ) |  hex2bin( *(buf+1) ) << 4;
 
  if ( crc  !=  calc_crc ) return -1;
 
  return 0;
}

Если собрать программу и подать ей на вход данные с GPS/ГЛОНАСС приемника, например такие:

$GPGSV,4,1,15,01,06,067,38,08,24,095,42,09,33,100,46,11,10,048,40*72
$GPGSV,4,2,15,15,54,278,47,17,41,140,47,18,13,312,41,24,23,289,41*7A
$GPGSV,4,3,15,26,62,192,48,28,56,057,48,33,18,235,37,37,33,193,00*7D
$GPGSV,4,4,15,39,33,188,00,40,29,150,00,41,16,121,39*47
$GLGSV,3,1,09,67,19,220,39,68,38,278,44,69,18,334,37,76,16,082,36*6A
$GLGSV,3,2,09,77,63,042,42,78,45,297,00,86,29,038,43,87,41,107,45*6E
$GLGSV,3,3,09,88,13,155,41*5A

то программа выведет:

GPS  N=1  elevation=6 azimuth=67 snr=38
GPS  N=8  elevation=24 azimuth=95 snr=42
GPS  N=9  elevation=33 azimuth=100 snr=46
GPS  N=11  elevation=10 azimuth=48 snr=40
GPS  N=15  elevation=54 azimuth=278 snr=47
GPS  N=17  elevation=41 azimuth=140 snr=47
GPS  N=18  elevation=13 azimuth=312 snr=41
GPS  N=24  elevation=23 azimuth=289 snr=41
GPS  N=26  elevation=62 azimuth=192 snr=48
GPS  N=28  elevation=56 azimuth=57 snr=48
GPS  N=33  elevation=18 azimuth=235 snr=37
GPS  N=37  elevation=33 azimuth=193 snr=0
GPS  N=39  elevation=33 azimuth=188 snr=0
GPS  N=40  elevation=29 azimuth=150 snr=0
GPS  N=41  elevation=16 azimuth=121 snr=39
GLONASS N=67  elevation=19 azimuth=220 snr=39
GLONASS N=68  elevation=38 azimuth=278 snr=44
GLONASS N=69  elevation=18 azimuth=334 snr=37
GLONASS N=76  elevation=16 azimuth=82 snr=36
GLONASS N=77  elevation=63 azimuth=42 snr=42
GLONASS N=78  elevation=45 azimuth=297 snr=0
GLONASS N=86  elevation=29 azimuth=38 snr=43
GLONASS N=87  elevation=41 azimuth=107 snr=45
GLONASS N=88  elevation=13 azimuth=155 snr=41

Архив с исходным текстом программы можно скачать тут

Запись опубликована в рубрике Великий и могучий Си. Добавьте в закладки постоянную ссылку.

Добавить комментарий

Ваш e-mail не будет опубликован.

Можно использовать следующие HTML-теги и атрибуты: <a href="" title=""> <abbr title=""> <acronym title=""> <b> <blockquote cite=""> <cite> <code> <del datetime=""> <em> <i> <q cite=""> <strike> <strong>