Exemplo n.º 1
0
void QgsNMEAConnection::processVTGSentence( const char* data, int len )
{
  nmeaGPVTG result;
  if ( nmea_parse_GPVTG( data, len, &result ) )
  {
    mLastGPSInformation.speed = result.spk;
  }
}
Exemplo n.º 2
0
/**
 * Parse a string and store the results in the nmeaINFO structure
 *
 * @param parser a pointer to the parser
 * @param s the string
 * @param len the length of the string
 * @param info a pointer to the nmeaINFO structure
 * @return the number of packets that were parsed
 */
int nmea_parse(nmeaPARSER * parser, const char * s, int len, nmeaINFO * info) {
  int sentences_count = 0;
  int charIndex = 0;

  assert(parser);
  assert(s);
  assert(info);

  for (charIndex = 0; charIndex < len; charIndex++) {
    bool sentence_read_successfully = nmea_parse_sentence_character(parser, &s[charIndex]);
    if (sentence_read_successfully) {
      enum nmeaPACKTYPE sentence_type = nmea_parse_get_sentence_type(&parser->buffer.buffer[1], parser->buffer.length - 1);
      switch (sentence_type) {
        case GPGGA:
          if (nmea_parse_GPGGA(parser->buffer.buffer, parser->buffer.length, parser->sentence_parser.has_checksum, &parser->sentence.gpgga)) {
            sentences_count++;
            nmea_GPGGA2info(&parser->sentence.gpgga, info);
          }
          break;

        case GPGSA:
          if (nmea_parse_GPGSA(parser->buffer.buffer, parser->buffer.length, parser->sentence_parser.has_checksum, &parser->sentence.gpgsa)) {
            sentences_count++;
            nmea_GPGSA2info(&parser->sentence.gpgsa, info);
          }
          break;

        case GPGSV:
          if (nmea_parse_GPGSV(parser->buffer.buffer, parser->buffer.length, parser->sentence_parser.has_checksum, &parser->sentence.gpgsv)) {
            sentences_count++;
            nmea_GPGSV2info(&parser->sentence.gpgsv, info);
          }
          break;

        case GPRMC:
          if (nmea_parse_GPRMC(parser->buffer.buffer, parser->buffer.length, parser->sentence_parser.has_checksum, &parser->sentence.gprmc)) {
            sentences_count++;
            nmea_GPRMC2info(&parser->sentence.gprmc, info);
          }
          break;

        case GPVTG:
          if (nmea_parse_GPVTG(parser->buffer.buffer, parser->buffer.length, parser->sentence_parser.has_checksum, &parser->sentence.gpvtg)) {
            sentences_count++;
            nmea_GPVTG2info(&parser->sentence.gpvtg, info);
          }
          break;

        case GPNON:
        default:
          break;
      }
    }
  }

  return sentences_count;
}
Exemplo n.º 3
0
int nmea_parser_real_push(nmeaPARSER *parser, const char *buff, int buff_sz)
{
    int nparsed = 0, crc, sen_sz, ptype;
    nmeaParserNODE *node = 0;

    NMEA_ASSERT(parser && parser->buffer);

    /* clear unuse buffer (for debug) */
    /*
    memset(
        parser->buffer + parser->buff_use, 0,
        parser->buff_size - parser->buff_use
        );
        */

    /* add */
    if(parser->buff_use + buff_sz >= parser->buff_size)
        nmea_parser_buff_clear(parser);

    memcpy(parser->buffer + parser->buff_use, buff, buff_sz);
    parser->buff_use += buff_sz;

    /* parse */
    for(;;node = 0)
    {
        sen_sz = nmea_find_tail(
            (const char *)parser->buffer + nparsed,
            (int)parser->buff_use - nparsed, &crc);

        if(!sen_sz)
        {
            if(nparsed)
                memcpy(
                parser->buffer,
                parser->buffer + nparsed,
                parser->buff_use -= nparsed);
            break;
        }
        else if(crc >= 0)
        {
            ptype = nmea_pack_type(
                (const char *)parser->buffer + nparsed + 1,
                parser->buff_use - nparsed - 1);

            if(0 == (node = malloc(sizeof(nmeaParserNODE))))
                goto mem_fail;

            node->pack = 0;

            switch(ptype)
            {
            case GPGGA:
                if(0 == (node->pack = malloc(sizeof(nmeaGPGGA))))
                    goto mem_fail;
                node->packType = GPGGA;
                if(!nmea_parse_GPGGA(
                    (const char *)parser->buffer + nparsed,
                    sen_sz, (nmeaGPGGA *)node->pack))
                {
                    free(node->pack);
                    free(node);
                    node = 0;
                }
                break;
            case GPGSA:
                if(0 == (node->pack = malloc(sizeof(nmeaGPGSA))))
                    goto mem_fail;
                node->packType = GPGSA;
                if(!nmea_parse_GPGSA(
                    (const char *)parser->buffer + nparsed,
                    sen_sz, (nmeaGPGSA *)node->pack))
                {
                    free(node->pack);
                    free(node);
                    node = 0;
                }
                break;
            case GPGSV:
                if(0 == (node->pack = malloc(sizeof(nmeaGPGSV))))
                    goto mem_fail;
                node->packType = GPGSV;
                if(!nmea_parse_GPGSV(
                    (const char *)parser->buffer + nparsed,
                    sen_sz, (nmeaGPGSV *)node->pack))
                {
                    free(node->pack);
                    free(node);
                    node = 0;
                }
                break;
            case GPRMC:
                if(0 == (node->pack = malloc(sizeof(nmeaGPRMC))))
                    goto mem_fail;
                node->packType = GPRMC;
                if(!nmea_parse_GPRMC(
                    (const char *)parser->buffer + nparsed,
                    sen_sz, (nmeaGPRMC *)node->pack))
                {
                    free(node->pack);
                    free(node);
                    node = 0;
                }
                break;
            case GPVTG:
                if(0 == (node->pack = malloc(sizeof(nmeaGPVTG))))
                    goto mem_fail;
                node->packType = GPVTG;
                if(!nmea_parse_GPVTG(
                    (const char *)parser->buffer + nparsed,
                    sen_sz, (nmeaGPVTG *)node->pack))
                {
                    free(node->pack);
                    free(node);
                    node = 0;
                }
                break;
            default:
                free(node);
                node = 0;
                break;
            };

            if(node)
            {
                if(parser->end_node)
                    ((nmeaParserNODE *)parser->end_node)->next_node = node;
                parser->end_node = node;
                if(!parser->top_node)
                    parser->top_node = node;
                node->next_node = 0;
            }
        }

        nparsed += sen_sz;
    }

    return nparsed;

mem_fail:
    if(node)
        free(node);

    nmea_error("Insufficient memory!");

    return -1;
}
Exemplo n.º 4
0
int nmea_parser_real_push(nmeaPARSER *parser, const char *buff, int buff_sz)
{
    int nparsed = 0, crc, sen_sz, ptype;
    nmeaParserNODE *node = 0;

    NMEA_ASSERT(parser && parser->buffer);

    /* clear unuse buffer (for debug) */
    /*
    memset(
        parser->buffer + parser->buff_use, 0,
        parser->buff_size - parser->buff_use
        );
        */
    /* add */
    if(parser->buff_use + buff_sz >= parser->buff_size)
        nmea_parser_buff_clear(parser);

    memcpy(parser->buffer + parser->buff_use, buff, buff_sz);
    parser->buff_use += buff_sz;

    /* parse */
    for(;;node = 0)
    {
        // return number of byte to packet tail. If find an invalid trailer or and invalid crc return 0.
        // if it is a valid message the function return > 0 with a crc >= 0
        sen_sz = nmea_find_tail(
            (const char *)parser->buffer + nparsed,
            (int)parser->buff_use - nparsed, &crc);

	//printf("sen_sz: %i\n", sen_sz);
        if(!sen_sz)
        {
            if(nparsed)
                memcpy(
                parser->buffer,
                parser->buffer + nparsed,
                parser->buff_use -= nparsed);
            break;
        }
        else if(crc >= 0)
        {
            ptype = nmea_pack_type(
                (const char *)parser->buffer + nparsed + 1,
                parser->buff_use - nparsed - 1);

            if(0 == (node = malloc(sizeof(nmeaParserNODE))))
                goto mem_fail;

            node->pack = 0;

            switch(ptype)  // ADD: add packet type case here
            {
            case GPGGA:
                if(0 == (node->pack = malloc(sizeof(nmeaGPGGA))))
                    goto mem_fail;
                node->packType = GPGGA;
                if(!nmea_parse_GPGGA(
                    (const char *)parser->buffer + nparsed,
                    sen_sz, (nmeaGPGGA *)node->pack))
                {
                    free(node);
                    node = 0;
                }
                break;
            case GPGSA:
                if(0 == (node->pack = malloc(sizeof(nmeaGPGSA))))
                    goto mem_fail;
                node->packType = GPGSA;
                if(!nmea_parse_GPGSA(
                    (const char *)parser->buffer + nparsed,
                    sen_sz, (nmeaGPGSA *)node->pack))
                {
                    free(node);
                    node = 0;
                }
                break;
            case GPGSV:
                if(0 == (node->pack = malloc(sizeof(nmeaGPGSV))))
                    goto mem_fail;
                node->packType = GPGSV;
                if(!nmea_parse_GPGSV(
                    (const char *)parser->buffer + nparsed,
                    sen_sz, (nmeaGPGSV *)node->pack))
                {
                    free(node);
                    node = 0;
                }
                break;
            case GPRMC:
                if(0 == (node->pack = malloc(sizeof(nmeaGPRMC))))
                    goto mem_fail;

                node->packType = GPRMC;
                if(!nmea_parse_GPRMC(
                    (const char *)parser->buffer + nparsed,
                    sen_sz, (nmeaGPRMC *)node->pack))
                {
                    free(node);
                    node = 0;
                }
                break;
            case GPVTG:
                if(0 == (node->pack = malloc(sizeof(nmeaGPVTG))))
                    goto mem_fail;
                node->packType = GPVTG;
                if(!nmea_parse_GPVTG(
                    (const char *)parser->buffer + nparsed,
                    sen_sz, (nmeaGPVTG *)node->pack))
                {
                    free(node);
                    node = 0;
                }
                break;

            case HCHDG:
              if(0 == (node->pack = malloc(sizeof(nmeaHCHDG))))
                goto mem_fail;
              node->packType = HCHDG;
              if(!nmea_parse_HCHDG(
                (const char *)parser->buffer + nparsed,
                sen_sz, (nmeaHCHDG *)node->pack))
              {
                  free(node);
                  node = 0;
              }
              break;

            case HCHDT:
              if(0 == (node->pack = malloc(sizeof(nmeaHCHDT))))
                goto mem_fail;
              node->packType = HCHDT;
              if(!nmea_parse_HCHDT(
                (const char *)parser->buffer + nparsed,
                sen_sz, (nmeaHCHDT *)node->pack))
              {
                  free(node);
                  node = 0;
              }
              break;

            case TIROT:
              if(0 == (node->pack = malloc(sizeof(nmeaTIROT))))
                goto mem_fail;
              node->packType = TIROT;
              if(!nmea_parse_TIROT(
                (const char *)parser->buffer + nparsed,
                sen_sz, (nmeaTIROT *)node->pack))
              {
                  free(node);
                  node = 0;
              }
              break;

            case YXXDR:
              if(0 == (node->pack = malloc(sizeof(nmeaYXXDR))))
                goto mem_fail;
              node->packType = YXXDR;
              if(!nmea_parse_YXXDR(
                (const char *)parser->buffer + nparsed,
                sen_sz, (nmeaYXXDR *)node->pack))
              {
                  free(node);
                  node = 0;
              }
              break;
	      
            default:
                free(node);
                node = 0;
                break;
            };

            if(node)
            {
                if(parser->end_node)
                    ((nmeaParserNODE *)parser->end_node)->next_node = node;
                parser->end_node = node;
                if(!parser->top_node)
                    parser->top_node = node;
                node->next_node = 0;
            }
        }

        nparsed += sen_sz;
    }

    return nparsed;

mem_fail:
    if(node)
        free(node);

    nmea_error("Insufficient memory!");

    return -1;
}