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 static void
 102 check_sirf_message(char *from, uint8_t *msg, int len)
 103 {
 104         uint16_t        encoded_len, encoded_cksum;
 105         uint16_t        cksum;
 106         uint8_t         id;
 107         int             i;
 108
 109         if (msg[0] != 0xa0 || msg[1] != 0xa2) {
 110                 printf ("bad header\n");
 111                 return;
 112         }
 113         if (len < 7) {
 114                 printf("short\n");
 115                 return;
 116         }
 117         if (msg[len-1] != 0xb3 || msg[len-2] != 0xb0) {
 118                 printf ("bad trailer\n");
 119                 return;
 120         }
 121         encoded_len = (msg[2] << 8) | msg[3];
 122         id = msg[4];
 123         if (encoded_len != len - 8) {
 124                 printf ("length mismatch (got %d, wanted %d)\n",
 125                         len - 8, encoded_len);
 126                 return;
 127         }
 128         encoded_cksum = (msg[len - 4] << 8) | msg[len-3];
 129         cksum = 0;
 130         for (i = 4; i < len - 4; i++)
 131                 cksum = (cksum + msg[i]) & 0x7fff;
 132         if (encoded_cksum != cksum) {
 133                 printf ("cksum mismatch (got %04x wanted %04x)\n",
 134                         cksum, encoded_cksum);
 135                 return;
 136         }
 137         id = msg[4];
 138         if (id == 41) {
 139                 int     off = 4;
 140
 141                 uint8_t         id;
 142                 uint16_t        nav_valid;
 143                 uint16_t        nav_type;
 144                 uint16_t        week;
 145                 uint32_t        tow;
 146                 uint16_t        year;
 147                 uint8_t         month;
 148                 uint8_t         day;
 149                 uint8_t         hour;
 150                 uint8_t         minute;
 151                 uint16_t        second;
 152                 uint32_t        sat_list;
 153                 int32_t         lat;
 154                 int32_t         lon;
 155                 int32_t         alt_ell;
 156                 int32_t         alt_msl;
 157                 int8_t          datum;
 158                 uint16_t        sog;
 159                 uint16_t        cog;
 160                 int16_t         mag_var;
 161                 int16_t         climb_rate;
 162                 int16_t         heading_rate;
 163                 uint32_t        h_error;
 164                 uint32_t        v_error;
 165                 uint32_t        t_error;
 166                 uint16_t        h_v_error;
 167
 168 #define get_u8(u)       u = (msg[off]); off+= 1
 169 #define get_u16(u)      u = (msg[off] << 8) | (msg[off + 1]); off+= 2
 170 #define get_u32(u)      u = (msg[off] << 24) | (msg[off + 1] << 16) | (msg[off+2] << 8) | (msg[off+3]); off+= 4
 171
 172                 get_u8(id);
 173                 get_u16(nav_valid);
 174                 get_u16(nav_type);
 175                 get_u16(week);
 176                 get_u32(tow);
 177                 get_u16(year);
 178                 get_u8(month);
 179                 get_u8(day);
 180                 get_u8(hour);
 181                 get_u8(minute);
 182                 get_u16(second);
 183                 get_u32(sat_list);
 184                 get_u32(lat);
 185                 get_u32(lon);
 186                 get_u32(alt_ell);
 187                 get_u32(alt_msl);
 188                 get_u8(datum);
 189                 get_u16(sog);
 190                 get_u16(cog);
 191                 get_u16(mag_var);
 192                 get_u16(climb_rate);
 193                 get_u16(heading_rate);
 194                 get_u32(h_error);
 195                 get_u32(v_error);
 196                 get_u32(t_error);
 197                 get_u16(h_v_error);
 198
 199
 200                 printf ("Geodetic Navigation Data (41):\n");
 201                 printf ("\tNav valid %04x\n", nav_valid);
 202                 printf ("\tNav type %04x\n", nav_type);
 203                 printf ("\tWeek %d\n", week);
 204                 printf ("\tTOW %d\n", tow);
 205                 printf ("\tyear %d\n", year);
 206                 printf ("\tmonth %d\n", month);
 207                 printf ("\tday %d\n", day);
 208                 printf ("\thour %d\n", hour);
 209                 printf ("\tminute %d\n", minute);
 210                 printf ("\tsecond %g\n", second / 1000.0);
 211                 printf ("\tsats: %08x\n", sat_list);
 212                 printf ("\tlat: %g\n", lat / 1.0e7);
 213                 printf ("\tlon: %g\n", lon / 1.0e7);
 214                 printf ("\talt_ell: %g\n", alt_ell / 100.0);
 215                 printf ("\talt_msll: %g\n", alt_msl / 100.0);
 216                 printf ("\tdatum: %d\n", datum);
 217                 printf ("\tground speed: %g\n", sog / 100.0);
 218                 printf ("\tcourse: %g\n", cog / 100.0);
 219                 printf ("\tclimb: %g\n", climb_rate / 100.0);
 220                 printf ("\theading rate: %g\n", heading_rate / 100.0);
 221                 printf ("\th error: %g\n", h_error / 100.0);
 222                 printf ("\tv error: %g\n", v_error / 100.0);
 223                 printf ("\tt error: %g\n", t_error / 100.0);
 224                 printf ("\th vel error: %g\n", h_v_error / 100.0);
 225         } else {
 226                 return;
 227                 printf ("%s %4d:", from, encoded_len);
 228                 for (i = 4; i < len - 4; i++) {
 229                         if (((i - 4) & 0xf) == 0)
 230                                 printf("\n   ");
 231                         printf (" %3d", msg[i]);
 232                 }
 233                 printf ("\n");
 234         }
 235 }
 236
 237 static uint8_t  sirf_in_message[4096];
 238 static int      sirf_in_len;
 239
 240 void *
 241 ao_gps_input(void *arg)
 242 {
 243         int     i;
 244         char    c;
 245
 246         printf("ao_gps_input\n");
 247         for (;;) {
 248                 i = read(ao_gps_fd, &c, 1);
 249                 if (i == 1) {
 250                         int     v;
 251                         uint8_t uc = c;
 252
 253                         if (sirf_in_len || uc == 0xa0) {
 254                                 if (sirf_in_len < 4096)
 255                                         sirf_in_message[sirf_in_len++] = uc;
 256                                 if (uc == 0xb3) {
 257                                         check_sirf_message("recv", sirf_in_message, sirf_in_len);
 258                                         sirf_in_len = 0;
 259                                 }
 260                         }
 261                         input_queue[input_tail] = c;
 262                         input_tail = (input_tail + 1) % QUEUE_LEN;
 263                         sem_post(&input_semaphore);
 264                         sem_getvalue(&input_semaphore, &v);
 265 //                      printf ("ao_gps_input %02x %d\n", ((int) c) & 0xff, v);
 266                         fflush(stdout);
 267                         continue;
 268                 }
 269                 if (i < 0 && (errno == EINTR || errno == EAGAIN))
 270                         continue;
 271                 perror("getchar");
 272                 exit(1);
 273         }
 274 }
 275
 276 static uint8_t  sirf_message[4096];
 277 static int      sirf_message_len;
 278
 279 void
 280 ao_serial_putchar(char c)
 281 {
 282         int     i;
 283         uint8_t uc = (uint8_t) c;
 284
 285         if (sirf_message_len || uc == 0xa0) {
 286                 if (sirf_message_len < 4096)
 287                         sirf_message[sirf_message_len++] = uc;
 288                 if (uc == 0xb3) {
 289                         check_sirf_message("send", sirf_message, sirf_message_len);
 290                         sirf_message_len = 0;
 291                 }
 292         }
 293         for (;;) {
 294                 i = write(ao_gps_fd, &c, 1);
 295                 if (i == 1) {
 296                         if ((uint8_t) c == 0xb3 || c == '\r') {
 297                                 tcdrain(ao_gps_fd);
 298                                 usleep (1000 * 100);
 299                         }
 300                         break;
 301                 }
 302                 if (i < 0 && (errno == EINTR || errno == EAGAIN))
 303                         continue;
 304                 perror("putchar");
 305                 exit(1);
 306         }
 307 }
 308
 309 static void
 310 ao_serial_set_speed(uint8_t fast)
 311 {
 312         int     fd = ao_gps_fd;
 313         struct termios  termios;
 314
 315         tcdrain(fd);
 316         tcgetattr(fd, &termios);
 317         cfsetspeed(&termios, fast ? B9600 : B4800);
 318         tcsetattr(fd, TCSAFLUSH, &termios);
 319         tcflush(fd, TCIFLUSH);
 320 }
 321
 322 #include "ao_gps_print.c"
 323 #include "ao_gps.c"
 324
 325 void
 326 ao_dump_state(void)
 327 {
 328         double  lat, lon;
 329         printf ("%02d:%02d:%02d",
 330                 ao_gps_data.hour, ao_gps_data.minute,
 331                 ao_gps_data.second);
 332         printf (" nsat %d %svalid",
 333                 ao_gps_data.flags & AO_GPS_NUM_SAT_MASK,
 334                 ao_gps_data.flags & AO_GPS_VALID ? "" : "not ");
 335         printf (" lat %g lon %g alt %d",
 336                 ao_gps_data.latitude / 1.0e7,
 337                 ao_gps_data.longitude / 1.0e7,
 338                 ao_gps_data.altitude);
 339         printf (" speed %g climb %g course %d",
 340                 ao_gps_data.ground_speed / 100.0,
 341                 ao_gps_data.climb_rate / 100.0,
 342                 ao_gps_data.course * 2);
 343         printf (" hdop %g h_error %d v_error %d",
 344                 ao_gps_data.hdop / 5.0,
 345                 ao_gps_data.h_error, ao_gps_data.v_error);
 346         printf("\n");
 347 }
 348
 349 int
 350 ao_gps_open(const char *tty)
 351 {
 352         struct termios  termios;
 353         int fd;
 354
 355         fd = open (tty, O_RDWR);
 356         if (fd < 0)
 357                 return -1;
 358
 359         tcgetattr(fd, &termios);
 360         cfmakeraw(&termios);
 361         cfsetspeed(&termios, B4800);
 362         tcsetattr(fd, TCSAFLUSH, &termios);
 363
 364         tcdrain(fd);
 365         tcflush(fd, TCIFLUSH);
 366         return fd;
 367 }
 368
 369 pthread_t input_thread;
 370
 371 int
 372 main (int argc, char **argv)
 373 {
 374         char    *gps_file = "/dev/ttyUSB0";
 375
 376         ao_gps_fd = ao_gps_open(gps_file);
 377         if (ao_gps_fd < 0) {
 378                 perror (gps_file);
 379                 exit (1);
 380         }
 381         ao_gps_setup();
 382         sem_init(&input_semaphore, 0, 0);
 383         if (pthread_create(&input_thread, NULL, ao_gps_input, NULL) != 0)
 384                 perror("pthread_create");
 385         ao_gps();
 386 }