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

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

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

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

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

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

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