as_gps.c

This a support library and assorted sample programs for interfacing

  result += (char)((*bcd>>4)&0xf)*10 + (char)(*bcd&0xf);
  return (sign * result);
}

double xbcd2lat(unsigned char *bcd)
{
  char digit;
  int deg, sign=1;
  double result;

  if( (char)((*bcd>>4)&0xf) == 0xB)
	sign=-1;
  digit = (char)(*bcd&0xf);
  if(digit==10) digit=0;  /* leading blank */
  deg = 100 * digit;
printf("debug: deg=%d, byte=%x\n", deg, *bcd);
  bcd++;
  deg += (char)((*bcd>>4)&0xf)*10 + (char)(*bcd&0xf);
  bcd++;
  result = (double)deg + (double)((char)((*bcd>>4)&0xf)*10 + (char)(*bcd&0xf))/60.0;
printf("debug: deg=%d, byte=%x, result=%8.5f\n", deg, *bcd, result);
  bcd++;
  result += (double)((char)((*bcd>>4)&0xf)*10 + (char)(*bcd&0xf))/3600.0;
  return (sign * result);
}

int as_gps_parse_gps_condition(unsigned char *data, AS_GPS_Condition *cnd)
{
  unsigned i;
  struct tm tm;

  cnd->status = data[2];
  cnd->healthy = bcd2uchar(data[3]);
  cnd->visible = bcd2uchar(data[4]);
  cnd->height = xbcd2int(data+5);
  cnd->lgt = xbcd2lat(data+8);
  cnd->lat = xbcd2lat(data+14);

#ifndef CENTURY_BIAS
#define CENTURY_BIAS 0
#endif
  tm.tm_year = bcd2uchar(data[20]) + CENTURY_BIAS;  /* #define to 100 after 1/1/2000 */
  tm.tm_mon  = bcd2uchar(data[21])-1;
  tm.tm_mday = bcd2uchar(data[22]);

  tm.tm_hour = bcd2uchar(data[23]);
  tm.tm_min  = bcd2uchar(data[24]);
  tm.tm_sec  = bcd2uchar(data[25]);
  tm.tm_isdst= -1;  /* dunno if DST  */

  cnd->time = mktime(&tm) - as_UTC_offset;  /* compensate for mktime()'s TZ correction */
  for (i=0; i<8; i++) {
    cnd->id_lvl[i].id = bcd2uchar(data[26+2*i]);
    cnd->id_lvl[i].level = bcd2uchar(data[27+2*i]);
  }
  cnd->diag[0] = data[42];
  cnd->diag[1] = data[43];
}

void as_gps_print_gps_condition(const AS_GPS_Condition *cnd)
{
  int i;

  printf("GPS Condition:  ");
  switch(cnd->status & 0xF)
  {
    case 0x0: printf("No Decoded Sats"); break;
    case 0x1: printf("1 Decoded Sat"); break;
    case 0x2: printf("2 Decoded Sats"); break;
    case 0x3: printf("2D Fix"); break;
    case 0x4: printf("3D Fix"); break;
    case 0x5: printf("Acceleration > 1G"); break;
    case 0x9: printf("Receiver error"); break;
  }
  printf(",  %d healthy, %d sats visible\n", cnd->healthy, cnd->visible);
  printf("  Lat: %02.7f, Long: %02.7f,", cnd->lat, cnd->lgt);
  printf(" Height: %5dm,", cnd->height);
  printf(" %s", asctime(localtime(&cnd->time)));

  for(i=0; i<8; i++) {
    printf("  Sat: %2d,  Lvl: %2d\n", cnd->id_lvl[i].id, cnd->id_lvl[i].level);
  }
  if ((cnd->diag[0]>>4)&0xf == 1) printf("  cold start RAM check NG1");
  else if ((cnd->diag[0]>>4)&0xf == 2) printf("  cold start RAM check NG2");
  if (cnd->diag[0]&0xf == 1) printf("  cold start RTC err 1");
  else if (cnd->diag[0]&0xf == 2) printf("  cold start RTC err 2");
  if (cnd->diag[1]&0xf == 1) printf("   Almanac too old");
}
#endif

int as_gps_parse_receive_sats(unsigned char *data, AS_GPS_ReceiveSats *rcv)
{
  unsigned entry;

  rcv->seconds = data[2];
  rcv->almanac_age = data[43];

  for(entry = 0; entry < 8; entry++)
  {
     rcv->sat_entries[entry].id = data[3+5*entry];  /* 0 means not used */
     rcv->sat_entries[entry].condition = data[4+5*entry];
     rcv->sat_entries[entry].level = data[5+5*entry];  /* in .2 units */
     rcv->sat_entries[entry].azimuth = data[6+5*entry] * 360 / 256;
     rcv->sat_entries[entry].elevation = data[6+5*entry] * 180 / 256;
  }

  for(entry = 0; entry < 32; entry++)
  {
     int byteoffset = entry/8;
     int bitoffset = entry%8;
     int mask = 0x01<<bitoffset;

     rcv->health[entry] = (data[44+byteoffset] & mask) ? 'H' : 'U';
  }
}

void as_gps_print_sat_entries(const AS_GPS_SatEntry *sat_entries)
{
  unsigned entry;

  printf("  Sat Info:\n");

  for(entry = 0; entry < 8; entry++)
  {
    AS_GPS_SatEntry sat_entry = sat_entries[entry];

    if(!sat_entry.valid) continue;

    printf("    %d: Sat: %2d Cond: ", entry,
      sat_entry.id);

    switch(sat_entry.condition)
    {
      case AS_GPS_SAT_CONDITION_SEARCH: 
        printf("Searching         "); break;
      case AS_GPS_SAT_CONDITION_TRACKING: 
        printf("Tracking          "); break;
      case AS_GPS_SAT_CONDITION_DECODED:  
        printf("Decoded (Not Used)"); break;
      case AS_GPS_SAT_CONDITION_DECODED_USED: 
        printf("Decoded (Used)    "); break;
    }
    
    printf(" Lvl: ");

    switch(sat_entry.level)
    {
      case AS_GPS_SAT_LEVEL_0:    printf("0"); break;
      case AS_GPS_SAT_LEVEL_LT10: printf("< 10"); break;
      case AS_GPS_SAT_LEVEL_LT20: printf("< 20"); break;
      case AS_GPS_SAT_LEVEL_GT20: printf("> 20"); break;
    }
 
    printf("\n");
  }
}

void as_gps_print_receive_sats(const AS_GPS_ReceiveSats *rcv)
{
  unsigned entry;

  printf("Almanac Age %d days;", rcv->almanac_age);

  printf(" Sat Health 1..32: %.4s %.4s %.4s %.4s  %.4s %.4s %.4s %.4s\n",
    rcv->health, rcv->health+4, rcv->health+8, rcv->health+12,
    rcv->health+16, rcv->health+20, rcv->health+24, rcv->health+28);

  for(entry = 0; entry < 8; entry++)
  {
    AS_GPS_SatInfoEntry sat_entry = rcv->sat_entries[entry];
    if (sat_entry.id == 0) continue; /* slot not used */
    printf("    Sat: %2d Cond: svacc=%2d,", sat_entry.id,
	                       (sat_entry.condition>>4)&0xf);

    switch(sat_entry.condition&0xf)
    {
      case 1: 
        printf("Searching         "); break;
      case 2: 
        printf("Tracking          "); break;
      case 4:  
        printf("Decoded (Not Used)"); break;
      case 8: 
        printf("Decoded (Used)    "); break;
      default:
        printf("Unknown bit config"); break;
    }
    
    printf(" Lvl: %2d", sat_entry.level);
    printf("  Az: %3d El: %3d\n", sat_entry.azimuth, sat_entry.elevation);
  }

}

void as_gps_print_position_data(const AS_GPS_PositionData *pos)
{
  printf("Position Data: ");

  if(pos->status == 0xFF)
  {
    printf("Diagnosis Info Available\n");
    return;
  }

  switch(pos->status & 0xF)
  {
    case 0x0: printf("No Decoded Sats"); break;
    case 0x1: printf("1 Decoded Sat"); break;
    case 0x2: printf("2 Decoded Sats"); break;
    case 0x3: printf("2D Fix"); break;
    case 0x4: printf("3D Fix"); break;
    case 0xF: printf("Acceleration > 1G"); break;
  }

  printf(", %d Healthy Sats", pos->healthy);

  if(pos->status & 0x10)
    printf(" -- Sats Changing");

  printf("\n");
 
//  if(avg_cnt)
//    printf("  Lat: %02.7f(%02.7f), Lgt: %02.7f(%02.7f)\n", 
//    pos->lat, avg_lat / (double) avg_cnt,
//    pos->lgt, avg_lgt / (double) avg_cnt);
//  else
    printf("  Lat: %02.7f, Long: %02.7f,", pos->lat, pos->lgt);
  printf(" Height: %04.1fm,", pos->hgt);
  printf(" %s", asctime(localtime(&pos->time)));

  printf("  Heading: %3.6f +/- %2.6f deg cw from N\n", pos->heading,
	 pos->heading_error);
  printf("  Speed: %3.6f +/- %3.6f km/h\n", pos->speed, pos->speed_error);
  printf("  HDOP: %02.1f, VDOP: %02.1f;", pos->HDOP, pos->VDOP);
  printf("  Error Ellipse: Long: %dm, Short: %dm\n", pos->long_error, pos->short_error);
  printf("  Unknown use: %2x %2x %2x %2x\n", pos->unknown[0], pos->unknown[1],
	pos->unknown[2], pos->unknown[3]);

  as_gps_print_sat_entries(pos->sat_entries);
}

int as_gps_get_data(unsigned char *data, int length)
{
  int count;
  unsigned char checksum = 0;


  for(count = 2; count < length; count++)
    read(as_gps_file, &data[count], 1);

  for(count = 0; count < length - 1; count++)
    checksum += data[count];

  if(checksum)
  {
    printf("Checksum Error: Cmd:%02x, E:%d, E:%02x, A:%02x\n", 
      data[0], checksum, checksum, data[length-2]);
    return 1;
  }
  else
    return 0;
}

int as_gps_send_cmd(unsigned char cmd, unsigned char *data, int length)
{ 
  int count;
  int result;

  data[0] = data[1] = cmd;

  data[length-2] = 0; // reset checksum
  for(count = 0; count < length - 2; count++)
    data[length-2] -= data[count];

  data[length-1] = 0xd; // terminate

#ifdef DEBUGWRITE
  printf("Sending: ");
#endif

  result = write(as_gps_file, data, length);

#if DEBUGWRITE
  for(count = 0; count < length; count++)
  {
    printf(" %02x", data[count]);
  }

  printf(" %d\n", result);
#endif

  return 0;
}

unsigned char bcd2uchar(unsigned char bcd)
{
  return (bcd & 0xF) + ((bcd >> 4) * 10);
}