git.gag.com Git - fw/altos/blob - src/ao_gps_test.c

   1 /*
   2  * Copyright © 2009 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 #define AO_GPS_TEST
  19 #include "ao_host.h"
  20 #include <termios.h>
  21 #include <errno.h>
  22 #include <sys/types.h>
  23 #include <sys/stat.h>
  24 #include <fcntl.h>
  25 #include <pthread.h>
  26 #include <semaphore.h>
  27 #define AO_GPS_NUM_SAT_MASK     (0xf << 0)
  28 #define AO_GPS_NUM_SAT_SHIFT    (0)
  29
  30 #define AO_GPS_VALID            (1 << 4)
  31
  32 struct ao_gps_data {
  33         uint8_t                 hour;
  34         uint8_t                 minute;
  35         uint8_t                 second;
  36         uint8_t                 flags;
  37         int32_t                 latitude;       /* degrees * 10⁷ */
  38         int32_t                 longitude;      /* degrees * 10⁷ */
  39         int16_t                 altitude;       /* m */
  40         uint16_t                ground_speed;   /* cm/s */
  41         uint8_t                 course;         /* degrees / 2 */
  42         uint8_t                 hdop;           /* * 5 */
  43         int16_t                 climb_rate;     /* cm/s */
  44         uint16_t                h_error;        /* m */
  45         uint16_t                v_error;        /* m */
  46 };
  47
  48 void
  49 ao_mutex_get(uint8_t *mutex)
  50 {
  51 }
  52
  53 void
  54 ao_mutex_put(uint8_t *mutex)
  55 {
  56 }
  57
  58 static int
  59 ao_gps_fd;
  60
  61 static void
  62 ao_dbg_char(char c)
  63 {
  64         char    line[128];
  65         line[0] = '\0';
  66         if (c < ' ') {
  67                 if (c == '\n')
  68                         sprintf (line, "\n");
  69                 else
  70                         sprintf (line, "\\%02x", ((int) c) & 0xff);
  71         } else {
  72                 sprintf (line, "%c", c);
  73         }
  74         write(1, line, strlen(line));
  75 }
  76
  77 #define QUEUE_LEN       4096
  78
  79 static char     input_queue[QUEUE_LEN];
  80 int             input_head, input_tail;
  81
  82 static sem_t input_semaphore;
  83
  84 char
  85 ao_serial_getchar(void)
  86 {
  87         char    c;
  88         int     value;
  89         char    line[100];
  90
  91         sem_getvalue(&input_semaphore, &value);
  92 //      printf ("ao_serial_getchar %d\n", value);
  93         sem_wait(&input_semaphore);
  94         c = input_queue[input_head];
  95         input_head = (input_head + 1) % QUEUE_LEN;
  96 //      sprintf (line, "%02x\n", ((int) c) & 0xff);
  97 //      write(1, line, strlen(line));
  98         return c;
  99 }
 100
 101 void *
 102 ao_gps_input(void *arg)
 103 {
 104         int     i;
 105         char    c;
 106
 107         printf("ao_gps_input\n");
 108         for (;;) {
 109                 i = read(ao_gps_fd, &c, 1);
 110                 if (i == 1) {
 111                         int     v;
 112
 113                         input_queue[input_tail] = c;
 114                         input_tail = (input_tail + 1) % QUEUE_LEN;
 115                         sem_post(&input_semaphore);
 116                         sem_getvalue(&input_semaphore, &v);
 117 //                      printf ("ao_gps_input %02x %d\n", ((int) c) & 0xff, v);
 118                         fflush(stdout);
 119                         continue;
 120                 }
 121                 if (i < 0 && (errno == EINTR || errno == EAGAIN))
 122                         continue;
 123                 perror("getchar");
 124                 exit(1);
 125         }
 126 }
 127
 128 void
 129 ao_serial_putchar(char c)
 130 {
 131         int     i;
 132
 133         ao_dbg_char(c);
 134         for (;;) {
 135                 i = write(ao_gps_fd, &c, 1);
 136                 if (i == 1)
 137                         break;
 138                 if (i < 0 && (errno == EINTR || errno == EAGAIN))
 139                         continue;
 140                 perror("putchar");
 141                 exit(1);
 142         }
 143 }
 144
 145 static void
 146 ao_serial_set_speed(uint8_t fast)
 147 {
 148         int     fd = ao_gps_fd;
 149         struct termios  termios;
 150
 151         tcdrain(fd);
 152         tcgetattr(fd, &termios);
 153         cfsetspeed(&termios, fast ? B9600 : B4800);
 154         tcsetattr(fd, TCSAFLUSH, &termios);
 155         tcflush(fd, TCIFLUSH);
 156 }
 157
 158 #include "ao_gps_print.c"
 159 #include "ao_gps.c"
 160
 161 void
 162 ao_dump_state(void)
 163 {
 164         printf ("dump state\n");
 165 }
 166
 167 int
 168 ao_gps_open(const char *tty)
 169 {
 170         struct termios  termios;
 171         int fd;
 172
 173         fd = open (tty, O_RDWR);
 174         if (fd < 0)
 175                 return -1;
 176
 177         tcgetattr(fd, &termios);
 178         cfmakeraw(&termios);
 179         cfsetspeed(&termios, B4800);
 180         tcsetattr(fd, TCSAFLUSH, &termios);
 181
 182         tcdrain(fd);
 183         tcflush(fd, TCIFLUSH);
 184         return fd;
 185 }
 186
 187 pthread_t input_thread;
 188
 189 int
 190 main (int argc, char **argv)
 191 {
 192         char    *gps_file = "/dev/ttyUSB0";
 193
 194         ao_gps_fd = ao_gps_open(gps_file);
 195         if (ao_gps_fd < 0) {
 196                 perror (gps_file);
 197                 exit (1);
 198         }
 199         ao_gps_setup();
 200         sem_init(&input_semaphore, 0, 0);
 201         if (pthread_create(&input_thread, NULL, ao_gps_input, NULL) != 0)
 202                 perror("pthread_create");
 203         ao_gps();
 204 }