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