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