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 <= 25) {
 282                 i = (uint8_t) 10 * i;
 283                 if (ao_gps_char == '.')
 284                         i = (i + ((uint8_t) ao_gps_decimal(1)));
 285         } else
 286                 i = 255;
 287         ao_gps_next.hdop = i;
 288         ao_gps_skip_field();
 289
 290         AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_next, ao_gps_decimal(0xff));
 291
 292         ao_gps_skip_field();    /* skip any fractional portion */
 293
 294         ao_nmea_finish();
 295
 296         if (!ao_gps_error) {
 297                 ao_mutex_get(&ao_gps_mutex);
 298                 ao_gps_new |= AO_GPS_NEW_DATA;
 299                 ao_gps_tick = ao_gps_next_tick;
 300                 ao_xmemcpy(&ao_gps_data, PDATA_TO_XDATA(&ao_gps_next), sizeof (ao_gps_data));
 301                 ao_mutex_put(&ao_gps_mutex);
 302                 ao_wakeup(&ao_gps_new);
 303         }
 304 }
 305
 306 static void
 307 ao_nmea_gsv(void)
 308 {
 309         char    c;
 310         uint8_t i;
 311         uint8_t done;
 312         /* Now read the data into the GPS tracking data record
 313          *
 314          * $GPGSV,3,1,12,05,54,069,45,12,44,061,44,21,07,184,46,22,78,289,47*72<CR><LF>
 315          *
 316          * Satellites in view data
 317          *
 318          *      3               Total number of GSV messages
 319          *      1               Sequence number of current GSV message
 320          *      12              Total sats in view (0-12)
 321          *      05              SVID
 322          *      54              Elevation
 323          *      069             Azimuth
 324          *      45              C/N0 in dB
 325          *      ...             other SVIDs
 326          *      72              checksum
 327          */
 328         c = ao_gps_decimal(1);  /* total messages */
 329         i = ao_gps_decimal(1);  /* message sequence */
 330         if (i == 1) {
 331                 ao_gps_tracking_next.channels = 0;
 332         }
 333         done = (uint8_t) c == i;
 334         ao_gps_lexchar();
 335         ao_gps_skip_field();    /* sats in view */
 336         while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
 337                 i = ao_gps_tracking_next.channels;
 338                 c = ao_gps_decimal(2);  /* SVID */
 339                 if (i < AO_MAX_GPS_TRACKING)
 340                         ao_gps_tracking_next.sats[i].svid = c;
 341                 ao_gps_lexchar();
 342                 ao_gps_skip_field();    /* elevation */
 343                 ao_gps_lexchar();
 344                 ao_gps_skip_field();    /* azimuth */
 345                 c = ao_gps_decimal(2);  /* C/N0 */
 346                 if (i < AO_MAX_GPS_TRACKING) {
 347                         if ((ao_gps_tracking_next.sats[i].c_n_1 = c) != 0)
 348                                 ao_gps_tracking_next.channels = i + 1;
 349                 }
 350         }
 351
 352         ao_nmea_finish();
 353
 354         if (ao_gps_error)
 355                 ao_gps_tracking_next.channels = 0;
 356         else if (done) {
 357                 ao_mutex_get(&ao_gps_mutex);
 358                 ao_gps_new |= AO_GPS_NEW_TRACKING;
 359                 ao_xmemcpy(&ao_gps_tracking_data, PDATA_TO_XDATA(&ao_gps_tracking_next), sizeof(ao_gps_tracking_data));
 360                 ao_mutex_put(&ao_gps_mutex);
 361                 ao_wakeup(&ao_gps_new);
 362         }
 363 }
 364
 365 static void
 366 ao_nmea_rmc(void)
 367 {
 368         char    a, c;
 369         uint8_t i;
 370         /* Parse the RMC record to read out the current date */
 371
 372         /* $GPRMC,111636.932,A,2447.0949,N,12100.5223,E,000.0,000.0,030407,,,A*61
 373          *
 374          * Recommended Minimum Specific GNSS Data
 375          *
 376          *      111636.932      UTC time 11:16:36.932
 377          *      A               Data Valid (V = receiver warning)
 378          *      2447.0949       Latitude
 379          *      N               North/south indicator
 380          *      12100.5223      Longitude
 381          *      E               East/west indicator
 382          *      000.0           Speed over ground
 383          *      000.0           Course over ground
 384          *      030407          UTC date (ddmmyy format)
 385          *      A               Mode indicator:
 386          *                      N = data not valid
 387          *                      A = autonomous mode
 388          *                      D = differential mode
 389          *                      E = estimated (dead reckoning) mode
 390          *                      M = manual input mode
 391          *                      S = simulator mode
 392          *      61              checksum
 393          */
 394         ao_gps_skip_field();
 395         for (i = 0; i < 8; i++) {
 396                 ao_gps_lexchar();
 397                 ao_gps_skip_field();
 398         }
 399         a = ao_gps_decimal(2);
 400         c = ao_gps_decimal(2);
 401         i = ao_gps_decimal(2);
 402
 403         ao_nmea_finish();
 404
 405         if (!ao_gps_error) {
 406                 ao_gps_next.year = i;
 407                 ao_gps_next.month = c;
 408                 ao_gps_next.day = a;
 409                 ao_gps_date_flags = AO_GPS_DATE_VALID;
 410         }
 411 }
 412
 413 #define ao_skytraq_sendstruct(s) ao_skytraq_sendbytes((s), sizeof(s))
 414
 415 static void
 416 ao_skytraq_sendbytes(__code uint8_t *b, uint8_t l)
 417 {
 418         while (l--) {
 419                 uint8_t c = *b++;
 420                 if (c == 0xa0)
 421                         ao_delay(AO_MS_TO_TICKS(500));
 422                 ao_gps_putchar(c);
 423         }
 424 }
 425
 426 static void
 427 ao_gps_nmea_parse(void)
 428 {
 429         uint8_t a, b, c;
 430
 431         ao_gps_cksum = 0;
 432         ao_gps_error = 0;
 433
 434         ao_gps_lexchar();
 435         if (ao_gps_char != 'G')
 436                 return;
 437         ao_gps_lexchar();
 438         if (ao_gps_char != 'P')
 439                 return;
 440
 441         ao_gps_lexchar();
 442         a = ao_gps_char;
 443         ao_gps_lexchar();
 444         b = ao_gps_char;
 445         ao_gps_lexchar();
 446         c = ao_gps_char;
 447         ao_gps_lexchar();
 448
 449         if (ao_gps_char != ',')
 450                 return;
 451
 452         if (a == (uint8_t) 'G' && b == (uint8_t) 'G' && c == (uint8_t) 'A') {
 453                 ao_nmea_gga();
 454         } else if (a == (uint8_t) 'G' && b == (uint8_t) 'S' && c == (uint8_t) 'V') {
 455                 ao_nmea_gsv();
 456         } else if (a == (uint8_t) 'R' && b == (uint8_t) 'M' && c == (uint8_t) 'C') {
 457                 ao_nmea_rmc();
 458         }
 459 }
 460
 461 static uint8_t  ao_gps_updating;
 462
 463 void
 464 ao_gps(void) __reentrant
 465 {
 466         ao_gps_set_speed(AO_SERIAL_SPEED_9600);
 467
 468         /* give skytraq time to boot in case of cold start */
 469         ao_delay(AO_MS_TO_TICKS(2000));
 470
 471         ao_skytraq_sendstruct(ao_gps_config);
 472
 473         for (;;) {
 474                 /* Locate the begining of the next record */
 475                 if (ao_gps_getchar() == '$') {
 476                         ao_gps_nmea_parse();
 477                 }
 478 #ifndef AO_GPS_TEST
 479                 while (ao_gps_updating) {
 480                         ao_usb_putchar(ao_gps_getchar());
 481                         if (ao_fifo_empty(ao_gps_fifo))
 482                                 flush();
 483                 }
 484 #endif
 485         }
 486 }
 487
 488 __xdata struct ao_task ao_gps_task;
 489
 490 static __code uint8_t ao_gps_115200[] = {
 491         SKYTRAQ_MSG_3(5,0,5,0)  /* Set to 115200 baud */
 492 };
 493
 494 static void
 495 ao_gps_set_speed_delay(uint8_t speed) {
 496         ao_delay(AO_MS_TO_TICKS(500));
 497         ao_gps_set_speed(speed);
 498         ao_delay(AO_MS_TO_TICKS(500));
 499 }
 500
 501 static void
 502 gps_update(void) __reentrant
 503 {
 504         ao_gps_updating = 1;
 505         ao_task_minimize_latency = 1;
 506 #if HAS_ADC
 507         ao_timer_set_adc_interval(0);
 508 #endif
 509         ao_skytraq_sendstruct(ao_gps_115200);
 510         ao_gps_set_speed_delay(AO_SERIAL_SPEED_4800);
 511         ao_skytraq_sendstruct(ao_gps_115200);
 512         ao_gps_set_speed_delay(AO_SERIAL_SPEED_115200);
 513
 514         /* It's a binary protocol; abandon attempts to escape */
 515         for (;;)
 516                 ao_gps_putchar(ao_usb_getchar());
 517 }
 518
 519 __code struct ao_cmds ao_gps_cmds[] = {
 520         { ao_gps_show,  "g\0Display GPS" },
 521         { gps_update,   "U\0Update GPS firmware" },
 522         { 0, NULL },
 523 };
 524
 525 void
 526 ao_gps_init(void)
 527 {
 528         ao_add_task(&ao_gps_task, ao_gps, "gps");
 529         ao_cmd_register(&ao_gps_cmds[0]);
 530 }