git.gag.com Git - fw/altos/blob - ao_gps.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 #include "ao.h"
  19
  20 #define AO_GPS_LEADER           6
  21
  22 static const char ao_gps_header[] = "GPGGA,";
  23 __xdata uint8_t ao_gps_mutex;
  24 static __xdata char ao_gps_char;
  25 static __xdata uint8_t ao_gps_cksum;
  26 static __xdata uint8_t ao_gps_error;
  27
  28 __xdata struct ao_gps_data      ao_gps_data;
  29 static __xdata struct ao_gps_data       ao_gps_next;
  30
  31 const char ao_gps_config[] =
  32         "$PSRF103,00,00,01,01*25\r\n"   /* GGA 1 per sec */
  33         "$PSRF103,01,00,00,01*25\r\n"   /* GLL disable */
  34         "$PSRF103,02,00,00,01*26\r\n"   /* GSA disable */
  35         "$PSRF103,03,00,00,01*27\r\n"   /* GSV disable */
  36         "$PSRF103,04,00,00,01*20\r\n"   /* RMC disable */
  37         "$PSRF103,05,00,00,01*21\r\n";  /* VTG disable */
  38
  39 static void
  40 ao_gps_lexchar(void)
  41 {
  42         if (ao_gps_error)
  43                 ao_gps_char = '\n';
  44         else
  45                 ao_gps_char = ao_serial_getchar();
  46         ao_gps_cksum ^= ao_gps_char;
  47 }
  48
  49 void
  50 ao_gps_skip(void)
  51 {
  52         while (ao_gps_char >= '0')
  53                 ao_gps_lexchar();
  54 }
  55
  56 void
  57 ao_gps_skip_field(void)
  58 {
  59         while (ao_gps_char != ',' && ao_gps_char != '*' && ao_gps_char != '\n')
  60                 ao_gps_lexchar();
  61 }
  62
  63 void
  64 ao_gps_skip_sep(void)
  65 {
  66         while (ao_gps_char == ',' || ao_gps_char == '.' || ao_gps_char == '*')
  67                 ao_gps_lexchar();
  68 }
  69
  70 __xdata static uint8_t ao_gps_num_width;
  71
  72 static int16_t
  73 ao_gps_decimal(uint8_t max_width)
  74 {
  75         int16_t v;
  76         __xdata uint8_t neg = 0;
  77
  78         ao_gps_skip_sep();
  79         if (ao_gps_char == '-') {
  80                 neg = 1;
  81                 ao_gps_lexchar();
  82         }
  83         v = 0;
  84         ao_gps_num_width = 0;
  85         while (ao_gps_num_width < max_width) {
  86                 if (ao_gps_char < '0' || '9' < ao_gps_char)
  87                         break;
  88                 v = v * (int16_t) 10 + ao_gps_char - '0';
  89                 ao_gps_num_width++;
  90                 ao_gps_lexchar();
  91         }
  92         if (neg)
  93                 v = -v;
  94         return v;
  95 }
  96
  97 static uint8_t
  98 ao_gps_hex(uint8_t max_width)
  99 {
 100         uint8_t v, d;
 101
 102         ao_gps_skip_sep();
 103         v = 0;
 104         ao_gps_num_width = 0;
 105         while (ao_gps_num_width < max_width) {
 106                 if ('0' <= ao_gps_char && ao_gps_char <= '9')
 107                         d = ao_gps_char - '0';
 108                 else if ('A' <= ao_gps_char && ao_gps_char <= 'F')
 109                         d = ao_gps_char - 'A' + 10;
 110                 else if ('a' <= ao_gps_char && ao_gps_char <= 'f')
 111                         d = ao_gps_char - 'a' + 10;
 112                 else
 113                         break;
 114                 v = (v << 4) | d;
 115                 ao_gps_num_width++;
 116                 ao_gps_lexchar();
 117         }
 118         return v;
 119 }
 120
 121 static void
 122 ao_gps_parse_pos(__xdata struct ao_gps_pos * pos, uint8_t deg_width) __reentrant
 123 {
 124         pos->degrees = ao_gps_decimal(deg_width);
 125         pos->minutes = ao_gps_decimal(2);
 126         if (ao_gps_char == '.') {
 127                 pos->minutes_fraction = ao_gps_decimal(4);
 128                 while (ao_gps_num_width < 4) {
 129                         pos->minutes_fraction *= 10;
 130                         ao_gps_num_width++;
 131                 }
 132         } else {
 133                 pos->minutes_fraction = 0;
 134                 if (ao_gps_char != ',')
 135                         ao_gps_error = 1;
 136         }
 137 }
 138
 139 static void
 140 ao_gps_parse_flag(char yes_c, uint8_t yes, char no_c, uint8_t no) __reentrant
 141 {
 142         ao_gps_skip_sep();
 143         if (ao_gps_char == yes_c)
 144                 ao_gps_next.flags |= yes;
 145         else if (ao_gps_char == no_c)
 146                 ao_gps_next.flags |= no;
 147         else
 148                 ao_gps_error = 1;
 149         ao_gps_lexchar();
 150 }
 151
 152
 153 void
 154 ao_gps(void) __reentrant
 155 {
 156         char    c;
 157         uint8_t i;
 158
 159         for (i = 0; (c = ao_gps_config[i]); i++)
 160                 ao_serial_putchar(c);
 161         for (;;) {
 162                 /* Locate the begining of the next record */
 163                 for (;;) {
 164                         c = ao_serial_getchar();
 165                         if (c == '$')
 166                                 break;
 167                 }
 168
 169                 ao_gps_cksum = 0;
 170                 ao_gps_error = 0;
 171
 172                 /* Skip anything other than GGA */
 173                 for (i = 0; i < AO_GPS_LEADER; i++) {
 174                         ao_gps_lexchar();
 175                         if (ao_gps_char != ao_gps_header[i])
 176                                 break;
 177                 }
 178                 if (i != AO_GPS_LEADER)
 179                         continue;
 180
 181                 /* Now read the data into the gps data record
 182                  *
 183                  * $GPGGA,025149.000,4528.1723,N,12244.2480,W,1,05,2.0,103.5,M,-19.5,M,,0000*66
 184                  *
 185                  * Essential fix data
 186                  *
 187                  *         025149.000   time (02:51:49.000 GMT)
 188                  *         4528.1723,N  Latitude 45°28.1723' N
 189                  *         12244.2480,W Longitude 122°44.2480' W
 190                  *         1            Fix quality:
 191                  *                                 0 = invalid
 192                  *                                 1 = GPS fix (SPS)
 193                  *                                 2 = DGPS fix
 194                  *                                 3 = PPS fix
 195                  *                                 4 = Real Time Kinematic
 196                  *                                 5 = Float RTK
 197                  *                                 6 = estimated (dead reckoning)
 198                  *                                 7 = Manual input mode
 199                  *                                 8 = Simulation mode
 200                  *         05           Number of satellites (5)
 201                  *         2.0          Horizontal dilution
 202                  *         103.5,M              Altitude, 103.5M above msl
 203                  *         -19.5,M              Height of geoid above WGS84 ellipsoid
 204                  *         ?            time in seconds since last DGPS update
 205                  *         0000         DGPS station ID
 206                  *         *66          checksum
 207                  */
 208
 209                 ao_gps_next.flags = 0;
 210                 ao_gps_next.hour = ao_gps_decimal(2);
 211                 ao_gps_next.minute = ao_gps_decimal(2);
 212                 ao_gps_next.second = ao_gps_decimal(2);
 213                 ao_gps_skip_field();    /* skip seconds fraction */
 214
 215                 ao_gps_parse_pos(&ao_gps_next.latitude, 2);
 216                 ao_gps_parse_flag('N', AO_GPS_LATITUDE_NORTH, 'S', AO_GPS_LATITUDE_SOUTH);
 217                 ao_gps_parse_pos(&ao_gps_next.longitude, 3);
 218                 ao_gps_parse_flag('W', AO_GPS_LONGITUDE_WEST, 'E', AO_GPS_LONGITUDE_EAST);
 219
 220                 i = ao_gps_decimal(0xff);
 221                 if (i == 1)
 222                         ao_gps_next.flags |= AO_GPS_VALID;
 223
 224                 i = ao_gps_decimal(0xff) << AO_GPS_NUM_SAT_SHIFT;
 225                 if (i > AO_GPS_NUM_SAT_MASK)
 226                         i = AO_GPS_NUM_SAT_MASK;
 227                 ao_gps_next.flags |= i;
 228
 229                 ao_gps_lexchar();
 230                 ao_gps_skip_field();    /* Horizontal dilution */
 231
 232                 ao_gps_next.altitude = ao_gps_decimal(0xff);
 233                 ao_gps_skip_field();    /* skip any fractional portion */
 234
 235                 /* Skip remaining fields */
 236                 while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
 237                         ao_gps_lexchar();
 238                         ao_gps_skip_field();
 239                 }
 240                 if (ao_gps_char == '*') {
 241                         uint8_t cksum = ao_gps_cksum ^ '*';
 242                         if (cksum != ao_gps_hex(2))
 243                                 ao_gps_error = 1;
 244                 } else
 245                         ao_gps_error = 1;
 246                 if (!ao_gps_error) {
 247                         ao_mutex_get(&ao_gps_mutex);
 248                         memcpy(&ao_gps_data, &ao_gps_next, sizeof (struct ao_gps_data));
 249                         ao_mutex_put(&ao_gps_mutex);
 250                         ao_wakeup(&ao_gps_data);
 251                 }
 252         }
 253 }
 254
 255 __xdata struct ao_task ao_gps_task;
 256
 257 static void
 258 gps_dump(void) __reentrant
 259 {
 260         ao_mutex_get(&ao_gps_mutex);
 261         ao_gps_print(&ao_gps_data);
 262         ao_mutex_put(&ao_gps_mutex);
 263 }
 264
 265 __code struct ao_cmds ao_gps_cmds[] = {
 266         { 'g', gps_dump,        "g                                  Display current GPS values" },
 267         { 0, gps_dump, NULL },
 268 };
 269
 270 void
 271 ao_gps_init(void)
 272 {
 273         ao_add_task(&ao_gps_task, ao_gps, "gps");
 274         ao_cmd_register(&ao_gps_cmds[0]);
 275 }