Parse GPS data locally. Add 'g' command to display recent GPS results.

This parses the GPS GGA message and stores it in a global variable,
protected by a mutex.

diff --git a/ao.h b/ao.h
index 20c2458cd87fc72766117784c25967120d3f409f..bbf358e971dc1328fc8cb30db69d2fb77458c8d9 100644 (file)
--- a/ao.h
+++ b/ao.h
@@ -477,15 +477,15 @@ enum ao_flight_state {
        ao_flight_invalid
 };
 
-extern __xdata struct ao_adc   ao_flight_data;
-extern __data enum flight_state        ao_flight_state;
-extern __data uint16_t                 ao_flight_tick;
-extern __data int16_t                  ao_flight_accel;
-extern __data int16_t                  ao_flight_pres;
-extern __data int16_t                  ao_ground_pres;
-extern __data int16_t                  ao_ground_accel;
-extern __data int16_t                  ao_min_pres;
-extern __data uint16_t                 ao_launch_time;
+extern __xdata struct ao_adc           ao_flight_data;
+extern __pdata enum flight_state       ao_flight_state;
+extern __pdata uint16_t                        ao_flight_tick;
+extern __pdata int16_t                 ao_flight_accel;
+extern __pdata int16_t                 ao_flight_pres;
+extern __pdata int16_t                 ao_ground_pres;
+extern __pdata int16_t                 ao_ground_accel;
+extern __pdata int16_t                 ao_min_pres;
+extern __pdata uint16_t                        ao_launch_time;
 
 /* Flight thread */
 void
@@ -585,6 +585,37 @@ ao_serial_init(void);
  * ao_gps.c
  */
 
+struct ao_gps_pos {
+       uint8_t degrees;
+       uint8_t minutes;
+       uint16_t minutes_fraction;      /* in units of 1/10000 minutes */
+};
+
+#define AO_GPS_NUM_SAT_MASK    (0xf << 0)
+#define AO_GPS_NUM_SAT_SHIFT   (0)
+
+#define AO_GPS_VALID           (1 << 4)
+#define AO_GPS_LONGITUDE_MASK  (1 << 5)
+#define AO_GPS_LONGITUDE_EAST  (0 << 5)
+#define AO_GPS_LONGITUDE_WEST  (1 << 5)
+
+#define AO_GPS_LATITUDE_MASK   (1 << 6)
+#define AO_GPS_LATITUDE_NORTH  (0 << 6)
+#define AO_GPS_LATITUDE_SOUTH  (1 << 6)
+
+struct ao_gps_data {
+       uint8_t                 hour;
+       uint8_t                 minute;
+       uint8_t                 second;
+       uint8_t                 flags;
+       struct ao_gps_pos       latitude;
+       struct ao_gps_pos       longitude;
+       int16_t                 altitude;
+};
+
+extern __xdata uint8_t ao_gps_mutex;
+extern __xdata struct ao_gps_data ao_gps_data;
+
 void
 ao_gps(void);
 

diff --git a/ao_cmd.c b/ao_cmd.c
index 575ed69aec8c3b301b4d36e4bfd2bf104b3ab7c3..6e91a72d5b452c31e75a01140afd9a4af20fd4a4 100644 (file)
--- a/ao_cmd.c
+++ b/ao_cmd.c
@@ -252,6 +252,34 @@ adc_dump(void)
        put_string("\n");
 }
 
+static void
+gps_dump(void) __reentrant
+{
+       ao_mutex_get(&ao_gps_mutex);
+       if (ao_gps_data.flags & AO_GPS_VALID) {
+               printf("GPS %2d:%02d:%02d %2d°%2d.%04d'%c %2d°%2d.%04d'%c %5dm %2d sat\n",
+                      ao_gps_data.hour,
+                      ao_gps_data.minute,
+                      ao_gps_data.second,
+                      ao_gps_data.latitude.degrees,
+                      ao_gps_data.latitude.minutes,
+                      ao_gps_data.latitude.minutes_fraction,
+                      (ao_gps_data.flags & AO_GPS_LATITUDE_MASK) == AO_GPS_LATITUDE_NORTH ?
+                      'N' : 'S',
+                      ao_gps_data.longitude.degrees,
+                      ao_gps_data.longitude.minutes,
+                      ao_gps_data.longitude.minutes_fraction,
+                      (ao_gps_data.flags & AO_GPS_LONGITUDE_MASK) == AO_GPS_LONGITUDE_WEST ?
+                      'W' : 'E',
+                      ao_gps_data.altitude,
+                      (ao_gps_data.flags & AO_GPS_NUM_SAT_MASK) >> AO_GPS_NUM_SAT_SHIFT);
+       } else {
+               printf("GPS %2d sat\n",
+                      (ao_gps_data.flags & AO_GPS_NUM_SAT_MASK) >> AO_GPS_NUM_SAT_SHIFT);;
+       }
+       ao_mutex_put(&ao_gps_mutex);
+}
+
 static void
 dump(void)
 {
@@ -488,6 +516,7 @@ static const uint8_t help_txt[] =
        "All numbers are in hex\n"
        "?                                  Print this message\n"
        "a                                  Display current ADC values\n"
+       "g                                  Display current GPS values\n"
        "d <start> <end>                    Dump memory\n"
        "e <block>                          Dump a block of EEPROM data\n"
        "w <block> <start> <len> <data> ... Write data to EEPROM\n"
@@ -546,6 +575,9 @@ ao_cmd(void *parameters)
                case 'a':
                        adc_dump();
                        break;
+               case 'g':
+                       gps_dump();
+                       break;
                case 'e':
                        ee_dump();
                        break;

diff --git a/ao_gps.c b/ao_gps.c
index e689b70496354b071de324267f6325637b1773bf..83f44d55f42f80c0c738b5f754d04b6c3f40d610 100644 (file)
--- a/ao_gps.c
+++ b/ao_gps.c
@@ -19,13 +19,16 @@
 
 __xdata struct ao_task ao_gps_task;
 
-#define AO_GPS_MAX_LINE                80
-
 #define AO_GPS_LEADER          6
 
-__xdata uint8_t ao_gps_line[AO_GPS_MAX_LINE+1] = "GPGGA,";
+static const uint8_t ao_gps_header[] = "GPGGA,";
 __xdata uint8_t ao_gps_mutex;
-__xdata uint8_t ao_gps_data;
+static __xdata uint8_t ao_gps_char;
+static __xdata uint8_t ao_gps_cksum;
+static __xdata uint8_t ao_gps_error;
+
+__xdata struct ao_gps_data     ao_gps_data;
+static __xdata struct ao_gps_data      ao_gps_next;
 
 const uint8_t ao_gps_config[] =
        "$PSRF103,00,00,01,01*25\r\n"   /* GGA 1 per sec */
@@ -35,9 +38,122 @@ const uint8_t ao_gps_config[] =
        "$PSRF103,04,00,00,01*20\r\n"   /* RMC disable */
        "$PSRF103,05,00,00,01*21\r\n";  /* VTG disable */
 
+static void
+ao_gps_lexchar(void)
+{
+       if (ao_gps_error)
+               ao_gps_char = '\n';
+       else 
+               ao_gps_char = ao_serial_getchar();
+       ao_gps_cksum ^= ao_gps_char;
+}
+
+void
+ao_gps_skip(void)
+{
+       while (ao_gps_char >= '0')
+               ao_gps_lexchar();
+}
+
+void
+ao_gps_skip_field(void)
+{
+       while (ao_gps_char != ',' && ao_gps_char != '*' && ao_gps_char != '\n')
+               ao_gps_lexchar();
+}
+
+void
+ao_gps_skip_sep(void)
+{
+       while (ao_gps_char == ',' || ao_gps_char == '.' || ao_gps_char == '*')
+               ao_gps_lexchar();
+}
+
+__xdata static uint8_t ao_gps_num_width;
+
+static int16_t
+ao_gps_decimal(uint8_t max_width)
+{
+       int16_t v;
+       __xdata uint8_t neg = 0;
+       
+       ao_gps_skip_sep();
+       if (ao_gps_char == '-') {
+               neg = 1;
+               ao_gps_lexchar();
+       }
+       v = 0;
+       ao_gps_num_width = 0;
+       while (ao_gps_num_width < max_width) {
+               if (ao_gps_char < '0' || '9' < ao_gps_char)
+                       break;
+               v = v * (int16_t) 10 + ao_gps_char - '0';
+               ao_gps_num_width++;
+               ao_gps_lexchar();
+       }
+       if (neg)
+               v = -v;
+       return v;
+}
+
+static uint8_t
+ao_gps_hex(uint8_t max_width)
+{
+       uint8_t v, d;
+
+       ao_gps_skip_sep();
+       v = 0;
+       ao_gps_num_width = 0;
+       while (ao_gps_num_width < max_width) {
+               if ('0' <= ao_gps_char && ao_gps_char <= '9')
+                       d = ao_gps_char - '0';
+               else if ('A' <= ao_gps_char && ao_gps_char <= 'F')
+                       d = ao_gps_char - 'A' + 10;
+               else if ('a' <= ao_gps_char && ao_gps_char <= 'f')
+                       d = ao_gps_char - 'a' + 10;
+               else
+                       break;
+               v = (v << 4) | d;
+               ao_gps_num_width++;
+               ao_gps_lexchar();
+       }
+       return v;
+}
+
+static void
+ao_gps_parse_pos(__xdata struct ao_gps_pos * pos, uint8_t deg_width) __reentrant
+{
+       pos->degrees = ao_gps_decimal(deg_width);
+       pos->minutes = ao_gps_decimal(2);
+       if (ao_gps_char == '.') {
+               pos->minutes_fraction = ao_gps_decimal(4);
+               while (ao_gps_num_width < 4) {
+                       pos->minutes_fraction *= 10;
+                       ao_gps_num_width++;
+               }
+       } else {
+               pos->minutes_fraction = 0;
+               if (ao_gps_char != ',')
+                       ao_gps_error = 1;
+       }
+}
+
+static void
+ao_gps_parse_flag(uint8_t yes_c, uint8_t yes, uint8_t no_c, uint8_t no) __reentrant
+{
+       ao_gps_skip_sep();
+       if (ao_gps_char == yes_c)
+               ao_gps_next.flags |= yes;
+       else if (ao_gps_char == no_c)
+               ao_gps_next.flags |= no;
+       else
+               ao_gps_error = 1;
+       ao_gps_lexchar();
+}
+
 
 void
-ao_gps(void)
+ao_gps(void) __reentrant
 {
        uint8_t c;
        uint8_t i;
@@ -45,35 +161,102 @@ ao_gps(void)
        for (i = 0; (c = ao_gps_config[i]); i++)
                ao_serial_putchar(c);
        for (;;) {
+               /* Locate the begining of the next record */
                for (;;) {
                        c = ao_serial_getchar();
                        if (c == '$')
                                break;
                }
-               for (i = 0; i < AO_GPS_LEADER; i++)
-                       if (ao_serial_getchar() != ao_gps_line[i])
+
+               ao_gps_cksum = 0;
+               ao_gps_error = 0;
+               
+               /* Skip anything other than GGA */
+               for (i = 0; i < AO_GPS_LEADER; i++) {
+                       ao_gps_lexchar();
+                       if (ao_gps_char != ao_gps_header[i])
                                break;
+               }
                if (i != AO_GPS_LEADER)
                        continue;
-               ao_mutex_get(&ao_gps_mutex);
-               for (;;) {
-                       c = ao_serial_getchar();
-                       if (c < ' ')
-                               break;
-                       if (i < AO_GPS_MAX_LINE)
-                               ao_gps_line[i++] = c;
+
+               /* Now read the data into the gps data record
+                *
+                * $GPGGA,025149.000,4528.1723,N,12244.2480,W,1,05,2.0,103.5,M,-19.5,M,,0000*66
+                *  
+                * Essential fix data
+                * 
+                *         025149.000   time (02:51:49.000 GMT)
+                *         4528.1723,N  Latitude 45°28.1723' N
+                *         12244.2480,W Longitude 122°44.2480' W
+                *         1            Fix quality:
+                *                                 0 = invalid
+                *                                 1 = GPS fix (SPS)
+                *                                 2 = DGPS fix
+                *                                 3 = PPS fix
+                *                                 4 = Real Time Kinematic
+                *                                 5 = Float RTK
+                *                                 6 = estimated (dead reckoning)
+                *                                 7 = Manual input mode
+                *                                 8 = Simulation mode
+                *         05           Number of satellites (5)
+                *         2.0          Horizontal dilution
+                *         103.5,M              Altitude, 103.5M above msl
+                *         -19.5,M              Height of geoid above WGS84 ellipsoid
+                *         ?            time in seconds since last DGPS update
+                *         0000         DGPS station ID
+                *         *66          checksum
+                */
+
+               ao_gps_next.flags = 0;
+               ao_gps_next.hour = ao_gps_decimal(2);
+               ao_gps_next.minute = ao_gps_decimal(2);
+               ao_gps_next.second = ao_gps_decimal(2);
+               ao_gps_skip_field();    /* skip seconds fraction */
+               
+               ao_gps_parse_pos(&ao_gps_next.latitude, 2);
+               ao_gps_parse_flag('N', AO_GPS_LATITUDE_NORTH, 'S', AO_GPS_LATITUDE_SOUTH);
+               ao_gps_parse_pos(&ao_gps_next.longitude, 3);
+               ao_gps_parse_flag('W', AO_GPS_LONGITUDE_WEST, 'E', AO_GPS_LONGITUDE_EAST);
+               
+               i = ao_gps_decimal(0xff);
+               if (i == 1)
+                       ao_gps_next.flags |= AO_GPS_VALID;
+               
+               i = ao_gps_decimal(0xff) << AO_GPS_NUM_SAT_SHIFT;
+               if (i > AO_GPS_NUM_SAT_MASK)
+                       i = AO_GPS_NUM_SAT_MASK;
+               ao_gps_next.flags |= i;
+               
+               ao_gps_lexchar();
+               ao_gps_skip_field();    /* Horizontal dilution */
+               
+               ao_gps_next.altitude = ao_gps_decimal(0xff);
+               ao_gps_skip_field();    /* skip any fractional portion */
+               
+               /* Skip remaining fields */
+               while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
+                       ao_gps_lexchar();
+                       ao_gps_skip_field();
+               }
+               if (ao_gps_char == '*') {
+                       c = ao_gps_cksum ^ '*';
+                       i = ao_gps_hex(2);
+                       if (c != i)
+                               ao_gps_error = 1;
+               } else 
+                       ao_gps_error = 1;
+               if (!ao_gps_error) {
+                       ao_mutex_get(&ao_gps_mutex);
+                       memcpy(&ao_gps_data, &ao_gps_next, sizeof (ao_gps_data));
+                       ao_mutex_put(&ao_gps_mutex);
+                       ao_wakeup(&ao_gps_data);
                }
-               ao_gps_line[i] = '\0';
-               ao_gps_data = 1;
-               ao_mutex_put(&ao_gps_mutex);
-               ao_wakeup(&ao_gps_data);
-               puts(ao_gps_line);
-               ao_usb_flush();
        }
 }
 
 void
 ao_gps_init(void)
 {
-       ao_add_task(&ao_gps_task, ao_gps);
+       ao_add_task(&ao_gps_task, ao_gps, "gps");
 }