bluegps.c

GPS接收软件

/*
 * BlueGPS -- a tool to control the RBT 3000 GPS Datalogger
 * Copyright (C) 2006 Till Harbaum and Simon Budig
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */

#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <unistd.h>
#include <time.h>
#include <math.h>
#include <signal.h>
#include <stdarg.h>
#include <ctype.h>

#include <netinet/ip.h>

#include <bluetooth/bluetooth.h>
#include <bluetooth/rfcomm.h>

#include "rbt3000.h"

#define RFCOMM_CHANNEL 1

#define NAUTIC_MILE  (1.85185)
#define KPH2KNOTS(a) ((a)/NAUTIC_MILE)


typedef enum
{
  STATE_AA,
  STATE_55,
  STATE_LEN_LO,
  STATE_LEN_HI,
  STATE_PAYLOAD,
  STATE_SUM,
  STATE_33,
  STATE_CC
} RbtParserState;

typedef enum
{
  SET_LOG_ENABLE    = 1 << 0,
  SET_LOG_OVERWRITE = 1 << 1,
  SET_LOG_HEIGHT    = 1 << 2,
  SET_CONST_TIME    = 1 << 3,
  SET_CONST_DIST    = 1 << 4,
  SET_MAX_SPEED     = 1 << 5,
  SET_NAME          = 1 << 6,
  SET_PASSWORD      = 1 << 7,
  ERASE_DATALOG     = 1 << 8,
} BlueGPSCommand;

typedef enum
{
  MSG_QUIET,
  MSG_NORMAL,
  MSG_VERBOSE,
} BlueMsgLevel;

typedef struct
{
  bdaddr_t         device;
  int              device_fd;
  int              req_quit;
  int              need_password;
  int              expected_entries;
  BlueMsgLevel     messages;
  char             password[4];
  char            *filename;
  FILE            *outfile;

  int              have_old_config;
  datalog_config_t old_config;

  int              commands;
  char             new_name[15];
  datalog_config_t new_config;
  char             new_password[4];
} BlueGPSContext;

BlueGPSContext *context = NULL;

void rbt3000_handle_reply (int device_fd, unsigned char *buffer, int len);
void print_message        (BlueMsgLevel level, char *format, ...)
                            __attribute__((__format__ (__printf__, 2, 3)));


int
full_write (int fd, void *buf, int count)
{
  int pos = 0, ret = 0;

  while (ret >= 0 && pos < count)
    {
      ret = write (fd, buf + pos, count - pos);
      if (ret >= 0)
        pos += ret;
    }

  if (ret < 0)
    return ret;
  else
    return count;
}


void
sigproc (int sig)
{
  signal (SIGINT, sigproc);

  if (context->req_quit)
    exit (1);

  context->req_quit = 1;
}


void
print_message (BlueMsgLevel level, char *format, ...)
{
  va_list ap;

  if (level <= context->messages)
    {
      va_start (ap, format);
      vfprintf (stderr, format, ap);
      va_end (ap);
    }
}


void
print_datalog_config (BlueMsgLevel level, datalog_config_t *datalog)
{
  print_message (level,
                 "Datalog:            %s\n"
                 "  - when full:      %s\n"
                 "  - height info:    %s\n",
                 datalog->enabled ? "on" : "off",
                 datalog->overwrite ? "overwrite" : "stop",
                 datalog->include_altitude ? "on" : "off");

  print_message (level, "  - constant time:  ");
  if (datalog->interval)
    print_message (level, "%lds\n", datalog->interval);
  else
    print_message (level, "off\n");
  print_message (level, "  - constant dist:  ");
  if (datalog->constant_distance)
    print_message (level, "%ldm\n", datalog->constant_distance);
  else
    print_message (level, "off\n");
  print_message (level, "  - maximum speed:  ");
  if (datalog->speeding_limit > 0.0001)
    print_message (level, "%.2f km/h\n", datalog->speeding_limit);
  else
    print_message (level, "off\n");
}


unsigned char
nmea_checksum (unsigned char *str)
{
  unsigned char check = 0;

  while (*str)
    {
      if (*str != '$')
        check ^= *str;
      str ++;
    }

  return check;
}


void
fprint_datalog_entry_nmea (FILE *stream, datalog_entry_t *entry)
{
  unsigned char nmea[128];
  int latdegrees, londegrees;
  float latminutes, lonminutes;
  char  latdir, londir;

  nmea[127] = 0;

  latdegrees = floor (fabs (entry->latitude));
  latminutes = 60 * fmod (fabs (entry->latitude), 1.0);
  latdir = entry->latitude < 0 ? 'S' : 'N';
  londegrees = floor (fabs (entry->longitude));
  lonminutes = 60 * fmod (fabs (entry->longitude), 1.0);
  londir = entry->longitude < 0 ? 'W' : 'E';

  /* GPRMC message */
  snprintf ((char *) nmea, 127,
            "$GPRMC,%02u%02u%06.3f,A,%02d%07.4f,%c,%03d%07.4f,%c,"
            "%.3f,%.3f,%02u%02u%02u,,",
            entry->hour, entry->minute, entry->second,
            latdegrees, latminutes, latdir,
            londegrees, lonminutes, londir,
            KPH2KNOTS (entry->speed), entry->direction,
            entry->day, entry->month, entry->year);
  fprintf (stream, "%s*%02X\r\n", nmea, nmea_checksum (nmea));

  /* GPGGA message */
  snprintf ((char *) nmea, 127,
            "$GPGGA,%02u%02u%06.3f,%02d%07.4f,%c,%03d%07.4f,%c,"
            "1,%02d,%.1f,%.2f,M,,M,,",
            entry->hour, entry->minute, entry->second,
            latdegrees, latminutes, latdir,
            londegrees, lonminutes, londir,
            entry->satellites, entry->HDOP, entry->altitude);
  fprintf (stream, "%s*%02X\r\n", nmea, nmea_checksum (nmea));

  /* GPGLL message */
  snprintf ((char *) nmea, 127,
            "$GPGLL,%02d%07.4f,%c,%03d%07.4f,%c,"
            "%02u%02u%06.3f,A",
            latdegrees, latminutes, latdir,
            londegrees, lonminutes, londir,
            entry->hour, entry->minute, entry->second);
  fprintf (stream, "%s*%02X\r\n", nmea, nmea_checksum (nmea));

  /* GPVTG message */
  snprintf ((char *) nmea, 127,
            "$GPVTG,%.3f,T,,M,%.3f,N,%.3f,K",
            entry->direction, KPH2KNOTS(entry->speed), entry->speed);
  fprintf (stream, "%s*%02X\r\n", nmea, nmea_checksum (nmea));

  /* GPGSA message */
  snprintf ((char *) nmea, 127,
            "$GPGSA,A,,,,,,,,,,,,,,%.2f,%.2f,,",
            entry->PDOP, entry->HDOP);
  fprintf (stream, "%s*%02X\r\n", nmea, nmea_checksum (nmea));
}


void
print_progress_bar (BlueMsgLevel level, int sent, int total)
{
#define BAR_WIDTH 50
  int i;

  print_message (level, "\rDownloading: [");
  for (i = 0; i < BAR_WIDTH; i++)
    print_message (level, "%c", i < (BAR_WIDTH * sent / total) ? '#' : '-');
  print_message (level, "]  %d/%d     \r", sent, total);
  fflush (stderr);
#undef BAR_WIDTH
}


int
rbt3000_connect (char *device_addr)
{

  struct sockaddr_rc rem_addr;

  baswap (&context->device, strtoba (device_addr));

  rem_addr.rc_family  = AF_BLUETOOTH;
  rem_addr.rc_bdaddr  = context->device;
  rem_addr.rc_channel = RFCOMM_CHANNEL;

  /* bluez connects to BlueClient */
  if ((context->device_fd = socket (PF_BLUETOOTH,
                                    SOCK_STREAM,
                                    BTPROTO_RFCOMM)) < 0 )
    {
      perror ("Can't create socket");
      return 0;
    }

  /* connect on rfcomm */
  if (connect (context->device_fd,
               (struct sockaddr *) &rem_addr,
               sizeof (rem_addr)) < 0 )
    {
      perror ("Can't connect");
      close (context->device_fd);
      return 0;
    }

  return 1;
}


void
rbt3000_init (int fd)
{
  /* sending $RCMD402*2E  */
  uint8_t cmd1[] = { 0xa0, 0xa2, 0x00, 0x31, 0xa5, 0x00, 0x04, 0x04,
                     0x00, 0x00, 0xe1, 0x00, 0x08, 0x01, 0x00, 0x00,
                     0x00, 0xff, 0x05, 0x05, 0x00, 0x00, 0x00, 0x00,
                     0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0x05, 0x05,
                     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                     0x00, 0xff, 0x05, 0x05, 0x00, 0x00, 0x00, 0x00,
                     0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0xb2, 0xb0,
                     0xb3 };

  print_message (MSG_NORMAL, "Initializing RBT-3000...\n");

  full_write (fd, "\r\n", 2);
  full_write (fd, "$RCMD402*2E\r\n", 13);
  full_write (fd, cmd1, sizeof (cmd1));
}


void
rbt3000_parse_byte (int device_fd, unsigned char byte)
{
  static RbtParserState state = STATE_AA;
  static int len = 0;
  static int count = 0;
  static int xsum = 0;
  static unsigned char *buffer = NULL;

  switch (state)
  {
    case STATE_AA:
      if (byte == 0xAA)
        state = STATE_55;
      break;

    case STATE_55:
      if (byte == 0x55)
        state = STATE_LEN_LO;
      else
        state = (byte != 0xAA) ? STATE_AA : STATE_55;
      break;

    case STATE_LEN_LO:
      len = byte;
      state = STATE_LEN_HI;
      break;

    case STATE_LEN_HI:
      len += 256 * byte;
      if (buffer)
        free (buffer);
      buffer = malloc (len);
      xsum = count = 0;
      state = STATE_PAYLOAD;
      break;

    case STATE_PAYLOAD:
      if (count < len)
        {
          buffer[count] = byte;
          xsum ^= byte;
          count ++;
        }
      if (count == len)
        {
          state = STATE_SUM;
        }
      break;

    case STATE_SUM:
      if (xsum == byte)
        {
          state = STATE_33;
        }
      else
        {
          print_message (MSG_QUIET, "Checksum error\n");
          state = (byte != 0xAA) ? STATE_AA : STATE_55;
        }
      break;

    case STATE_33:
      if (byte == 0x33)
        {
          state = STATE_CC;
        }
      else
        {
          print_message (MSG_QUIET, "Postamble 0x33 error\n");
          state = (byte != 0xAA) ? STATE_AA : STATE_55;
        }
      break;

    case STATE_CC:
      if (byte == 0xCC)
        {
          rbt3000_handle_reply (device_fd, buffer, len);
          state = STATE_AA;
        }
      else
        {
          print_message (MSG_QUIET, "Postamble 0xCC error\n");
          state = (byte != 0xAA) ? STATE_AA : STATE_55;
        }
      break;
  }
}


int
rbt3000_send_cmd_seq (int   fd,
                      char  command,
                      char *data,
                      int   len)
{
  unsigned char buffer[5] = { 0xAA, 0x55, 0x00, 0x00, 0x00 };
  int ret;

  /* Preamble */
  buffer[2] = (len + 1) & 0xff;
  buffer[3] = (len + 1) >> 8;
  buffer[4] = command;

  ret = full_write (fd, buffer, 5);
  if (ret < 0)
    return ret;

  if (len > 0)
    {
      ret = full_write (fd, data, len);
      if (ret < 0)
        return ret;
    }

  /* Postamble */

  buffer[0] = command;
  while (len)
    {
      buffer[0] ^= *data;
      data++;
      len--;
    }

  buffer[1] = 0x33;
  buffer[2] = 0xCC;

  ret = full_write (fd, buffer, 3);
  if (ret < 0)
    return ret;

  return 0;
}


int
rbt3000_send_cmd (int  fd,
                  char cmd)
{
  return rbt3000_send_cmd_seq (fd, cmd, NULL, 0);
}


void
rbt3000_abort (char *message)
{
  print_message (MSG_QUIET, "%s Aborting.\n", message);
  context->req_quit = 1;
}


void
rbt3000_handle_reply_action (int device_fd, unsigned char *buffer, int len)
{
  static int entrycount;

  switch (buffer[0])
  {
    case CMD_ECHO:
      break;

    case CMD_STATUS:
      if (len - 1 == sizeof (status_t))
        {
          status_t *status = (status_t *) (buffer + 1);

          print_message (MSG_NORMAL,
                         "Device name:        %s\n",
                         status->name);
          print_message (MSG_VERBOSE,
                         "Firmware version:   %s\n"
                         "Battery:            %s\n"
                         "Password:           %s\n",
                         status->version,
                         status->batt ? (status->batt == 2 ? "low" : "mid")
                                      : "high",
                         status->password ? "enabled" : "disabled");

          print_datalog_config (MSG_VERBOSE, &status->datalog);

          context->old_config  = status->datalog;
          context->have_old_config = 1;
          context->need_password = status->password;
        }
      else
        {
          rbt3000_abort ("Wrong status reply size.");
        }

      break;

    case CMD_PASSWORD:
      if (buffer[1])
        context->need_password = 0;
      break;

    case CMD_LOGSTATE:
      if (len - 1 == sizeof (datalog_state_t))
        {
          datalog_state_t *state = (datalog_state_t *) (buffer + 1);