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