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         int32_t d;
 157         int32_t m;
 158         int32_t f;
 159
 160         d = ao_gps_decimal(deg_width);
 161         m = ao_gps_decimal(2);
 162         if (ao_gps_char == '.') {
 163                 f = ao_gps_decimal(4);
 164                 while (ao_gps_num_width < 4) {
 165                         f *= 10;
 166                         ao_gps_num_width++;
 167                 }
 168         } else {
 169                 f = 0;
 170                 if (ao_gps_char != ',')
 171                         ao_gps_error = 1;
 172         }
 173         d = d * 10000000l;
 174         m = m * 10000l + f;
 175         d = d + m * 50 / 3;
 176         return d;
 177 }
 178
 179 static uint8_t
 180 ao_gps_parse_flag(char no_c, char yes_c) __reentrant
 181 {
 182         uint8_t ret = 0;
 183         ao_gps_skip_sep();
 184         if (ao_gps_char == yes_c)
 185                 ret = 1;
 186         else if (ao_gps_char == no_c)
 187                 ret = 0;
 188         else
 189                 ao_gps_error = 1;
 190         ao_gps_lexchar();
 191         return ret;
 192 }
 193
 194 static void
 195 ao_nmea_gga()
 196 {
 197         uint8_t i;
 198
 199         /* Now read the data into the gps data record
 200          *
 201          * $GPGGA,025149.000,4528.1723,N,12244.2480,W,1,05,2.0,103.5,M,-19.5,M,,0000*66
 202          *
 203          * Essential fix data
 204          *
 205          *         025149.000   time (02:51:49.000 GMT)
 206          *         4528.1723,N  Latitude 45°28.1723' N
 207          *         12244.2480,W Longitude 122°44.2480' W
 208          *         1            Fix quality:
 209          *                                 0 = invalid
 210          *                                 1 = GPS fix (SPS)
 211          *                                 2 = DGPS fix
 212          *                                 3 = PPS fix
 213          *                                 4 = Real Time Kinematic
 214          *                                 5 = Float RTK
 215          *                                 6 = estimated (dead reckoning)
 216          *                                 7 = Manual input mode
 217          *                                 8 = Simulation mode
 218          *         05           Number of satellites (5)
 219          *         2.0          Horizontal dilution
 220          *         103.5,M              Altitude, 103.5M above msl
 221          *         -19.5,M              Height of geoid above WGS84 ellipsoid
 222          *         ?            time in seconds since last DGPS update
 223          *         0000         DGPS station ID
 224          *         *66          checksum
 225          */
 226
 227         ao_gps_next_tick = ao_time();
 228         ao_gps_next.flags = AO_GPS_RUNNING | ao_gps_date_flags;
 229         ao_gps_next.hour = ao_gps_decimal(2);
 230         ao_gps_next.minute = ao_gps_decimal(2);
 231         ao_gps_next.second = ao_gps_decimal(2);
 232         ao_gps_skip_field();    /* skip seconds fraction */
 233
 234         ao_gps_next.latitude = ao_gps_parse_pos(2);
 235         if (ao_gps_parse_flag('N', 'S'))
 236                 ao_gps_next.latitude = -ao_gps_next.latitude;
 237         ao_gps_next.longitude = ao_gps_parse_pos(3);
 238         if (ao_gps_parse_flag('E', 'W'))
 239                 ao_gps_next.longitude = -ao_gps_next.longitude;
 240
 241         i = ao_gps_decimal(0xff);
 242         if (i == 1)
 243                 ao_gps_next.flags |= AO_GPS_VALID;
 244
 245         i = ao_gps_decimal(0xff) << AO_GPS_NUM_SAT_SHIFT;
 246         if (i > AO_GPS_NUM_SAT_MASK)
 247                 i = AO_GPS_NUM_SAT_MASK;
 248         ao_gps_next.flags |= i;
 249
 250         ao_gps_lexchar();
 251         i = ao_gps_decimal(0xff);
 252         if (i <= 50) {
 253                 i = (uint8_t) 5 * i;
 254                 if (ao_gps_char == '.')
 255                         i = (i + ((uint8_t) ao_gps_decimal(1) >> 1));
 256         } else
 257                 i = 255;
 258         ao_gps_next.hdop = i;
 259         ao_gps_skip_field();
 260
 261         ao_gps_next.altitude = ao_gps_decimal(0xff);
 262         ao_gps_skip_field();    /* skip any fractional portion */
 263
 264         /* Skip remaining fields */
 265         while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
 266                 ao_gps_lexchar();
 267                 ao_gps_skip_field();
 268         }
 269         if (ao_gps_char == '*') {
 270                 uint8_t cksum = ao_gps_cksum ^ '*';
 271                 if (cksum != ao_gps_hex())
 272                         ao_gps_error = 1;
 273         } else
 274                 ao_gps_error = 1;
 275         if (!ao_gps_error) {
 276                 ao_mutex_get(&ao_gps_mutex);
 277                 ao_gps_tick = ao_gps_next_tick;
 278                 ao_xmemcpy(&ao_gps_data, &ao_gps_next, sizeof (ao_gps_data));
 279                 ao_mutex_put(&ao_gps_mutex);
 280                 ao_wakeup(&ao_gps_data);
 281         }
 282 }
 283
 284 static void
 285 ao_nmea_gsv(void)
 286 {
 287         char    c;
 288         uint8_t i;
 289         uint8_t done;
 290         /* Now read the data into the GPS tracking data record
 291          *
 292          * $GPGSV,3,1,12,05,54,069,45,12,44,061,44,21,07,184,46,22,78,289,47*72<CR><LF>
 293          *
 294          * Satellites in view data
 295          *
 296          *      3               Total number of GSV messages
 297          *      1               Sequence number of current GSV message
 298          *      12              Total sats in view (0-12)
 299          *      05              SVID
 300          *      54              Elevation
 301          *      069             Azimuth
 302          *      45              C/N0 in dB
 303          *      ...             other SVIDs
 304          *      72              checksum
 305          */
 306         c = ao_gps_decimal(1);  /* total messages */
 307         i = ao_gps_decimal(1);  /* message sequence */
 308         if (i == 1) {
 309                 ao_gps_tracking_next.channels = 0;
 310         }
 311         done = (uint8_t) c == i;
 312         ao_gps_lexchar();
 313         ao_gps_skip_field();    /* sats in view */
 314         while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
 315                 i = ao_gps_tracking_next.channels;
 316                 c = ao_gps_decimal(2);  /* SVID */
 317                 if (i < AO_MAX_GPS_TRACKING)
 318                         ao_gps_tracking_next.sats[i].svid = c;
 319                 ao_gps_lexchar();
 320                 ao_gps_skip_field();    /* elevation */
 321                 ao_gps_lexchar();
 322                 ao_gps_skip_field();    /* azimuth */
 323                 c = ao_gps_decimal(2);  /* C/N0 */
 324                 if (i < AO_MAX_GPS_TRACKING) {
 325                         if ((ao_gps_tracking_next.sats[i].c_n_1 = c) != 0)
 326                                 ao_gps_tracking_next.channels = i + 1;
 327                 }
 328         }
 329         if (ao_gps_char == '*') {
 330                 uint8_t cksum = ao_gps_cksum ^ '*';
 331                 if (cksum != ao_gps_hex())
 332                         ao_gps_error = 1;
 333         }
 334         else
 335                 ao_gps_error = 1;
 336         if (ao_gps_error)
 337                 ao_gps_tracking_next.channels = 0;
 338         else if (done) {
 339                 ao_mutex_get(&ao_gps_mutex);
 340                 ao_xmemcpy(&ao_gps_tracking_data, &ao_gps_tracking_next,
 341                        sizeof(ao_gps_tracking_data));
 342                 ao_mutex_put(&ao_gps_mutex);
 343                 ao_wakeup(&ao_gps_tracking_data);
 344         }
 345 }
 346
 347 static void
 348 ao_nmea_rmc(void)
 349 {
 350         char    a, c;
 351         uint8_t i;
 352         /* Parse the RMC record to read out the current date */
 353
 354         /* $GPRMC,111636.932,A,2447.0949,N,12100.5223,E,000.0,000.0,030407,,,A*61
 355          *
 356          * Recommended Minimum Specific GNSS Data
 357          *
 358          *      111636.932      UTC time 11:16:36.932
 359          *      A               Data Valid (V = receiver warning)
 360          *      2447.0949       Latitude
 361          *      N               North/south indicator
 362          *      12100.5223      Longitude
 363          *      E               East/west indicator
 364          *      000.0           Speed over ground
 365          *      000.0           Course over ground
 366          *      030407          UTC date (ddmmyy format)
 367          *      A               Mode indicator:
 368          *                      N = data not valid
 369          *                      A = autonomous mode
 370          *                      D = differential mode
 371          *                      E = estimated (dead reckoning) mode
 372          *                      M = manual input mode
 373          *                      S = simulator mode
 374          *      61              checksum
 375          */
 376         ao_gps_skip_field();
 377         for (i = 0; i < 8; i++) {
 378                 ao_gps_lexchar();
 379                 ao_gps_skip_field();
 380         }
 381         a = ao_gps_decimal(2);
 382         c = ao_gps_decimal(2);
 383         i = ao_gps_decimal(2);
 384         /* Skip remaining fields */
 385         while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
 386                 ao_gps_lexchar();
 387                 ao_gps_skip_field();
 388         }
 389         if (ao_gps_char == '*') {
 390                 uint8_t cksum = ao_gps_cksum ^ '*';
 391                 if (cksum != ao_gps_hex())
 392                         ao_gps_error = 1;
 393         } else
 394                 ao_gps_error = 1;
 395         if (!ao_gps_error) {
 396                 ao_gps_next.year = i;
 397                 ao_gps_next.month = c;
 398                 ao_gps_next.day = a;
 399                 ao_gps_date_flags = AO_GPS_DATE_VALID;
 400         }
 401 }
 402
 403 #define ao_skytraq_sendstruct(s) ao_skytraq_sendbytes((s), sizeof(s))
 404
 405 static void
 406 ao_skytraq_sendbytes(__code uint8_t *b, uint8_t l)
 407 {
 408         while (l--) {
 409                 uint8_t c = *b++;
 410                 if (c == 0xa0)
 411                         ao_delay(AO_MS_TO_TICKS(500));
 412                 ao_serial_putchar(c);
 413         }
 414 }
 415
 416 static void
 417 ao_gps_nmea_parse(void)
 418 {
 419         uint8_t a, b, c;
 420
 421         ao_gps_cksum = 0;
 422         ao_gps_error = 0;
 423
 424         for (a = 0; a < AO_GPS_LEADER; a++) {
 425                 ao_gps_lexchar();
 426                 if (ao_gps_char != ao_gps_header[a])
 427                         return;
 428         }
 429
 430         ao_gps_lexchar();
 431         a = ao_gps_char;
 432         ao_gps_lexchar();
 433         b = ao_gps_char;
 434         ao_gps_lexchar();
 435         c = ao_gps_char;
 436         ao_gps_lexchar();
 437
 438         if (ao_gps_char != ',')
 439                 return;
 440
 441         if (a == (uint8_t) 'G' && b == (uint8_t) 'G' && c == (uint8_t) 'A') {
 442                 ao_nmea_gga();
 443         } else if (a == (uint8_t) 'G' && b == (uint8_t) 'S' && c == (uint8_t) 'V') {
 444                 ao_nmea_gsv();
 445         } else if (a == (uint8_t) 'R' && b == (uint8_t) 'M' && c == (uint8_t) 'C') {
 446                 ao_nmea_rmc();
 447         }
 448 }
 449
 450 void
 451 ao_gps(void) __reentrant
 452 {
 453         ao_serial_set_speed(AO_SERIAL_SPEED_9600);
 454
 455         /* give skytraq time to boot in case of cold start */
 456         ao_delay(AO_MS_TO_TICKS(2000));
 457
 458         ao_skytraq_sendstruct(ao_gps_config);
 459
 460         for (;;) {
 461                 /* Locate the begining of the next record */
 462                 if (ao_serial_getchar() == '$') {
 463                         ao_gps_nmea_parse();
 464                 }
 465
 466         }
 467 }
 468
 469 __xdata struct ao_task ao_gps_task;
 470
 471 static void
 472 gps_dump(void) __reentrant
 473 {
 474         uint8_t i;
 475         ao_mutex_get(&ao_gps_mutex);
 476         printf ("Date: %02d/%02d/%02d\n", ao_gps_data.year, ao_gps_data.month, ao_gps_data.day);
 477         printf ("Time: %02d:%02d:%02d\n", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second);
 478         printf ("Lat/Lon: %ld %ld\n", ao_gps_data.latitude, ao_gps_data.longitude);
 479         printf ("Alt: %d\n", ao_gps_data.altitude);
 480         printf ("Flags: 0x%x\n", ao_gps_data.flags);
 481         printf ("Sats: %d", ao_gps_tracking_data.channels);
 482         for (i = 0; i < ao_gps_tracking_data.channels; i++)
 483                 printf (" %d %d",
 484                         ao_gps_tracking_data.sats[i].svid,
 485                         ao_gps_tracking_data.sats[i].c_n_1);
 486         printf ("\ndone\n");
 487         ao_mutex_put(&ao_gps_mutex);
 488 }
 489
 490 __code struct ao_cmds ao_gps_cmds[] = {
 491         { gps_dump,     "g\0Display GPS" },
 492         { 0, NULL },
 493 };
 494
 495 void
 496 ao_gps_init(void)
 497 {
 498         ao_add_task(&ao_gps_task, ao_gps, "gps");
 499         ao_cmd_register(&ao_gps_cmds[0]);
 500 }