as_gps.c

This a support library and assorted sample programs for interfacing

#include "config.h"

#if defined (HAVE_SYS_TERMIOS_H)
#include <sys/termios.h>
#else
#if defined (HAVE_TERMIOS_H)
#include <termios.h>
#endif
#endif

#if defined (HAVE_TERMIO_H)
#include <termio.h>
#define USE_TERMIO 1
#define TIOCGETA        TCGETA
#define TIOCSETAF       TCSETAF

#ifndef ONLCR
#define ONLCR           ONLRET
#endif

#define termios         termio
#define tcflag_t        ushort
#endif

#include "as_gps.h"

int as_gps_file;

unsigned as_gps_snd_size[32] =
  {13, 8, 10, 5, 0, 9, 4, 4, 4, 4, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 4, 4, 5, 4, 0, 0, 0, 4, 4, 0, 0, 0, 0};

unsigned as_gps_rcv_size[32] =
  {4, 4, 4, 4, 0, 4, 4, 4, 4, 4, 0, 0, 0, 0, 0, 0,
   0, 0, 0, 11, 11, 46, 46, 0, 0, 0, 44, 50, 0, 0, 0, 0};

int as_gps_open(char *device)
{
  as_gps_file = open(device, O_RDWR);

  if (as_gps_file < 0) return 1;

  if (isatty(as_gps_file)) 
  {
    static struct termios ttyset;

    if (ioctl(as_gps_file, TIOCGETA, &ttyset) < 0)
      return (1);

#if defined (USE_TERMIO)
    ttyset.c_cflag = CBAUD & B9600;
#else
    ttyset.c_ispeed = B9600;
    ttyset.c_ospeed = B9600;
#endif
    ttyset.c_cflag |= (CSIZE & CS8) | CREAD | CLOCAL | PARENB | CRTSCTS;

    ttyset.c_iflag = ttyset.c_oflag = ttyset.c_lflag = (tcflag_t) 0;

    if (ioctl(as_gps_file, TIOCSETAF, &ttyset) < 0)
      return 1;
  }

  return 0;
}

int as_gps_close(void)
{
  close(as_gps_file);

  return 0;
}

int as_gps_handle_input(void **result_data)
{
  static int offset = 0;
  static unsigned char mode = 0x0;

  static unsigned char data[100];
  static unsigned char cur, last;

  int count;
  unsigned char checksum = 0;

  if(mode == 0x0) // not receiving a response yet
  {
    while(1)
    {
      last = cur;

      if(read(as_gps_file, &cur, 1) != 1)
        return 0;

      if((last == cur) && (cur >= 0xb0) && (cur <= 0xcb))
      { // start of a response
        mode = data[0] = data[1] = cur;
        offset = 2;
        break;
      }
    }
  }

  while(offset < as_gps_rcv_size[mode - 0xB0]) // grab response
  {
    if(read(as_gps_file, &data[offset], 1) != 1) return 0;
    offset++;
  }

  for(count = 0; count <  as_gps_rcv_size[mode - 0xB0] - 1; count++)
    checksum += data[count];

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

  switch(mode)
  {
    case 0xca: // Position Data 
      *result_data = (void *) malloc(sizeof(AS_GPS_PositionData));
      as_gps_parse_position_data(data, *result_data);
      break;

    case 0xc4: // Diagnosis
      *result_data = (void *) malloc(sizeof(AS_GPS_Diagnosis));
      as_gps_parse_diagnosis(data, *result_data);
      break;

    case 0xc3: // Receiver Mode
      *result_data = (void *) malloc(sizeof(AS_GPS_ReceiverMode));
      as_gps_parse_receiver_mode(data, *result_data);
      break;

    case 0xcb: /* Receive sat */
      *result_data = (void *) malloc(sizeof(AS_GPS_ReceiveSats));
      as_gps_parse_receive_sats(data, *result_data);
      break;

#ifdef INCLUDE_GPS_CONDITION
    case 0xc6: /* gps condition */
      *result_data = (void *) malloc(sizeof(AS_GPS_Condition));
      as_gps_parse_gps_condition(data, *result_data);
      break;
#endif

    default:
      printf("Unsupported cmd: '%x'\n", mode);
      mode = 0x0;
      return 0;
  }

  mode = 0x0;

  return data[0];
}

int as_gps_parse_diagnosis(char *data, AS_GPS_Diagnosis *diag)
{
  if(data[6] || data[7] || data[8])
    printf("Diagnosis: %02x %02x %02x\n", data[6],
       data[7], data[8]);
}

int as_gps_parse_receiver_mode(char *data, AS_GPS_ReceiverMode *rcv)
{
//     printf("Receiver Mode: %02x %02x %02x %02x %02x\n", 
//       data[2], data[3], data[4], data[5], data[6]);
  rcv->status = data[2];
  rcv->elevation_mask = data[3];
  rcv->signal_level_mask = data[4];  /* .2 units */
  rcv->PDOP_limit = data[5];
  rcv->smooth_level = data[6];
  rcv->hgt = (double)(data[7]<<8 | data[8]);
}

static time_t as_UTC_offset=-1L;
int as_gps_parse_position_data(unsigned char *data, AS_GPS_PositionData *pos)
{
  signed lat_tmp, lgt_tmp, hdg_tmp, speed_tmp;
  signed short hgt_tmp;
  unsigned entry, entries, sat_status;
  int sat;
  static int century_bias;
  struct tm tm;

#if BREAK_WHEN_DST_CHANGES
  if(as_UTC_offset == -1L) {  /* determine proper delta from UTC to localtime */
#endif
    time(&as_UTC_offset);  /* system's UTC value */
    memcpy(&tm, gmtime(&as_UTC_offset), sizeof(struct tm));
    tm.tm_isdst = -1;  /* gmtime may not set isdst right.  We force to "don't know" */
    century_bias = tm.tm_year - (tm.tm_year % 100);  /* 0, 100, ... */
    as_UTC_offset = mktime(&tm) - as_UTC_offset;  /* positive if W if Grenwich */
#if BREAK_WHEN_DST_CHANGES
  }
#endif

  pos->healthy = data[32];
  pos->status = data[2];
 
  lat_tmp = (data[3] << 24) | (data[4] << 16) |
            (data[5] << 8)  | data[6];

  pos->lat = (double) lat_tmp / (256.0 * 3600.0);

  
  lgt_tmp = (data[7] << 24) | (data[8] << 16) |
            (data[9] << 8)  | data[10];

  pos->lgt = (double) lgt_tmp / (256.0 * 3600.0);
  
  hgt_tmp = (data[11] << 8) | data[12];
  pos->hgt = (double)hgt_tmp / 2.0;

  pos->long_error  = data[13] * 2;
  pos->short_error = data[14] * 2;

  hdg_tmp = (data[15] << 8) | data[16];
  pos->heading = (double) hdg_tmp * 360.0 / 1024.0;

  pos->heading_error = (double) data[17] * 90.0 / 256.0;

  speed_tmp = (data[18] << 8) | data[19];
  pos->speed = (double) speed_tmp / 4.0;

  pos->speed_error = (double) data[20] / 4.0;
  pos->unknown[0] = data[21];
  pos->unknown[1] = data[22];
  pos->unknown[2] = data[23];
  pos->unknown[3] = data[41];

  tm.tm_year = bcd2uchar(data[24]) + century_bias;  /* #define to 100 after 1/1/2000 */
  tm.tm_mon  = bcd2uchar(data[25])-1;
  tm.tm_mday = bcd2uchar(data[26]);

  tm.tm_hour = bcd2uchar(data[27]);
  tm.tm_min  = bcd2uchar(data[28]);
  tm.tm_sec  = bcd2uchar(data[29]);
  tm.tm_isdst= -1;  /* dunno if DST  */

  pos->time = mktime(&tm) - as_UTC_offset;  /* compensate for mktime()'s TZ correction */

  pos->HDOP = (double) data[30] * 0.2;
  pos->VDOP = (double) data[31] * 0.2;

  entries = (data[33] << 24) | 
                         (data[34] << 16) |
                         (data[35] << 8) |
                          data[36];
  sat_status =  (data[40] << 24) | 
                         (data[39] << 16) |
                         (data[38] << 8) |
                          data[37];

  for(entry = 0; entry < 8; entry++)
    pos->sat_entries[entry].valid = 0;

  for(entry = 0, sat = 1; sat < 33; sat++)
  {
    if(entries & 0x00000001)
    {
      unsigned status;

      if(entry == 8)
      {
        printf("More than 8 sats found???\n");
        break;
      }

      pos->sat_entries[entry].id     = sat;
      pos->sat_entries[entry].valid  = 1;

      status = sat_status >> (4 * entry);
      
      pos->sat_entries[entry].condition = (status >> 2) & 0x3;
      pos->sat_entries[entry].level     = status & 0x3;
      
      entry++;
    }

    entries = entries >> 1;
  }

  return 0;
}


#ifdef INCLUDE_GPS_CONDITION
int xbcd2int(unsigned char *bcd)
{
  int result, sign=1;

  if((char)(*bcd&0xf) == 0xB)
	 sign=-1;
  bcd++;
  result = 100 * ((char)((*bcd>>4)&0xf)*10 + (char)(*bcd&0xf));
  bcd++;