#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
-#include <pthread.h>
-#include <semaphore.h>
#define AO_GPS_NUM_SAT_MASK (0xf << 0)
#define AO_GPS_NUM_SAT_SHIFT (0)
#define AO_GPS_VALID (1 << 4)
+#define AO_GPS_RUNNING (1 << 5)
+#define AO_GPS_DATE_VALID (1 << 6)
struct ao_gps_data {
+ uint8_t year;
+ uint8_t month;
+ uint8_t day;
uint8_t hour;
uint8_t minute;
uint8_t second;
uint16_t v_error; /* m */
};
+#define SIRF_SAT_STATE_ACQUIRED (1 << 0)
+#define SIRF_SAT_STATE_CARRIER_PHASE_VALID (1 << 1)
+#define SIRF_SAT_BIT_SYNC_COMPLETE (1 << 2)
+#define SIRF_SAT_SUBFRAME_SYNC_COMPLETE (1 << 3)
+#define SIRF_SAT_CARRIER_PULLIN_COMPLETE (1 << 4)
+#define SIRF_SAT_CODE_LOCKED (1 << 5)
+#define SIRF_SAT_ACQUISITION_FAILED (1 << 6)
+#define SIRF_SAT_EPHEMERIS_AVAILABLE (1 << 7)
+
+struct ao_gps_sat_data {
+ uint8_t svid;
+ uint8_t c_n_1;
+};
+
+struct ao_gps_tracking_data {
+ uint8_t channels;
+ struct ao_gps_sat_data sats[12];
+};
+
void
ao_mutex_get(uint8_t *mutex)
{
static char input_queue[QUEUE_LEN];
int input_head, input_tail;
-static sem_t input_semaphore;
+#include <sys/time.h>
-char
-ao_serial_getchar(void)
+int
+get_millis(void)
{
- char c;
- int value;
- char line[100];
-
- sem_getvalue(&input_semaphore, &value);
-// printf ("ao_serial_getchar %d\n", value);
- sem_wait(&input_semaphore);
- c = input_queue[input_head];
- input_head = (input_head + 1) % QUEUE_LEN;
-// sprintf (line, "%02x\n", ((int) c) & 0xff);
-// write(1, line, strlen(line));
- return c;
+ struct timeval tv;
+ gettimeofday(&tv, NULL);
+ return tv.tv_sec * 1000 + tv.tv_usec / 1000;
}
static void
}
encoded_len = (msg[2] << 8) | msg[3];
id = msg[4];
+/* printf ("%9d: %3d\n", get_millis(), id); */
if (encoded_len != len - 8) {
- printf ("length mismatch (got %d, wanted %d)\n",
- len - 8, encoded_len);
+ if (id != 52)
+ printf ("length mismatch (got %d, wanted %d)\n",
+ len - 8, encoded_len);
return;
}
encoded_cksum = (msg[len - 4] << 8) | msg[len-3];
return;
}
id = msg[4];
- if (id == 41) {
+ switch (id) {
+ case 41:{
int off = 4;
uint8_t id;
printf ("Geodetic Navigation Data (41):\n");
printf ("\tNav valid %04x\n", nav_valid);
printf ("\tNav type %04x\n", nav_type);
- printf ("\tWeek %d\n", week);
- printf ("\tTOW %d\n", tow);
- printf ("\tyear %d\n", year);
- printf ("\tmonth %d\n", month);
- printf ("\tday %d\n", day);
- printf ("\thour %d\n", hour);
- printf ("\tminute %d\n", minute);
- printf ("\tsecond %g\n", second / 1000.0);
+ printf ("\tWeek %5d", week);
+ printf (" TOW %9d", tow);
+ printf (" %4d-%2d-%2d %02d:%02d:%07.4f\n",
+ year, month, day,
+ hour, minute, second / 1000.0);
printf ("\tsats: %08x\n", sat_list);
- printf ("\tlat: %g\n", lat / 1.0e7);
- printf ("\tlon: %g\n", lon / 1.0e7);
- printf ("\talt_ell: %g\n", alt_ell / 100.0);
- printf ("\talt_msll: %g\n", alt_msl / 100.0);
- printf ("\tdatum: %d\n", datum);
- printf ("\tground speed: %g\n", sog / 100.0);
- printf ("\tcourse: %g\n", cog / 100.0);
- printf ("\tclimb: %g\n", climb_rate / 100.0);
- printf ("\theading rate: %g\n", heading_rate / 100.0);
- printf ("\th error: %g\n", h_error / 100.0);
- printf ("\tv error: %g\n", v_error / 100.0);
- printf ("\tt error: %g\n", t_error / 100.0);
- printf ("\th vel error: %g\n", h_v_error / 100.0);
- } else {
+ printf ("\tlat: %g", lat / 1.0e7);
+ printf (" lon: %g", lon / 1.0e7);
+ printf (" alt_ell: %g", alt_ell / 100.0);
+ printf (" alt_msll: %g", alt_msl / 100.0);
+ printf (" datum: %d\n", datum);
+ printf ("\tground speed: %g", sog / 100.0);
+ printf (" course: %g", cog / 100.0);
+ printf (" climb: %g", climb_rate / 100.0);
+ printf (" heading rate: %g\n", heading_rate / 100.0);
+ printf ("\th error: %g", h_error / 100.0);
+ printf (" v error: %g", v_error / 100.0);
+ printf (" t error: %g", t_error / 100.0);
+ printf (" h vel error: %g\n", h_v_error / 100.0);
+ break;
+ }
+ case 4: {
+ int off = 4;
+ uint8_t id;
+ int16_t gps_week;
+ uint32_t gps_tow;
+ uint8_t channels;
+ int j, k;
+
+ get_u8(id);
+ get_u16(gps_week);
+ get_u32(gps_tow);
+ get_u8(channels);
+
+ printf ("Measured Tracker Data (4):\n");
+ printf ("GPS week: %d\n", gps_week);
+ printf ("GPS time of week: %d\n", gps_tow);
+ printf ("channels: %d\n", channels);
+ for (j = 0; j < 12; j++) {
+ uint8_t svid, azimuth, elevation;
+ uint16_t state;
+ uint8_t c_n[10];
+ get_u8(svid);
+ get_u8(azimuth);
+ get_u8(elevation);
+ get_u16(state);
+ for (k = 0; k < 10; k++) {
+ get_u8(c_n[k]);
+ }
+ printf ("Sat %3d:", svid);
+ printf (" aziumuth: %6.1f", azimuth * 1.5);
+ printf (" elevation: %6.1f", elevation * 0.5);
+ printf (" state: 0x%02x", state);
+ printf (" c_n:");
+ for (k = 0; k < 10; k++)
+ printf(" %3d", c_n[k]);
+ if (state & SIRF_SAT_STATE_ACQUIRED)
+ printf(" acq,");
+ if (state & SIRF_SAT_STATE_CARRIER_PHASE_VALID)
+ printf(" car,");
+ if (state & SIRF_SAT_BIT_SYNC_COMPLETE)
+ printf(" bit,");
+ if (state & SIRF_SAT_SUBFRAME_SYNC_COMPLETE)
+ printf(" sub,");
+ if (state & SIRF_SAT_CARRIER_PULLIN_COMPLETE)
+ printf(" pullin,");
+ if (state & SIRF_SAT_CODE_LOCKED)
+ printf(" code,");
+ if (state & SIRF_SAT_ACQUISITION_FAILED)
+ printf(" fail,");
+ if (state & SIRF_SAT_EPHEMERIS_AVAILABLE)
+ printf(" ephem,");
+ printf ("\n");
+ }
+ break;
+ }
+ default:
return;
printf ("%s %4d:", from, encoded_len);
for (i = 4; i < len - 4; i++) {
}
}
+static uint8_t sirf_message[4096];
+static int sirf_message_len;
static uint8_t sirf_in_message[4096];
static int sirf_in_len;
-void *
-ao_gps_input(void *arg)
+char
+ao_serial_getchar(void)
{
- int i;
char c;
-
- printf("ao_gps_input\n");
- for (;;) {
- i = read(ao_gps_fd, &c, 1);
- if (i == 1) {
- int v;
- uint8_t uc = c;
-
- if (sirf_in_len || uc == 0xa0) {
- if (sirf_in_len < 4096)
- sirf_in_message[sirf_in_len++] = uc;
- if (uc == 0xb3) {
- check_sirf_message("recv", sirf_in_message, sirf_in_len);
- sirf_in_len = 0;
- }
+ uint8_t uc;
+
+ while (input_head == input_tail) {
+ for (;;) {
+ input_tail = read(ao_gps_fd, input_queue, QUEUE_LEN);
+ if (input_tail < 0) {
+ if (errno == EINTR || errno == EAGAIN)
+ continue;
+ perror ("getchar");
+ exit (1);
}
- input_queue[input_tail] = c;
- input_tail = (input_tail + 1) % QUEUE_LEN;
- sem_post(&input_semaphore);
- sem_getvalue(&input_semaphore, &v);
-// printf ("ao_gps_input %02x %d\n", ((int) c) & 0xff, v);
- fflush(stdout);
- continue;
+ input_head = 0;
+ break;
}
- if (i < 0 && (errno == EINTR || errno == EAGAIN))
- continue;
- perror("getchar");
- exit(1);
}
+ c = input_queue[input_head];
+ input_head = (input_head + 1) % QUEUE_LEN;
+ uc = c;
+ if (sirf_in_len || uc == 0xa0) {
+ if (sirf_in_len < 4096)
+ sirf_in_message[sirf_in_len++] = uc;
+ if (uc == 0xb3) {
+ check_sirf_message("recv", sirf_in_message, sirf_in_len);
+ sirf_in_len = 0;
+ }
+ }
+ return c;
}
-static uint8_t sirf_message[4096];
-static int sirf_message_len;
void
ao_serial_putchar(char c)
i = write(ao_gps_fd, &c, 1);
if (i == 1) {
if ((uint8_t) c == 0xb3 || c == '\r') {
+ static const struct timespec delay = {
+ .tv_sec = 0,
+ .tv_nsec = 100 * 1000 * 1000
+ };
tcdrain(ao_gps_fd);
- usleep (1000 * 100);
+// nanosleep(&delay, NULL);
}
break;
}
}
}
+#define AO_SERIAL_SPEED_4800 0
+#define AO_SERIAL_SPEED_57600 1
+
static void
-ao_serial_set_speed(uint8_t fast)
+ao_serial_set_speed(uint8_t speed)
{
int fd = ao_gps_fd;
struct termios termios;
tcdrain(fd);
tcgetattr(fd, &termios);
- cfsetspeed(&termios, fast ? B9600 : B4800);
+ switch (speed) {
+ case AO_SERIAL_SPEED_4800:
+ cfsetspeed(&termios, B4800);
+ break;
+ case AO_SERIAL_SPEED_57600:
+ cfsetspeed(&termios, B57600);
+ break;
+ }
tcsetattr(fd, TCSAFLUSH, &termios);
tcflush(fd, TCIFLUSH);
}
#include "ao_gps_print.c"
-#include "ao_gps.c"
+#include "ao_gps_sirf.c"
void
-ao_dump_state(void)
+ao_dump_state(void *wchan)
{
double lat, lon;
+ int i;
+ if (wchan == &ao_gps_data)
+ ao_gps_print(&ao_gps_data);
+ else
+ ao_gps_tracking_print(&ao_gps_tracking_data);
+ putchar('\n');
+ return;
printf ("%02d:%02d:%02d",
ao_gps_data.hour, ao_gps_data.minute,
ao_gps_data.second);
ao_gps_data.hdop / 5.0,
ao_gps_data.h_error, ao_gps_data.v_error);
printf("\n");
+ printf ("\t");
+ for (i = 0; i < 12; i++)
+ printf (" %2d(%02d)",
+ ao_gps_tracking_data.sats[i].svid,
+ ao_gps_tracking_data.sats[i].c_n_1);
+ printf ("\n");
}
int
return fd;
}
-pthread_t input_thread;
+#include <getopt.h>
+
+static const struct option options[] = {
+ { .name = "tty", .has_arg = 1, .val = 'T' },
+ { 0, 0, 0, 0},
+};
+
+static void usage(char *program)
+{
+ fprintf(stderr, "usage: %s [--tty <tty-name>]\n", program);
+ exit(1);
+}
int
main (int argc, char **argv)
{
- char *gps_file = "/dev/ttyUSB0";
+ char *tty = "/dev/ttyUSB0";
+ int c;
- ao_gps_fd = ao_gps_open(gps_file);
+ while ((c = getopt_long(argc, argv, "T:", options, NULL)) != -1) {
+ switch (c) {
+ case 'T':
+ tty = optarg;
+ break;
+ default:
+ usage(argv[0]);
+ break;
+ }
+ }
+ ao_gps_fd = ao_gps_open(tty);
if (ao_gps_fd < 0) {
- perror (gps_file);
+ perror (tty);
exit (1);
}
ao_gps_setup();
- sem_init(&input_semaphore, 0, 0);
- if (pthread_create(&input_thread, NULL, ao_gps_input, NULL) != 0)
- perror("pthread_create");
ao_gps();
}
projects / fw / altos / blobdiff