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