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