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