git.gag.com Git - fw/altos/blob - src/drivers/ao_gps_skytraq.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 #ifndef AO_GPS_TEST
  19 #include "ao.h"
  20 #endif
  21
  22 #ifndef ao_gps_getchar
  23 #define ao_gps_getchar  ao_serial1_getchar
  24 #define ao_gps_fifo     ao_serial1_rx_fifo
  25 #endif
  26
  27 #ifndef ao_gps_putchar
  28 #define ao_gps_putchar  ao_serial1_putchar
  29 #endif
  30
  31 #ifndef ao_gps_set_speed
  32 #define ao_gps_set_speed        ao_serial1_set_speed
  33 #endif
  34
  35 __xdata uint8_t ao_gps_mutex;
  36 static __data char ao_gps_char;
  37 static __data uint8_t ao_gps_cksum;
  38 static __data uint8_t ao_gps_error;
  39
  40 __pdata uint16_t ao_gps_tick;
  41 __xdata struct ao_telemetry_location    ao_gps_data;
  42 __xdata struct ao_telemetry_satellite   ao_gps_tracking_data;
  43
  44 static __pdata uint16_t                         ao_gps_next_tick;
  45 static __pdata struct ao_telemetry_location     ao_gps_next;
  46 static __pdata uint8_t                          ao_gps_date_flags;
  47 static __pdata struct ao_telemetry_satellite    ao_gps_tracking_next;
  48
  49 #define STQ_S 0xa0, 0xa1
  50 #define STQ_E 0x0d, 0x0a
  51 #define SKYTRAQ_MSG_2(id,a,b) \
  52     STQ_S, 0, 3, id, a,b, (id^a^b), STQ_E
  53 #define SKYTRAQ_MSG_3(id,a,b,c) \
  54     STQ_S, 0, 4, id, a,b,c, (id^a^b^c), STQ_E
  55 #define SKYTRAQ_MSG_8(id,a,b,c,d,e,f,g,h) \
  56     STQ_S, 0, 9, id, a,b,c,d,e,f,g,h, (id^a^b^c^d^e^f^g^h), STQ_E
  57 #define SKYTRAQ_MSG_14(id,a,b,c,d,e,f,g,h,i,j,k,l,m,n) \
  58     STQ_S, 0,15, id, a,b,c,d,e,f,g,h,i,j,k,l,m,n, \
  59     (id^a^b^c^d^e^f^g^h^i^j^k^l^m^n), STQ_E
  60
  61 static __code uint8_t ao_gps_config[] = {
  62         SKYTRAQ_MSG_8(0x08, 1, 0, 1, 0, 1, 0, 0, 0), /* configure nmea */
  63         /* gga interval */
  64         /* gsa interval */
  65         /* gsv interval */
  66         /* gll interval */
  67         /* rmc interval */
  68         /* vtg interval */
  69         /* zda interval */
  70         /* attributes (0 = update to sram, 1 = update flash too) */
  71
  72         SKYTRAQ_MSG_2(0x3c, 0x00, 0x00), /* configure navigation mode */
  73         /* 0 = car, 1 = pedestrian */
  74         /* 0 = update to sram, 1 = update sram + flash */
  75 };
  76
  77 static void
  78 ao_gps_lexchar(void)
  79 {
  80         char c;
  81         if (ao_gps_error)
  82                 c = '\n';
  83         else
  84                 c = ao_gps_getchar();
  85         ao_gps_cksum ^= c;
  86         ao_gps_char = c;
  87 }
  88
  89 void
  90 ao_gps_skip_field(void)
  91 {
  92         for (;;) {
  93                 char c = ao_gps_char;
  94                 if (c == ',' || c == '*' || c == '\n')
  95                         break;
  96                 ao_gps_lexchar();
  97         }
  98 }
  99
 100 void
 101 ao_gps_skip_sep(void)
 102 {
 103         char c = ao_gps_char;
 104         if (c == ',' || c == '.' || c == '*')
 105                 ao_gps_lexchar();
 106 }
 107
 108 __data static uint8_t ao_gps_num_width;
 109
 110 static int16_t
 111 ao_gps_decimal(uint8_t max_width)
 112 {
 113         int16_t v;
 114         uint8_t neg = 0;
 115
 116         ao_gps_skip_sep();
 117         if (ao_gps_char == '-') {
 118                 neg = 1;
 119                 ao_gps_lexchar();
 120         }
 121         v = 0;
 122         ao_gps_num_width = 0;
 123         while (ao_gps_num_width < max_width) {
 124                 uint8_t c = ao_gps_char;
 125                 if (c < (uint8_t) '0' || (uint8_t) '9' < c)
 126                         break;
 127                 v = v * 10 + (uint8_t) (c - (uint8_t) '0');
 128                 ao_gps_num_width++;
 129                 ao_gps_lexchar();
 130         }
 131         if (neg)
 132                 v = -v;
 133         return v;
 134 }
 135
 136 static uint8_t
 137 ao_gps_hex(void)
 138 {
 139         uint8_t v;
 140
 141         ao_gps_skip_sep();
 142         v = 0;
 143         ao_gps_num_width = 0;
 144         while (ao_gps_num_width < 2) {
 145                 uint8_t c = ao_gps_char;
 146                 uint8_t d;
 147                 if ((uint8_t) '0' <= c && c <= (uint8_t) '9')
 148                         d = - '0';
 149                 else if ((uint8_t) 'A' <= c && c <= (uint8_t) 'F')
 150                         d = - 'A' + 10;
 151                 else if ((uint8_t) 'a' <= c && c <= (uint8_t) 'f')
 152                         d = - 'a' + 10;
 153                 else
 154                         break;
 155                 v = (v << 4) | (c + d);
 156                 ao_gps_num_width++;
 157                 ao_gps_lexchar();
 158         }
 159         return v;
 160 }
 161
 162 static int32_t
 163 ao_gps_parse_pos(uint8_t deg_width) __reentrant
 164 {
 165         static __pdata uint16_t d;
 166         static __pdata uint8_t  m;
 167         static __pdata uint16_t f;
 168         char c;
 169
 170         d = ao_gps_decimal(deg_width);
 171         m = ao_gps_decimal(2);
 172         c = ao_gps_char;
 173         if (c == '.') {
 174                 f = ao_gps_decimal(4);
 175                 while (ao_gps_num_width < 4) {
 176                         f *= 10;
 177                         ao_gps_num_width++;
 178                 }
 179         } else {
 180                 f = 0;
 181                 if (c != ',')
 182                         ao_gps_error = 1;
 183         }
 184         return d * 10000000l + (m * 10000l + f) * 50 / 3;
 185 }
 186
 187 static uint8_t
 188 ao_gps_parse_flag(char no_c, char yes_c)
 189 {
 190         uint8_t ret = 0;
 191         ao_gps_skip_sep();
 192         if (ao_gps_char == yes_c)
 193                 ret = 1;
 194         else if (ao_gps_char == no_c)
 195                 ret = 0;
 196         else
 197                 ao_gps_error = 1;
 198         ao_gps_lexchar();
 199         return ret;
 200 }
 201
 202 static void
 203 ao_nmea_finish(void)
 204 {
 205         char c;
 206         /* Skip remaining fields */
 207         for (;;) {
 208                 c = ao_gps_char;
 209                 if (c == '*' || c == '\n' || c == '\r')
 210                         break;
 211                 ao_gps_lexchar();
 212                 ao_gps_skip_field();
 213         }
 214         if (c == '*') {
 215                 uint8_t cksum = ao_gps_cksum ^ '*';
 216                 if (cksum != ao_gps_hex())
 217                         ao_gps_error = 1;
 218         } else
 219                 ao_gps_error = 1;
 220 }
 221
 222 static void
 223 ao_nmea_gga(void)
 224 {
 225         uint8_t i;
 226
 227         /* Now read the data into the gps data record
 228          *
 229          * $GPGGA,025149.000,4528.1723,N,12244.2480,W,1,05,2.0,103.5,M,-19.5,M,,0000*66
 230          *
 231          * Essential fix data
 232          *
 233          *         025149.000   time (02:51:49.000 GMT)
 234          *         4528.1723,N  Latitude 45°28.1723' N
 235          *         12244.2480,W Longitude 122°44.2480' W
 236          *         1            Fix quality:
 237          *                                 0 = invalid
 238          *                                 1 = GPS fix (SPS)
 239          *                                 2 = DGPS fix
 240          *                                 3 = PPS fix
 241          *                                 4 = Real Time Kinematic
 242          *                                 5 = Float RTK
 243          *                                 6 = estimated (dead reckoning)
 244          *                                 7 = Manual input mode
 245          *                                 8 = Simulation mode
 246          *         05           Number of satellites (5)
 247          *         2.0          Horizontal dilution
 248          *         103.5,M              Altitude, 103.5M above msl
 249          *         -19.5,M              Height of geoid above WGS84 ellipsoid
 250          *         ?            time in seconds since last DGPS update
 251          *         0000         DGPS station ID
 252          *         *66          checksum
 253          */
 254
 255         ao_gps_next_tick = ao_time();
 256         ao_gps_next.flags = AO_GPS_RUNNING | ao_gps_date_flags;
 257         ao_gps_next.hour = ao_gps_decimal(2);
 258         ao_gps_next.minute = ao_gps_decimal(2);
 259         ao_gps_next.second = ao_gps_decimal(2);
 260         ao_gps_skip_field();    /* skip seconds fraction */
 261
 262         ao_gps_next.latitude = ao_gps_parse_pos(2);
 263         if (ao_gps_parse_flag('N', 'S'))
 264                 ao_gps_next.latitude = -ao_gps_next.latitude;
 265         ao_gps_next.longitude = ao_gps_parse_pos(3);
 266         if (ao_gps_parse_flag('E', 'W'))
 267                 ao_gps_next.longitude = -ao_gps_next.longitude;
 268
 269         i = ao_gps_decimal(0xff);
 270         if (i == 1)
 271                 ao_gps_next.flags |= AO_GPS_VALID;
 272
 273         i = ao_gps_decimal(0xff) << AO_GPS_NUM_SAT_SHIFT;
 274         if (i > AO_GPS_NUM_SAT_MASK)
 275                 i = AO_GPS_NUM_SAT_MASK;
 276         ao_gps_next.flags |= i;
 277
 278         ao_gps_lexchar();
 279         i = ao_gps_decimal(0xff);
 280         if (i <= 50) {
 281                 i = (uint8_t) 5 * i;
 282                 if (ao_gps_char == '.')
 283                         i = (i + ((uint8_t) ao_gps_decimal(1) >> 1));
 284         } else
 285                 i = 255;
 286         ao_gps_next.hdop = i;
 287         ao_gps_skip_field();
 288
 289         ao_gps_next.altitude = ao_gps_decimal(0xff);
 290         ao_gps_skip_field();    /* skip any fractional portion */
 291
 292         ao_nmea_finish();
 293
 294         if (!ao_gps_error) {
 295                 ao_mutex_get(&ao_gps_mutex);
 296                 ao_gps_tick = ao_gps_next_tick;
 297                 ao_xmemcpy(&ao_gps_data, PDATA_TO_XDATA(&ao_gps_next), sizeof (ao_gps_data));
 298                 ao_mutex_put(&ao_gps_mutex);
 299                 ao_wakeup(&ao_gps_data);
 300         }
 301 }
 302
 303 static void
 304 ao_nmea_gsv(void)
 305 {
 306         char    c;
 307         uint8_t i;
 308         uint8_t done;
 309         /* Now read the data into the GPS tracking data record
 310          *
 311          * $GPGSV,3,1,12,05,54,069,45,12,44,061,44,21,07,184,46,22,78,289,47*72<CR><LF>
 312          *
 313          * Satellites in view data
 314          *
 315          *      3               Total number of GSV messages
 316          *      1               Sequence number of current GSV message
 317          *      12              Total sats in view (0-12)
 318          *      05              SVID
 319          *      54              Elevation
 320          *      069             Azimuth
 321          *      45              C/N0 in dB
 322          *      ...             other SVIDs
 323          *      72              checksum
 324          */
 325         c = ao_gps_decimal(1);  /* total messages */
 326         i = ao_gps_decimal(1);  /* message sequence */
 327         if (i == 1) {
 328                 ao_gps_tracking_next.channels = 0;
 329         }
 330         done = (uint8_t) c == i;
 331         ao_gps_lexchar();
 332         ao_gps_skip_field();    /* sats in view */
 333         while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
 334                 i = ao_gps_tracking_next.channels;
 335                 c = ao_gps_decimal(2);  /* SVID */
 336                 if (i < AO_MAX_GPS_TRACKING)
 337                         ao_gps_tracking_next.sats[i].svid = c;
 338                 ao_gps_lexchar();
 339                 ao_gps_skip_field();    /* elevation */
 340                 ao_gps_lexchar();
 341                 ao_gps_skip_field();    /* azimuth */
 342                 c = ao_gps_decimal(2);  /* C/N0 */
 343                 if (i < AO_MAX_GPS_TRACKING) {
 344                         if ((ao_gps_tracking_next.sats[i].c_n_1 = c) != 0)
 345                                 ao_gps_tracking_next.channels = i + 1;
 346                 }
 347         }
 348
 349         ao_nmea_finish();
 350
 351         if (ao_gps_error)
 352                 ao_gps_tracking_next.channels = 0;
 353         else if (done) {
 354                 ao_mutex_get(&ao_gps_mutex);
 355                 ao_xmemcpy(&ao_gps_tracking_data, PDATA_TO_XDATA(&ao_gps_tracking_next), sizeof(ao_gps_tracking_data));
 356                 ao_mutex_put(&ao_gps_mutex);
 357                 ao_wakeup(&ao_gps_tracking_data);
 358         }
 359 }
 360
 361 static void
 362 ao_nmea_rmc(void)
 363 {
 364         char    a, c;
 365         uint8_t i;
 366         /* Parse the RMC record to read out the current date */
 367
 368         /* $GPRMC,111636.932,A,2447.0949,N,12100.5223,E,000.0,000.0,030407,,,A*61
 369          *
 370          * Recommended Minimum Specific GNSS Data
 371          *
 372          *      111636.932      UTC time 11:16:36.932
 373          *      A               Data Valid (V = receiver warning)
 374          *      2447.0949       Latitude
 375          *      N               North/south indicator
 376          *      12100.5223      Longitude
 377          *      E               East/west indicator
 378          *      000.0           Speed over ground
 379          *      000.0           Course over ground
 380          *      030407          UTC date (ddmmyy format)
 381          *      A               Mode indicator:
 382          *                      N = data not valid
 383          *                      A = autonomous mode
 384          *                      D = differential mode
 385          *                      E = estimated (dead reckoning) mode
 386          *                      M = manual input mode
 387          *                      S = simulator mode
 388          *      61              checksum
 389          */
 390         ao_gps_skip_field();
 391         for (i = 0; i < 8; i++) {
 392                 ao_gps_lexchar();
 393                 ao_gps_skip_field();
 394         }
 395         a = ao_gps_decimal(2);
 396         c = ao_gps_decimal(2);
 397         i = ao_gps_decimal(2);
 398
 399         ao_nmea_finish();
 400
 401         if (!ao_gps_error) {
 402                 ao_gps_next.year = i;
 403                 ao_gps_next.month = c;
 404                 ao_gps_next.day = a;
 405                 ao_gps_date_flags = AO_GPS_DATE_VALID;
 406         }
 407 }
 408
 409 #define ao_skytraq_sendstruct(s) ao_skytraq_sendbytes((s), sizeof(s))
 410
 411 static void
 412 ao_skytraq_sendbytes(__code uint8_t *b, uint8_t l)
 413 {
 414         while (l--) {
 415                 uint8_t c = *b++;
 416                 if (c == 0xa0)
 417                         ao_delay(AO_MS_TO_TICKS(500));
 418                 ao_gps_putchar(c);
 419         }
 420 }
 421
 422 static void
 423 ao_gps_nmea_parse(void)
 424 {
 425         uint8_t a, b, c;
 426
 427         ao_gps_cksum = 0;
 428         ao_gps_error = 0;
 429
 430         ao_gps_lexchar();
 431         if (ao_gps_char != 'G')
 432                 return;
 433         ao_gps_lexchar();
 434         if (ao_gps_char != 'P')
 435                 return;
 436
 437         ao_gps_lexchar();
 438         a = ao_gps_char;
 439         ao_gps_lexchar();
 440         b = ao_gps_char;
 441         ao_gps_lexchar();
 442         c = ao_gps_char;
 443         ao_gps_lexchar();
 444
 445         if (ao_gps_char != ',')
 446                 return;
 447
 448         if (a == (uint8_t) 'G' && b == (uint8_t) 'G' && c == (uint8_t) 'A') {
 449                 ao_nmea_gga();
 450         } else if (a == (uint8_t) 'G' && b == (uint8_t) 'S' && c == (uint8_t) 'V') {
 451                 ao_nmea_gsv();
 452         } else if (a == (uint8_t) 'R' && b == (uint8_t) 'M' && c == (uint8_t) 'C') {
 453                 ao_nmea_rmc();
 454         }
 455 }
 456
 457 static uint8_t  ao_gps_updating;
 458
 459 void
 460 ao_gps(void) __reentrant
 461 {
 462         ao_gps_set_speed(AO_SERIAL_SPEED_9600);
 463
 464         /* give skytraq time to boot in case of cold start */
 465         ao_delay(AO_MS_TO_TICKS(2000));
 466
 467         ao_skytraq_sendstruct(ao_gps_config);
 468
 469         for (;;) {
 470                 /* Locate the begining of the next record */
 471                 if (ao_gps_getchar() == '$') {
 472                         ao_gps_nmea_parse();
 473                 }
 474 #ifndef AO_GPS_TEST
 475                 while (ao_gps_updating) {
 476                         ao_usb_putchar(ao_gps_getchar());
 477                         if (ao_fifo_empty(ao_gps_fifo))
 478                                 flush();
 479                 }
 480 #endif
 481         }
 482 }
 483
 484 __xdata struct ao_task ao_gps_task;
 485
 486 static void
 487 gps_dump(void) __reentrant
 488 {
 489         uint8_t i;
 490         ao_mutex_get(&ao_gps_mutex);
 491         printf ("Date: %02d/%02d/%02d\n", ao_gps_data.year, ao_gps_data.month, ao_gps_data.day);
 492         printf ("Time: %02d:%02d:%02d\n", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second);
 493         printf ("Lat/Lon: %ld %ld\n", (long) ao_gps_data.latitude, (long) ao_gps_data.longitude);
 494         printf ("Alt: %d\n", ao_gps_data.altitude);
 495         printf ("Flags: 0x%x\n", ao_gps_data.flags);
 496         printf ("Sats: %d", ao_gps_tracking_data.channels);
 497         for (i = 0; i < ao_gps_tracking_data.channels; i++)
 498                 printf (" %d %d",
 499                         ao_gps_tracking_data.sats[i].svid,
 500                         ao_gps_tracking_data.sats[i].c_n_1);
 501         printf ("\ndone\n");
 502         ao_mutex_put(&ao_gps_mutex);
 503 }
 504
 505 static __code uint8_t ao_gps_115200[] = {
 506         SKYTRAQ_MSG_3(5,0,5,0)  /* Set to 115200 baud */
 507 };
 508
 509 static void
 510 ao_gps_set_speed_delay(uint8_t speed) {
 511         ao_delay(AO_MS_TO_TICKS(500));
 512         ao_gps_set_speed(speed);
 513         ao_delay(AO_MS_TO_TICKS(500));
 514 }
 515
 516 static void
 517 gps_update(void) __reentrant
 518 {
 519         ao_gps_updating = 1;
 520         ao_task_minimize_latency = 1;
 521 #if HAS_ADC
 522         ao_timer_set_adc_interval(0);
 523 #endif
 524         ao_skytraq_sendstruct(ao_gps_115200);
 525         ao_gps_set_speed_delay(AO_SERIAL_SPEED_4800);
 526         ao_skytraq_sendstruct(ao_gps_115200);
 527         ao_gps_set_speed_delay(AO_SERIAL_SPEED_115200);
 528
 529         /* It's a binary protocol; abandon attempts to escape */
 530         for (;;)
 531                 ao_gps_putchar(ao_usb_getchar());
 532 }
 533
 534 __code struct ao_cmds ao_gps_cmds[] = {
 535         { gps_dump,     "g\0Display GPS" },
 536         { gps_update,   "U\0Update GPS firmware" },
 537         { 0, NULL },
 538 };
 539
 540 void
 541 ao_gps_init(void)
 542 {
 543         ao_add_task(&ao_gps_task, ao_gps, "gps");
 544         ao_cmd_register(&ao_gps_cmds[0]);
 545 }