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