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