garminnmea.cc

机器人仿真软件

  //fflush(stdout);
  
  while(!(ptr = strchr((const char*)nmea_buf, NMEA_START_CHAR)))
  {
    nmea_buf_len=0;
    memset(nmea_buf,0,sizeof(nmea_buf));
    if(FillBuffer())
      return(-1);
  }

  nmea_buf_len = strlen(ptr);
  memmove(nmea_buf,ptr,strlen(ptr)+1);

  //printf("found start char:[%s]:[%d]\n", nmea_buf,nmea_buf_len);
  //fflush(stdout);
  
  while(!(ptr = strchr((const char*)nmea_buf, NMEA_END_CHAR)))
  {
    if(nmea_buf_len >= sizeof(nmea_buf) - 1)
    {
      // couldn't get an end char and the buffer is full.
      PLAYER_WARN("couldn't find an end character; discarding data");
      memset(nmea_buf,0,sizeof(nmea_buf));
      nmea_buf_len=0;
      buf = NULL;
      return(0);
    }
    if(FillBuffer())
      return(-1);
  }
  
  //printf("found end char:[%s]\n", nmea_buf);
  //fflush(stdout);
  
  sentlen = nmea_buf_len - strlen(ptr) + 1;
  if(sentlen > len - 1)
  {
    PLAYER_WARN1("NMEA sentence too long (%d bytes); truncating", sentlen);
    sentlen = len - 1;
  }
  
  //printf("reading checksum\n");
  //fflush(stdout);
  
  // copy in all but the leading $ and trailing carriage return and line feed
  if (sentlen > 3)
  {
    strncpy(buf,nmea_buf+1,sentlen-3);
    buf[sentlen-3] = '\0';
  }
  else
  {
    PLAYER_WARN("NMEA sentence is too short; ignoring");
    buf[0] = '\0';
  }

  //printf("got: [%s]\n", buf);
  //fflush(stdout);

  // verify the checksum, if present.  two hex digits are the XOR of all the 
  // characters between the $ and *.
  if((ptr2 = strchr((const char*)buf,NMEA_CHKSUM_CHAR)) && (strlen(ptr2) == 3))
  {
    ////printf("ptr2 %s\n", ptr2);
    ////fflush(stdout);

    strncpy(tmp,ptr2+1,2);
    tmp[2]='\0';
    chksum = strtol(tmp,NULL,16);
    
    oursum=0;
    for(int i=0;i<(int)(strlen(buf)-strlen(ptr2));i++)
      oursum ^= buf[i];

    // HACK
    //chksum = oursum;

    if(oursum != chksum)
    {
      PLAYER_WARN2("checksum mismatch (0x%2x != 0x%2x); discarding sentence",
                   oursum, chksum);
      buf=NULL;
    }
    else
    {
      // strip off checksum
      *ptr2='\0';
    }
  }
  else
  {
    PLAYER_WARN1("no checksum: [%s]", buf);
    buf = NULL;
  }

  memmove(nmea_buf,ptr+1,strlen(ptr));
  nmea_buf_len -= sentlen;
  nmea_buf[nmea_buf_len]='\0';

  //printf("done reading sentence\n");
  //fflush(stdout);

  return(0);
}


/*
 * Read more data into the buffer 'nmea_buf', starting 'nmea_buf_len' chars
 * in, and not overrunning the total length.  'nmea_buf' will be
 * NULL-terminated.
 */
int
GarminNMEA::FillBuffer()
{
  int numread=0;
  int readcnt=0;

  while(numread<=0)
  {
    if((numread = read(gps_fd,nmea_buf+nmea_buf_len,
                       sizeof(nmea_buf)-nmea_buf_len-1)) < 0)
    {      
      if(!gps_fd_blocking && (errno == EAGAIN))
      {
        if(readcnt >= GPS_STARTUP_CYCLES)
          return(-1);
        else
        {
          readcnt++;
          usleep(GPS_STARTUP_CYCLE_USEC);
        }
      }
      else
      {
        PLAYER_ERROR1("read(): %s", strerror(errno));
        return(-1);
      }
    }
  }
  nmea_buf_len += numread;
  nmea_buf[nmea_buf_len] = '\0';

  /*
  for (int i = 0; i < nmea_buf_len; i++)
    printf("%02X ", (int) (unsigned char) nmea_buf[i]);
  printf("\n");
  */
  
  return(0);
}


/*
 * Write a sentence to the GPS unit
 */
int
GarminNMEA::WriteSentence(const char *buf, size_t len)
{
  int s;
  size_t sent, remaining;
  
	sent = 0;
  remaining = len;

  while (sent < len)
  {
    s = ::write(this->gps_fd, buf + sent, remaining);
    if (s < 0)
    {
      PLAYER_ERROR1("error writing to GPS [%s]", strerror(errno));
      return -1;
    }
    sent += s;
    remaining -= s;
	}

  return 0;
}


/*
 * Get the next field from an NMEA sentence.
 */
char*
GarminNMEA::GetNextField(char* field, size_t len, const char* ptr)
{
  char* start;
  char* end;
  size_t fieldlen;

  if(strlen(ptr) < 2 || !(start = strchr(ptr, ',')))
  {
    field[0]='\0';
    return(NULL);
  }

  if(!(end = strchr(start+1, ',')))
    fieldlen = strlen(ptr) - (start - ptr);
  else
    fieldlen = end - start - 1;

  if(fieldlen > (len - 1))
  {
    PLAYER_WARN("NMEA field too big; truncating");
    fieldlen = len - 1;
  }

  strncpy(field,start+1,fieldlen);
  field[fieldlen] = '\0';

  return(end);
}


/*
 * Parse an NMEA sentence, doing something appropriate with each message in
 * which we're interested. 'buf' should be NULL-terminated.
 */
int
GarminNMEA::ParseSentence(const char* buf)
{
  const char* ptr = buf;
  char tmp[8];
  
  if(!buf)
    return(0);
  if (strlen(buf) < 5)
    return 0;

  // copy in the sentence header, for checking
  strncpy(tmp,buf,5);
  tmp[5]='\0';

  //printf("sentence [%s]\n", tmp);
  //fflush(stdout);
      
  // the GGA msg has the position data that we want
  if(!strcmp(tmp,NMEA_GPGGA))
    ParseGPGGA(ptr);

  // the RMC msg has the date and time
  if(!strcmp(tmp,NMEA_GPRMC))
    ParseGPRMC(ptr);

  // the PGRME msg has the error
  if(!strcmp(tmp,NMEA_PGRME))
    ParsePGRME(ptr);

  return(0);
}


/*
 * Parse the GPGGA sentence, which has lat/lon.
 */
int GarminNMEA::ParseGPGGA(const char *buf)
{
  const char *ptr = buf;
  char field[32];
  char tmp[8];
  double degrees, minutes, arcseconds;
  double lat, lon;
  double utm_e, utm_n;
  
  //printf("got GGA (%s)\n", buf);
  //fflush(stdout);

  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);

  // first field is time of day. Skip
  
  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);

  // 2nd field is latitude.  first two chars are degrees.
  strncpy(tmp,field,2);
  tmp[2]='\0';
  degrees = atoi(tmp);
  // next is minutes
  minutes = atof(field+2);

  arcseconds = ((degrees * 60.0) + minutes) * 60.0;

  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);
  // 3rd field is N or S for north or south. adjust sign accordingly.
  if(field[0] == 'S')
    arcseconds *= -1;

  lat = arcseconds / 3600.0;
  data.latitude = (int32_t)rint(lat * 1e7);

  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);
  // 4th field is longitude.  first 3 chars are degrees.
  strncpy(tmp,field,3);
  tmp[3]='\0';
  degrees = atoi(tmp);
  // next is minutes
  minutes = atof(field+3);

  arcseconds = ((degrees * 60.0) + minutes) * 60.0;
    
  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);
  // 5th field is E or W for east or west. adjust sign accordingly.
  if(field[0] == 'W')
    arcseconds *= -1;

  lon = arcseconds / 3600.0;
  data.longitude = (int32_t)rint(lon * 1e7);

  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);
  // 6th field is fix quality
  data.quality = atoi(field);
  
  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);
  // 7th field is number of sats in view
  data.num_sats = atoi(field);

  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);
  // 8th field is HDOP.  we'll multiply by ten to make it an integer.
  data.hdop = (uint16_t)rint(atof(field) * 10);

  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);
  // 9th field is altitude, in meters.  we'll convert to mm.
  data.altitude = (int32_t)rint(atof(field) * 1000.0);

  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);
  // 10th field tells us the reference for measuring altitude. e.g., 'M' is
  // mean sea level.  ignore it.

  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);
  // 11th field is "height of geoid above WGS84 ellipsoid" ignore it.

  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);
  // 12th field tells us the reference for the above-mentioned geode.  e.g.,
  // 'M' is mean sea level.  ignore it.

  // fields 13 and 14 are for DGPS. ignore them.


  ////printf("%f %f %d\n", lat, lon, (int) data.quality);

  // Update the filtered lat/lon, and see if the new values are any
  // good
  filter_lat = filter_a * lat + (1 - filter_a) * filter_lat;
  filter_lon = filter_a * lon + (1 - filter_a) * filter_lon;

  // Reject outliers
  filter_good = true;
  if (fabs(lat - filter_lat) > filter_thresh)
    filter_good = false;
  if (fabs(lon - filter_lon) > filter_thresh)
    filter_good = false;

  if (!filter_good)
  {
    PLAYER_WARN4("rejected: %f %f (should be %f %f)\n", lat, lon, filter_lat, filter_lon);
    return -1;
  }
  
  // Compute the UTM coordindates
  UTM(lat, lon, &utm_e, &utm_n);
  //printf("utm: %.3f %.3f\n", utm_e, utm_n);
  
  data.utm_e = (int32_t) rint(utm_e * 100);
  data.utm_n = (int32_t) rint(utm_n * 100);

  Publish(device_addr, NULL, PLAYER_MSGTYPE_DATA, PLAYER_GPS_DATA_STATE, &data,sizeof(player_gps_data_t),NULL);

  return 0;
}


/*
 * Parse the GPRMC sentence, which has date/time
 */
int GarminNMEA::ParseGPRMC(const char *buf)
{
  const char *ptr = buf;
  char field[32];
  char tmp[8];
  struct tm tms;
  time_t utc;
  
  //printf("got RMC (%s)\n", buf);
  //fflush(stdout);
  
  memset(&tms, 0, sizeof(tms));
      
  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);

  if (strlen(field) < 6)
  {
    PLAYER_WARN("short time field; ignoring");
    return -1;
  }

  // first field is time of day. HHMMSS
  strncpy(tmp,field,2);
  tmp[2]='\0';
  tms.tm_hour = atoi(tmp);

  strncpy(tmp,field + 2,2);
  tmp[2]='\0';
  tms.tm_min = atoi(tmp);

  strncpy(tmp,field + 4,2);
  tmp[2]='\0';
  tms.tm_sec = atoi(tmp);

  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);
  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);
  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);
  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);
  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);
  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);
  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);
  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);

  if (strlen(field) < 6)
  {
    PLAYER_WARN("short date field; ignoring");
    return -1;
  }
  
  // fifth field has the date DDMMYY
  strncpy(tmp,field,2);
  tmp[2]='\0';
  tms.tm_mday = atoi(tmp);
  
  strncpy(tmp,field + 2,2);
  tmp[2]='\0';
  tms.tm_mon = atoi(tmp) - 1;
  
  strncpy(tmp,field + 4,2);
  tmp[2]='\0';
  tms.tm_year = 100 + atoi(tmp);
  
  //printf("%02d %02d %02d : %02d %02d %02d \n",
  //       tms.tm_year, tms.tm_mon, tms.tm_mday,
  //       tms.tm_hour, tms.tm_min, tms.tm_sec);
  
  // Compute to time since the epoch.  We only get it to the nearest
  // second, unfortunately.
  utc = mktime(&tms);
  
  data.time_sec = (uint32_t) utc;
  data.time_usec = (uint32_t) 0;

  /* Dont write here
  // Need to parse to sentences before write data
  read_count += 1;
  if (filter_good && filter_good >= 2)
  {
    PutData(&data,sizeof(player_gps_data_t),0,0);
    read_count = 0;
  }
  */
  
  return 0;
}


/*
 * Parse the PGRME sentence, which has esimated position error.
 * This is a proprietry Garmin message
 */
int GarminNMEA::ParsePGRME(const char *buf)
{
  const char *ptr = buf;
  char field[32];
  double err;
  
  //printf("got PGRME (%s)\n", buf);
  //fflush(stdout);
  
  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);

  // First field is horizontal error
  err = atof(field);
  data.err_horz = (uint32_t) (err * 1000);
  
  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);

  // Should be "M"
  if (strcmp(field, "M") != 0)
  {
    PLAYER_WARN1("invalid unit code [%s]", field);
    return -1;
  }

  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);

  // Third field is vertical error
  err = atof(field);
  data.err_vert = (uint32_t) (err * 1000);

  if(!(ptr = GetNextField(field, sizeof(field), ptr)))
    return(-1);

  // Should be "M"
  if (strcmp(field, "M") != 0)
  {
    PLAYER_WARN1("invalid unit code [%s]", field);
    return -1;
  }

  /* Dont write here
  PutData(&data,sizeof(player_gps_data_t),0,0);
  */

  return 0;
}


/*
 * Read DGPS sentence from the UDP socket
 */
int GarminNMEA::ReadSocket(char *buf, size_t len)
{
  int plen;
  
  plen = recv(this->dgps_fd, buf, len, 0);
  if (plen < 0)
  {
    PLAYER_ERROR1("error reading from udp socket [%s]", strerror(errno));
    return -1;
  }
  return plen;
}


/*
 * Utility functions to convert geodetic to UTM position
 */
void GarminNMEA::UTM(double lat, double lon, double *x, double *y)
{
	// constants
	const static double m0 = (1 - UTM_E2/4 - 3*UTM_E4/64 - 5*UTM_E6/256);
	const static double m1 = -(3*UTM_E2/8 + 3*UTM_E4/32 + 45*UTM_E6/1024);
	const static double m2 = (15*UTM_E4/256 + 45*UTM_E6/1024);
	const static double m3 = -(35*UTM_E6/3072);

	// compute the central meridian
	int cm = (lon >= 0.0) ? ((int)lon - ((int)lon)%6 + 3) : ((int)lon - ((int)lon)%6 - 3);

	// convert degrees into radians
	double rlat = lat * M_PI/180;
	double rlon = lon * M_PI/180;
	double rlon0 = cm * M_PI/180;

	// compute trigonometric functions
	double slat = sin(rlat);
	double clat = cos(rlat);
	double tlat = tan(rlat);

	// decide the flase northing at origin
	double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S;

	double T = tlat * tlat;
	double C = UTM_EP2 * clat * clat;
	double A = (rlon - rlon0) * clat;
	double M = WGS84_A * (m0*rlat + m1*sin(2*rlat) + m2*sin(4*rlat) + m3*sin(6*rlat));
	double V = WGS84_A / sqrt(1 - UTM_E2*slat*slat);

	// compute the easting-northing coordinates
	*x = UTM_FE + UTM_K0 * V * (A + (1-T+C)*pow(A,3)/6 + (5-18*T+T*T+72*C-58*UTM_EP2)*pow(A,5)/120);
	*y = fn + UTM_K0 * (M + V * tlat * (A*A/2 + (5-T+9*C+4*C*C)*pow(A,4)/24 + (61-58*T+T*T+600*C-330*UTM_EP2)*pow(A,6)/720));

  return;
}