Add host-side gps protocol testing program

[fw/altos] / src / ao_gps_test.c

diff --git a/src/ao_gps_test.c b/src/ao_gps_test.c
new file mode 100644 (file)
index 0000000..7489e78
--- /dev/null
+++ b/src/ao_gps_test.c
@@ -0,0 +1,204 @@
+/*
+ * Copyright © 2009 Keith Packard <keithp@keithp.com>
+ *
+ * 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; version 2 of the License.
+ *
+ * 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.
+ */
+
+#define AO_GPS_TEST
+#include "ao_host.h"
+#include <termios.h>
+#include <errno.h>
+#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)
+
+struct ao_gps_data {
+       uint8_t                 hour;
+       uint8_t                 minute;
+       uint8_t                 second;
+       uint8_t                 flags;
+       int32_t                 latitude;       /* degrees * 10⁷ */
+       int32_t                 longitude;      /* degrees * 10⁷ */
+       int16_t                 altitude;       /* m */
+       uint16_t                ground_speed;   /* cm/s */
+       uint8_t                 course;         /* degrees / 2 */
+       uint8_t                 hdop;           /* * 5 */
+       int16_t                 climb_rate;     /* cm/s */
+       uint16_t                h_error;        /* m */
+       uint16_t                v_error;        /* m */
+};
+
+void
+ao_mutex_get(uint8_t *mutex)
+{
+}
+
+void
+ao_mutex_put(uint8_t *mutex)
+{
+}
+
+static int
+ao_gps_fd;
+
+static void
+ao_dbg_char(char c)
+{
+       char    line[128];
+       line[0] = '\0';
+       if (c < ' ') {
+               if (c == '\n')
+                       sprintf (line, "\n");
+               else
+                       sprintf (line, "\\%02x", ((int) c) & 0xff);
+       } else {
+               sprintf (line, "%c", c);
+       }
+       write(1, line, strlen(line));
+}
+
+#define QUEUE_LEN      4096
+
+static char    input_queue[QUEUE_LEN];
+int            input_head, input_tail;
+
+static sem_t input_semaphore;
+
+char
+ao_serial_getchar(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;
+}
+
+void *
+ao_gps_input(void *arg)
+{
+       int     i;
+       char    c;
+
+       printf("ao_gps_input\n");
+       for (;;) {
+               i = read(ao_gps_fd, &c, 1);
+               if (i == 1) {
+                       int     v;
+
+                       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;
+               }
+               if (i < 0 && (errno == EINTR || errno == EAGAIN))
+                       continue;
+               perror("getchar");
+               exit(1);
+       }
+}
+
+void
+ao_serial_putchar(char c)
+{
+       int     i;
+
+       ao_dbg_char(c);
+       for (;;) {
+               i = write(ao_gps_fd, &c, 1);
+               if (i == 1)
+                       break;
+               if (i < 0 && (errno == EINTR || errno == EAGAIN))
+                       continue;
+               perror("putchar");
+               exit(1);
+       }
+}
+
+static void
+ao_serial_set_speed(uint8_t fast)
+{
+       int     fd = ao_gps_fd;
+       struct termios  termios;
+
+       tcdrain(fd);
+       tcgetattr(fd, &termios);
+       cfsetspeed(&termios, fast ? B9600 : B4800);
+       tcsetattr(fd, TCSAFLUSH, &termios);
+       tcflush(fd, TCIFLUSH);
+}
+
+#include "ao_gps_print.c"
+#include "ao_gps.c"
+
+void
+ao_dump_state(void)
+{
+       printf ("dump state\n");
+}
+
+int
+ao_gps_open(const char *tty)
+{
+       struct termios  termios;
+       int fd;
+
+       fd = open (tty, O_RDWR);
+       if (fd < 0)
+               return -1;
+
+       tcgetattr(fd, &termios);
+       cfmakeraw(&termios);
+       cfsetspeed(&termios, B4800);
+       tcsetattr(fd, TCSAFLUSH, &termios);
+
+       tcdrain(fd);
+       tcflush(fd, TCIFLUSH);
+       return fd;
+}
+
+pthread_t input_thread;
+
+int
+main (int argc, char **argv)
+{
+       char    *gps_file = "/dev/ttyUSB0";
+
+       ao_gps_fd = ao_gps_open(gps_file);
+       if (ao_gps_fd < 0) {
+               perror (gps_file);
+               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();
+}