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