C++ NMEA_ASSERT函数代码示例

/**
 * \brief Parse XDR packet from buffer.
 * @param buff a constant character pointer of packet buffer.
 * @param buff_sz buffer size.
 * @param pack a pointer of packet which will filled by function.
 * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
 */
int nmea_parse_YXXDR(const char *buff, int buff_sz, nmeaYXXDR *pack)
{
    NMEA_ASSERT(buff && pack);

    memset(pack, 0, sizeof(nmeaYXXDR));

    nmea_trace_buff(buff, buff_sz);

    if(8 != nmea_scanf(buff, buff_sz,
        "$YXXDR,%C,%f,%C,%s,%C,%f,%C,%s*",
        &(pack->angular_pitch), &(pack->pitch),
        &(pack->degrees_pitch), &(pack->pitch_id),
        &(pack->angular_roll), &(pack->roll),
        &(pack->degrees_roll), &(pack->roll_id)))
    {
        nmea_error("YXXDR parse error!");
        return 0;
    }

    if( pack->angular_pitch != 'A' ||
        pack->degrees_pitch != 'D' ||
        memcmp(pack->pitch_id, "PTCH", 4) != 0 ||
        pack->angular_roll != 'A' ||
        pack->degrees_roll != 'D' ||
        memcmp(pack->roll_id, "ROLL", 4) != 0
      )
    {
        nmea_error("YXXDR parse error (format error)!");
        return 0;
    }
    return 1;
}

/**
 * \brief Define packet type by header (nmeaPACKTYPE).
 * @param buff a constant character pointer of packet buffer.
 * @param buff_sz buffer size.
 * @return The defined packet type
 * @see nmeaPACKTYPE
 */
int nmea_pack_type( const char *buff, int buff_sz )
{
  static const char *P_HEADS[] =
  {
    "GPGGA",
    "GPGSA",
    "GPGSV",
    "GPRMC",
    "GPVTG",
    "GNRMC",
  };

  NMEA_ASSERT( buff );

  if ( buff_sz < 5 )
    return GPNON;
  else if ( 0 == memcmp( buff, P_HEADS[0], 5 ) )
    return GPGGA;
  else if ( 0 == memcmp( buff, P_HEADS[1], 5 ) )
    return GPGSA;
  else if ( 0 == memcmp( buff, P_HEADS[2], 5 ) )
    return GPGSV;
  else if ( 0 == memcmp( buff, P_HEADS[3], 5 ) )
    return GPRMC;
  else if ( 0 == memcmp( buff, P_HEADS[4], 5 ) )
    return GPVTG;
  else if ( 0 == memcmp( buff, P_HEADS[5], 5 ) )
    return GPRMC;

  return GPNON;
}

/**
 * \brief Fill nmeaINFO structure by XDR packet data.
 * @param pack a pointer of packet structure.
 * @param info a pointer of summary information structure.
 */
void nmea_YXXDR2info(nmeaYXXDR *pack, nmeaINFO *info)
{
    NMEA_ASSERT(pack && info);

    info->pitch_osc = pack->pitch;
    info->roll_osc = pack->roll;
}

/**
 * \brief Parse GGA packet from buffer.
 * @param buff a constant character pointer of packet buffer.
 * @param buff_sz buffer size.
 * @param pack a pointer of packet which will filled by function.
 * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
 */
int nmea_parse_GPGGA(const char *buff, int buff_sz, nmeaGPGGA *pack)
{
    char time_buff[NMEA_TIMEPARSE_BUF];

    NMEA_ASSERT(buff && pack);

    memset(pack, 0, sizeof(nmeaGPGGA));

    nmea_trace_buff(buff, buff_sz);

    if(14 != nmea_scanf(buff, buff_sz,
        "$GPGGA,%s,%f,%C,%f,%C,%d,%d,%f,%f,%C,%f,%C,%f,%d*",
        &(time_buff[0]),
        &(pack->lat), &(pack->ns), &(pack->lon), &(pack->ew),
        &(pack->sig), &(pack->satinuse), &(pack->HDOP), &(pack->elv), &(pack->elv_units),
        &(pack->diff), &(pack->diff_units), &(pack->dgps_age), &(pack->dgps_sid)))
    {
        nmea_error("GPGGA parse error!");
        return 0;
    }

    if(0 != _nmea_parse_time(&time_buff[0], (int)strlen(&time_buff[0]), &(pack->utc)))
    {
        nmea_error("GPGGA time parse error!");
        return 0;
    }

    return 1;
}

/**
 * \brief Parse GSV packet from buffer.
 * @param buff a constant character pointer of packet buffer.
 * @param buff_sz buffer size.
 * @param pack a pointer of packet which will filled by function.
 * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
 */
int nmea_parse_GPGSV(const char *buff, int buff_sz, nmeaGPGSV *pack)
{
    int nsen, nsat;

    NMEA_ASSERT(buff && pack);

    memset(pack, 0, sizeof(nmeaGPGSV));

    nmea_trace_buff(buff, buff_sz);

    nsen = nmea_scanf(buff, buff_sz,
        "$GPGSV,%d,%d,%d,"
        "%d,%d,%d,%d,"
        "%d,%d,%d,%d,"
        "%d,%d,%d,%d,"
        "%d,%d,%d,%d*",
        &(pack->pack_count), &(pack->pack_index), &(pack->sat_count),
        &(pack->sat_data[0].id), &(pack->sat_data[0].elv), &(pack->sat_data[0].azimuth), &(pack->sat_data[0].sig),
        &(pack->sat_data[1].id), &(pack->sat_data[1].elv), &(pack->sat_data[1].azimuth), &(pack->sat_data[1].sig),
        &(pack->sat_data[2].id), &(pack->sat_data[2].elv), &(pack->sat_data[2].azimuth), &(pack->sat_data[2].sig),
        &(pack->sat_data[3].id), &(pack->sat_data[3].elv), &(pack->sat_data[3].azimuth), &(pack->sat_data[3].sig));

    nsat = (pack->pack_index - 1) * NMEA_SATINPACK;
    nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK;
    nsat = nsat * 4 + 3 /* first three sentence`s */;

    if(nsen < nsat || nsen > (NMEA_SATINPACK * 4 + 3))
    {
        nmea_error("GPGSV parse error!");
        return 0;
    }

    return 1;
}

/**
 * \brief Destroy parser object
 */
void nmea_parser_destroy(nmeaPARSER *parser)
{
    NMEA_ASSERT(parser && parser->buffer);
    free(parser->buffer);
    nmea_parser_queue_clear(parser);
    memset(parser, 0, sizeof(nmeaPARSER));
}

/**
 * \brief Fill nmeaINFO structure by GSA packet data.
 * @param pack a pointer of packet structure.
 * @param info a pointer of summary information structure.
 */
void nmea_GPGSA2info(nmeaGPGSA *pack, nmeaINFO *info)
{
  int i, j, nuse = 0;

  NMEA_ASSERT(pack && info);

  info->fix = pack->fix_type;
  info->PDOP = pack->PDOP;
  info->HDOP = pack->HDOP;
  info->VDOP = pack->VDOP;

  for(i = 0; i < NMEA_MAXSAT; ++i)
  {
    if(pack->sat_prn[i])
    {
      nuse++;
      for(j = 0; j < info->satinfo.inview; ++j)
      {
        if(pack->sat_prn[i] == info->satinfo.sat[j].id)
        {
          info->satinfo.sat[j].in_use = 1;
          break;
        }
      }
    }
  }

  info->satinfo.inuse = nuse;
  info->smask |= GPGSA;
}

/**
 * \brief Define packet type by header (nmeaPACKTYPE).
 * @param buff a constant character pointer of packet buffer.
 * @param buff_sz buffer size.
 * @return The defined packet type
 * @see nmeaPACKTYPE
 */
int nmea_pack_type(const char *buff, int buff_sz)
{
    static const char *pheads[] = {
        "GPGGA",
        "GPGSA",
        "GPGSV",
        "GPRMC",
        "GPVTG",
    };

    NMEA_ASSERT(buff);

    if(buff_sz < 5)
        return GPNON;
    else if(0 == memcmp(buff, pheads[0], 5))
        return GPGGA;
    else if(0 == memcmp(buff, pheads[1], 5))
        return GPGSA;
    else if(0 == memcmp(buff, pheads[2], 5))
        return GPGSV;
    else if(0 == memcmp(buff, pheads[3], 5))
        return GPRMC;
    else if(0 == memcmp(buff, pheads[4], 5))
        return GPVTG;

    return GPNON;
}

/**
 * \brief Fill nmeaINFO structure by GSV packet data.
 * @param pack a pointer of packet structure.
 * @param info a pointer of summary information structure.
 */
void nmea_GPGSV2info(nmeaGPGSV *pack, nmeaINFO *info)
{
    int isat, isi, nsat;

    NMEA_ASSERT(pack && info);

    if(pack->pack_index > pack->pack_count ||
        pack->pack_index * NMEA_SATINPACK > NMEA_MAXSAT)
        return;

    if(pack->pack_index < 1)
        pack->pack_index = 1;

    // This field contain data only in the first instance
    if(pack->pack_index == 1)
      info->satinfo.inview = pack->sat_count;

    nsat = (pack->pack_index - 1) * NMEA_SATINPACK;
    nsat = (nsat + NMEA_SATINPACK > pack->sat_count)?pack->sat_count - nsat:NMEA_SATINPACK;

    for(isat = 0; isat < nsat; ++isat)
    {
        isi = (pack->pack_index - 1) * NMEA_SATINPACK + isat;
        info->satinfo.sat[isi].id = pack->sat_data[isat].id;
        info->satinfo.sat[isi].elv = pack->sat_data[isat].elv;
        info->satinfo.sat[isi].azimuth = pack->sat_data[isat].azimuth;
        info->satinfo.sat[isi].sig = pack->sat_data[isat].sig;
    }

    info->smask |= GPGSV;
}

/**
 * \brief Clear packets queue into parser
 * @return true (1) - success
 */
int nmea_parser_queue_clear(nmeaPARSER *parser)
{
    NMEA_ASSERT(parser);
    while(parser->top_node)
        nmea_parser_drop(parser);
    return 1;
}

/**
 * \brief Parse VTG packet from buffer.
 * @param buff a constant character pointer of packet buffer.
 * @param buff_sz buffer size.
 * @param pack a pointer of packet which will filled by function.
 * @return 1 (true) - if parsed successfully or 0 (false) - if fail.
 */
int nmea_parse_GPVTG(const char *buff, int buff_sz, nmeaGPVTG *pack)
{
    NMEA_ASSERT(buff && pack);

    memset(pack, 0, sizeof(nmeaGPVTG));

    nmea_trace_buff(buff, buff_sz);

    if(8 != nmea_scanf(buff, buff_sz,
        "$GPVTG,%f,%C,%f,%C,%f,%C,%f,%C*",
        &(pack->dir), &(pack->dir_t),
        &(pack->dec), &(pack->dec_m),
        &(pack->spn), &(pack->spn_n),
        &(pack->spk), &(pack->spk_k)))
    {
        nmea_error("GPVTG parse error!");
        return 0;
    }

    if( pack->dir_t != 'T' ||
        pack->dec_m != 'M' ||
        pack->spn_n != 'N' ||
        pack->spk_k != 'K')
    {
        nmea_error("GPVTG parse error (format error)!");
        return 0;
    }

    return 1;
}

/**
 * \brief Fill nmeaINFO structure by DBT packet data.
 * @param pack a pointer of packet structure.
 * @param info a pointer of summary information structure.
 */
void nmea_SDDBT2info(nmeaSDDBT *pack, nmeaINFO *info)
{
	NMEA_ASSERT(pack && info);
	info->depthinfo.depth_f = pack->depth_f;
	info->depthinfo.depth_F = pack->depth_F;
	info->depthinfo.depth_M = pack->depth_M;
	info->smask |= SDDBT;
}

/**
 * \brief Fill nmeaINFO structure by HCHDG packet data.
 * @param pack a pointer of packet structure.
 * @param info a pointer of summary information structure.
 */
void nmea_HCHDG2info(nmeaHCHDG *pack, nmeaINFO *info)
{
    NMEA_ASSERT(pack && info);

    info->magnetic_sensor_heading = pack->mag_heading;
    info->magnetic_sensor_deviation = ((pack->ew_deviation == 'E')?pack->mag_deviation:-(pack->mag_deviation));
    info->magnetic_sensor_variation = ((pack->ew_variation == 'E')?pack->mag_variation:-(pack->mag_variation));
}

/**
 * \brief Fill nmeaINFO structure by HCHDT packet data.
 * @param pack a pointer of packet structure.
 * @param info a pointer of summary information structure.
 */
void nmea_HCHDT2info(nmeaHCHDT *pack, nmeaINFO *info)
{
    NMEA_ASSERT(pack && info);

    if('T' == pack->t_flag)
      info->magnetic_sensor_heading_true = pack->direction;
    
    info->smask |= HCHDT;
}

/**
 * \brief Fill nmeaINFO structure by VTG packet data.
 * @param pack a pointer of packet structure.
 * @param info a pointer of summary information structure.
 */
void nmea_GPVTG2info(nmeaGPVTG *pack, nmeaINFO *info)
{
    NMEA_ASSERT(pack && info);

    info->direction = pack->dir;
    info->declination = pack->dec;
    info->speed = pack->spk;
    info->smask |= GPVTG;
}