Increase the initial accel/baro average to 1000 samples

[fw/altos] / ao_gps.c

diff --git a/ao_gps.c b/ao_gps.c
index e689b70496354b071de324267f6325637b1773bf..e5a5a8842d9c38f76504afef3d5a65ba1508e385 100644 (file)
--- a/ao_gps.c
+++ b/ao_gps.c
@@ -17,17 +17,18 @@
 
 #include "ao.h"
 
 
 #include "ao.h"
 

-__xdata struct ao_task ao_gps_task;
-
-#define AO_GPS_MAX_LINE                80
-

 #define AO_GPS_LEADER          6
 
 #define AO_GPS_LEADER          6
 

-__xdata uint8_t ao_gps_line[AO_GPS_MAX_LINE+1] = "GPGGA,";
+static const char ao_gps_header[] = "GPGGA,";

 __xdata uint8_t ao_gps_mutex;
 __xdata uint8_t ao_gps_mutex;

-__xdata uint8_t ao_gps_data;
+static __xdata char 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[] =
+const char ao_gps_config[] =

        "$PSRF103,00,00,01,01*25\r\n"   /* GGA 1 per sec */
        "$PSRF103,01,00,00,01*25\r\n"   /* GLL disable */
        "$PSRF103,02,00,00,01*26\r\n"   /* GSA disable */
        "$PSRF103,00,00,01,01*25\r\n"   /* GGA 1 per sec */
        "$PSRF103,01,00,00,01*25\r\n"   /* GLL disable */
        "$PSRF103,02,00,00,01*26\r\n"   /* GSA disable */


@@ -35,45 +36,240 @@ 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 */
 
        "$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(char yes_c, uint8_t yes, char 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
 
 void

-ao_gps(void)
+ao_gps(void) __reentrant

 {
 {

-       uint8_t c;
+       char    c;

        uint8_t i;
 
        for (i = 0; (c = ao_gps_config[i]); i++)
                ao_serial_putchar(c);
        for (;;) {
        uint8_t i;
 
        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 (;;) {
                        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;
                                break;

+               }

                if (i != AO_GPS_LEADER)
                        continue;
                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 == '*') {
+                       uint8_t cksum = ao_gps_cksum ^ '*';
+                       if (cksum != ao_gps_hex(2))
+                               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 (struct 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();

        }
 }
 
        }
 }
 

+__xdata struct ao_task ao_gps_task;
+
+static void
+gps_dump(void) __reentrant
+{
+       ao_mutex_get(&ao_gps_mutex);
+       ao_gps_print(&ao_gps_data);
+       ao_mutex_put(&ao_gps_mutex);
+}
+
+__code struct ao_cmds ao_gps_cmds[] = {
+       { 'g', gps_dump,        "g                                  Display current GPS values" },
+       { 0, gps_dump, NULL },
+};
+

 void
 ao_gps_init(void)
 {
 void
 ao_gps_init(void)
 {

-       ao_add_task(&ao_gps_task, ao_gps);
+       ao_add_task(&ao_gps_task, ao_gps, "gps");
+       ao_cmd_register(&ao_gps_cmds[0]);

 }
 }