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