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