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 #define AO_GPS_NUM_SAT_MASK     (0xf << 0)
  26 #define AO_GPS_NUM_SAT_SHIFT    (0)
  27
  28 #define AO_GPS_VALID            (1 << 4)
  29 #define AO_GPS_RUNNING          (1 << 5)
  30
  31 struct ao_gps_data {
  32         uint8_t                 hour;
  33         uint8_t                 minute;
  34         uint8_t                 second;
  35         uint8_t                 flags;
  36         int32_t                 latitude;       /* degrees * 10⁷ */
  37         int32_t                 longitude;      /* degrees * 10⁷ */
  38         int16_t                 altitude;       /* m */
  39         uint16_t                ground_speed;   /* cm/s */
  40         uint8_t                 course;         /* degrees / 2 */
  41         uint8_t                 hdop;           /* * 5 */
  42         int16_t                 climb_rate;     /* cm/s */
  43         uint16_t                h_error;        /* m */
  44         uint16_t                v_error;        /* m */
  45 };
  46
  47 void
  48 ao_mutex_get(uint8_t *mutex)
  49 {
  50 }
  51
  52 void
  53 ao_mutex_put(uint8_t *mutex)
  54 {
  55 }
  56
  57 static int
  58 ao_gps_fd;
  59
  60 static void
  61 ao_dbg_char(char c)
  62 {
  63         char    line[128];
  64         line[0] = '\0';
  65         if (c < ' ') {
  66                 if (c == '\n')
  67                         sprintf (line, "\n");
  68                 else
  69                         sprintf (line, "\\%02x", ((int) c) & 0xff);
  70         } else {
  71                 sprintf (line, "%c", c);
  72         }
  73         write(1, line, strlen(line));
  74 }
  75
  76 #define QUEUE_LEN       4096
  77
  78 static char     input_queue[QUEUE_LEN];
  79 int             input_head, input_tail;
  80
  81
  82 static void
  83 check_sirf_message(char *from, uint8_t *msg, int len)
  84 {
  85         uint16_t        encoded_len, encoded_cksum;
  86         uint16_t        cksum;
  87         uint8_t         id;
  88         int             i;
  89
  90         if (msg[0] != 0xa0 || msg[1] != 0xa2) {
  91                 printf ("bad header\n");
  92                 return;
  93         }
  94         if (len < 7) {
  95                 printf("short\n");
  96                 return;
  97         }
  98         if (msg[len-1] != 0xb3 || msg[len-2] != 0xb0) {
  99                 printf ("bad trailer\n");
 100                 return;
 101         }
 102         encoded_len = (msg[2] << 8) | msg[3];
 103         id = msg[4];
 104         if (encoded_len != len - 8) {
 105                 printf ("length mismatch (got %d, wanted %d)\n",
 106                         len - 8, encoded_len);
 107                 return;
 108         }
 109         encoded_cksum = (msg[len - 4] << 8) | msg[len-3];
 110         cksum = 0;
 111         for (i = 4; i < len - 4; i++)
 112                 cksum = (cksum + msg[i]) & 0x7fff;
 113         if (encoded_cksum != cksum) {
 114                 printf ("cksum mismatch (got %04x wanted %04x)\n",
 115                         cksum, encoded_cksum);
 116                 return;
 117         }
 118         id = msg[4];
 119         if (id == 41) {
 120                 int     off = 4;
 121
 122                 uint8_t         id;
 123                 uint16_t        nav_valid;
 124                 uint16_t        nav_type;
 125                 uint16_t        week;
 126                 uint32_t        tow;
 127                 uint16_t        year;
 128                 uint8_t         month;
 129                 uint8_t         day;
 130                 uint8_t         hour;
 131                 uint8_t         minute;
 132                 uint16_t        second;
 133                 uint32_t        sat_list;
 134                 int32_t         lat;
 135                 int32_t         lon;
 136                 int32_t         alt_ell;
 137                 int32_t         alt_msl;
 138                 int8_t          datum;
 139                 uint16_t        sog;
 140                 uint16_t        cog;
 141                 int16_t         mag_var;
 142                 int16_t         climb_rate;
 143                 int16_t         heading_rate;
 144                 uint32_t        h_error;
 145                 uint32_t        v_error;
 146                 uint32_t        t_error;
 147                 uint16_t        h_v_error;
 148
 149 #define get_u8(u)       u = (msg[off]); off+= 1
 150 #define get_u16(u)      u = (msg[off] << 8) | (msg[off + 1]); off+= 2
 151 #define get_u32(u)      u = (msg[off] << 24) | (msg[off + 1] << 16) | (msg[off+2] << 8) | (msg[off+3]); off+= 4
 152
 153                 get_u8(id);
 154                 get_u16(nav_valid);
 155                 get_u16(nav_type);
 156                 get_u16(week);
 157                 get_u32(tow);
 158                 get_u16(year);
 159                 get_u8(month);
 160                 get_u8(day);
 161                 get_u8(hour);
 162                 get_u8(minute);
 163                 get_u16(second);
 164                 get_u32(sat_list);
 165                 get_u32(lat);
 166                 get_u32(lon);
 167                 get_u32(alt_ell);
 168                 get_u32(alt_msl);
 169                 get_u8(datum);
 170                 get_u16(sog);
 171                 get_u16(cog);
 172                 get_u16(mag_var);
 173                 get_u16(climb_rate);
 174                 get_u16(heading_rate);
 175                 get_u32(h_error);
 176                 get_u32(v_error);
 177                 get_u32(t_error);
 178                 get_u16(h_v_error);
 179
 180
 181                 printf ("Geodetic Navigation Data (41):\n");
 182                 printf ("\tNav valid %04x\n", nav_valid);
 183                 printf ("\tNav type %04x\n", nav_type);
 184                 printf ("\tWeek %d\n", week);
 185                 printf ("\tTOW %d\n", tow);
 186                 printf ("\tyear %d\n", year);
 187                 printf ("\tmonth %d\n", month);
 188                 printf ("\tday %d\n", day);
 189                 printf ("\thour %d\n", hour);
 190                 printf ("\tminute %d\n", minute);
 191                 printf ("\tsecond %g\n", second / 1000.0);
 192                 printf ("\tsats: %08x\n", sat_list);
 193                 printf ("\tlat: %g\n", lat / 1.0e7);
 194                 printf ("\tlon: %g\n", lon / 1.0e7);
 195                 printf ("\talt_ell: %g\n", alt_ell / 100.0);
 196                 printf ("\talt_msll: %g\n", alt_msl / 100.0);
 197                 printf ("\tdatum: %d\n", datum);
 198                 printf ("\tground speed: %g\n", sog / 100.0);
 199                 printf ("\tcourse: %g\n", cog / 100.0);
 200                 printf ("\tclimb: %g\n", climb_rate / 100.0);
 201                 printf ("\theading rate: %g\n", heading_rate / 100.0);
 202                 printf ("\th error: %g\n", h_error / 100.0);
 203                 printf ("\tv error: %g\n", v_error / 100.0);
 204                 printf ("\tt error: %g\n", t_error / 100.0);
 205                 printf ("\th vel error: %g\n", h_v_error / 100.0);
 206         } else {
 207                 return;
 208                 printf ("%s %4d:", from, encoded_len);
 209                 for (i = 4; i < len - 4; i++) {
 210                         if (((i - 4) & 0xf) == 0)
 211                                 printf("\n   ");
 212                         printf (" %3d", msg[i]);
 213                 }
 214                 printf ("\n");
 215         }
 216 }
 217
 218 static uint8_t  sirf_message[4096];
 219 static int      sirf_message_len;
 220 static uint8_t  sirf_in_message[4096];
 221 static int      sirf_in_len;
 222
 223 char
 224 ao_serial_getchar(void)
 225 {
 226         char    c;
 227         uint8_t uc;
 228
 229         while (input_head == input_tail) {
 230                 for (;;) {
 231                         input_tail = read(ao_gps_fd, input_queue, QUEUE_LEN);
 232                         if (input_tail < 0) {
 233                                 if (errno == EINTR || errno == EAGAIN)
 234                                         continue;
 235                                 perror ("getchar");
 236                                 exit (1);
 237                         }
 238                         input_head = 0;
 239                         break;
 240                 }
 241         }
 242         c = input_queue[input_head];
 243         input_head = (input_head + 1) % QUEUE_LEN;
 244         uc = c;
 245         if (sirf_in_len || uc == 0xa0) {
 246                 if (sirf_in_len < 4096)
 247                         sirf_in_message[sirf_in_len++] = uc;
 248                 if (uc == 0xb3) {
 249                         check_sirf_message("recv", sirf_in_message, sirf_in_len);
 250                         sirf_in_len = 0;
 251                 }
 252         }
 253         return c;
 254 }
 255
 256
 257 void
 258 ao_serial_putchar(char c)
 259 {
 260         int     i;
 261         uint8_t uc = (uint8_t) c;
 262
 263         if (sirf_message_len || uc == 0xa0) {
 264                 if (sirf_message_len < 4096)
 265                         sirf_message[sirf_message_len++] = uc;
 266                 if (uc == 0xb3) {
 267                         check_sirf_message("send", sirf_message, sirf_message_len);
 268                         sirf_message_len = 0;
 269                 }
 270         }
 271         for (;;) {
 272                 i = write(ao_gps_fd, &c, 1);
 273                 if (i == 1) {
 274                         if ((uint8_t) c == 0xb3 || c == '\r') {
 275                                 static const struct timespec delay = {
 276                                         .tv_sec = 0,
 277                                         .tv_nsec = 100 * 1000 * 1000
 278                                 };
 279                                 tcdrain(ao_gps_fd);
 280 //                              nanosleep(&delay, NULL);
 281                         }
 282                         break;
 283                 }
 284                 if (i < 0 && (errno == EINTR || errno == EAGAIN))
 285                         continue;
 286                 perror("putchar");
 287                 exit(1);
 288         }
 289 }
 290
 291 #define AO_SERIAL_SPEED_4800    0
 292 #define AO_SERIAL_SPEED_57600   1
 293
 294 static void
 295 ao_serial_set_speed(uint8_t speed)
 296 {
 297         int     fd = ao_gps_fd;
 298         struct termios  termios;
 299
 300         tcdrain(fd);
 301         tcgetattr(fd, &termios);
 302         switch (speed) {
 303         case AO_SERIAL_SPEED_4800:
 304                 cfsetspeed(&termios, B4800);
 305                 break;
 306         case AO_SERIAL_SPEED_57600:
 307                 cfsetspeed(&termios, B57600);
 308                 break;
 309         }
 310         tcsetattr(fd, TCSAFLUSH, &termios);
 311         tcflush(fd, TCIFLUSH);
 312 }
 313
 314 #include "ao_gps_print.c"
 315 #include "ao_gps.c"
 316
 317 void
 318 ao_dump_state(void)
 319 {
 320         double  lat, lon;
 321         printf ("%02d:%02d:%02d",
 322                 ao_gps_data.hour, ao_gps_data.minute,
 323                 ao_gps_data.second);
 324         printf (" nsat %d %svalid",
 325                 ao_gps_data.flags & AO_GPS_NUM_SAT_MASK,
 326                 ao_gps_data.flags & AO_GPS_VALID ? "" : "not ");
 327         printf (" lat %g lon %g alt %d",
 328                 ao_gps_data.latitude / 1.0e7,
 329                 ao_gps_data.longitude / 1.0e7,
 330                 ao_gps_data.altitude);
 331         printf (" speed %g climb %g course %d",
 332                 ao_gps_data.ground_speed / 100.0,
 333                 ao_gps_data.climb_rate / 100.0,
 334                 ao_gps_data.course * 2);
 335         printf (" hdop %g h_error %d v_error %d",
 336                 ao_gps_data.hdop / 5.0,
 337                 ao_gps_data.h_error, ao_gps_data.v_error);
 338         printf("\n");
 339 }
 340
 341 int
 342 ao_gps_open(const char *tty)
 343 {
 344         struct termios  termios;
 345         int fd;
 346
 347         fd = open (tty, O_RDWR);
 348         if (fd < 0)
 349                 return -1;
 350
 351         tcgetattr(fd, &termios);
 352         cfmakeraw(&termios);
 353         cfsetspeed(&termios, B4800);
 354         tcsetattr(fd, TCSAFLUSH, &termios);
 355
 356         tcdrain(fd);
 357         tcflush(fd, TCIFLUSH);
 358         return fd;
 359 }
 360
 361 int
 362 main (int argc, char **argv)
 363 {
 364         char    *gps_file = "/dev/ttyUSB0";
 365
 366         ao_gps_fd = ao_gps_open(gps_file);
 367         if (ao_gps_fd < 0) {
 368                 perror (gps_file);
 369                 exit (1);
 370         }
 371         ao_gps_setup();
 372         ao_gps();
 373 }