Merge branch 'master' into skytraq
authorKeith Packard <keithp@keithp.com>
Sat, 10 Oct 2009 20:41:00 +0000 (13:41 -0700)
committerKeith Packard <keithp@keithp.com>
Sat, 10 Oct 2009 20:41:00 +0000 (13:41 -0700)
.gitignore
src/Makefile
src/ao.h
src/ao_gps.c [deleted file]
src/ao_gps_sirf.c [new file with mode: 0644]
src/ao_gps_skytraq.c [new file with mode: 0644]
src/ao_gps_test.c
src/ao_gps_test_skytraq.c [new file with mode: 0644]
src/ao_serial.c
src/ao_teleterra.c
src/skytraq-cksum [new file with mode: 0644]

index 0ca4bed47f8dd406942e314167f798b8bb30d0b1..a6f94439108a0dbe2a3e8208a0b4ba45642abd7a 100644 (file)
@@ -16,6 +16,7 @@ TAGS
 aclocal.m4
 src/ao_flight_test
 src/ao_gps_test
+src/ao_gps_test_skytraq
 ao-teledongle.h
 ao-telemetrum.h
 ao-teleterra.h
@@ -47,6 +48,7 @@ missing
 stamp-h1
 tags
 teledongle
-telemetrum
+telemetrum-sirf
+telemetrum-sky
 teleterra
 tidongle
index 828c48bdc5fc6775cf30ab81317bb623a6ccbd63..9891cdadf708947bf838bd43b2cabe83e9908aa0 100644 (file)
@@ -59,7 +59,6 @@ TELE_RECEIVER_SRC =\
 
 TELE_DRIVER_SRC = \
        ao_convert.c \
-       ao_gps.c \
        ao_serial.c
 
 #
@@ -84,6 +83,17 @@ TM_DRIVER_SRC = \
        ao_gps_report.c \
        ao_ignite.c
 
+#
+# Drivers only on TeleMetrum
+#
+TM_SIRF_DRIVER_SRC = \
+       ao_gps_sirf.c
+#
+# Drivers only on TeleMetrum
+#
+TM_SKY_DRIVER_SRC = \
+       ao_gps_skytraq.c
+
 #
 # Tasks run on TeleMetrum
 #
@@ -108,6 +118,14 @@ TM_SRC = \
        $(TM_TASK_SRC) \
        $(TM_MAIN_SRC)
 
+TM_SIRF_SRC = \
+       $(TM_SRC) \
+       $(TM_SIRF_DRIVER_SRC)
+
+TM_SKY_SRC = \
+       $(TM_SRC) \
+       $(TM_SKY_DRIVER_SRC)
+
 TI_MAIN_SRC = \
        ao_tidongle.c
 
@@ -161,13 +179,16 @@ SRC = \
        $(TELE_COMMON_SRC) \
        $(TELE_FAKE_SRC) \
        $(TM_DRIVER_SRC) \
+       $(TM_SIRF_DRIVER_SRC) \
+       $(TM_SKY_DRIVER_SRC) \
        $(TM_TASK_SRC) \
        $(TM_MAIN_SRC) \
        $(TI_MAIN_SRC) \
        $(TD_MAIN_SRC) \
        $(TT_MAIN_SRC)
 
-TM_REL=$(TM_SRC:.c=.rel) ao_product-telemetrum.rel
+TM_SIRF_REL=$(TM_SIRF_SRC:.c=.rel) ao_product-telemetrum.rel
+TM_SKY_REL=$(TM_SKY_SRC:.c=.rel) ao_product-telemetrum.rel
 TI_REL=$(TI_SRC:.c=.rel) ao_product-tidongle.rel
 TT_REL=$(TT_SRC:.c=.rel) ao_product-teleterra.rel
 TD_REL=$(TD_SRC:.c=.rel) ao_product-teledongle.rel
@@ -186,10 +207,10 @@ LST=$(REL:.rel=.lst)
 RST=$(REL:.rel=.rst)
 SYM=$(REL:.rel=.sym)
 
-PROGS= telemetrum.ihx tidongle.ihx \
+PROGS= telemetrum-sirf.ihx telemetrum-sky.ihx tidongle.ihx \
        teleterra.ihx teledongle.ihx
 
-HOST_PROGS=ao_flight_test ao_gps_test
+HOST_PROGS=ao_flight_test ao_gps_test ao_gps_test_skytraq
 
 PCDB=$(PROGS:.ihx=.cdb)
 PLNK=$(PROGS:.ihx=.lnk)
@@ -202,15 +223,21 @@ PAOM=$(PROGS:.ihx=)
 
 all: $(PROGS) $(HOST_PROGS)
 
-telemetrum.ihx: $(TM_REL) Makefile
-       $(CC) $(LDFLAGS) $(CFLAGS) -o $@ $(TM_REL)
-       sh check-stack ao.h telemetrum.mem
+telemetrum-sirf.ihx: $(TM_SIRF_REL) Makefile
+       $(CC) $(LDFLAGS) $(CFLAGS) -o $@ $(TM_SIRF_REL)
+       sh check-stack ao.h telemetrum-sirf.mem
+
+telemetrum-sky.ihx: $(TM_SKY_REL) Makefile
+       $(CC) $(LDFLAGS) $(CFLAGS) -o $@ $(TM_SKY_REL)
+       sh check-stack ao.h telemetrum-sky.mem
+
+telemetrum-sky.ihx: telemetrum-sirf.ihx
 
 tidongle.ihx: $(TI_REL) Makefile
        $(CC) $(LDFLAGS) $(CFLAGS) -o $@ $(TI_REL)
        sh check-stack ao.h tidongle.mem
 
-tidongle.ihx: telemetrum.ihx
+tidongle.ihx: telemetrum-sky.ihx
 
 teleterra.ihx: $(TT_REL) Makefile
        $(CC) $(LDFLAGS) $(CFLAGS) -o $@ $(TT_REL)
@@ -271,5 +298,8 @@ install:
 ao_flight_test: ao_flight.c ao_flight_test.c ao_host.h
        cc -g -o $@ ao_flight_test.c
 
-ao_gps_test: ao_gps.c ao_gps_test.c ao_gps_print.c ao_host.h
+ao_gps_test: ao_gps_sirf.c ao_gps_test.c ao_gps_print.c ao_host.h
        cc -g -o $@ ao_gps_test.c
+
+ao_gps_test_skytraq: ao_gps_skytraq.c ao_gps_test_skytraq.c ao_gps_print.c ao_host.h
+       cc -g -o $@ ao_gps_test_skytraq.c
index 4116be65ae13f22aef242957363d4b2ffa42147b..0f3f0ea7394e57a66061cac9adade96a55deb0d0 100644 (file)
--- a/src/ao.h
+++ b/src/ao.h
@@ -668,7 +668,8 @@ void
 ao_serial_putchar(char c) __critical;
 
 #define AO_SERIAL_SPEED_4800   0
-#define AO_SERIAL_SPEED_57600  1
+#define AO_SERIAL_SPEED_9600   1
+#define AO_SERIAL_SPEED_57600  2
 
 void
 ao_serial_set_speed(uint8_t speed);
diff --git a/src/ao_gps.c b/src/ao_gps.c
deleted file mode 100644 (file)
index 2b3a517..0000000
+++ /dev/null
@@ -1,457 +0,0 @@
-/*
- * Copyright © 2009 Keith Packard <keithp@keithp.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; version 2 of the License.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
- */
-
-#ifndef AO_GPS_TEST
-#include "ao.h"
-#endif
-
-__xdata uint8_t ao_gps_mutex;
-__xdata struct ao_gps_data     ao_gps_data;
-__xdata struct ao_gps_tracking_data    ao_gps_tracking_data;
-
-static const char ao_gps_set_nmea[] = "\r\n$PSRF100,0,57600,8,1,0*37\r\n";
-
-const char ao_gps_config[] = {
-
-       0xa0, 0xa2, 0x00, 0x0e, /* length: 14 bytes */
-       136,                    /* mode control */
-       0, 0,                   /* reserved */
-       0,                      /* degraded mode (allow 1-SV navigation) */
-       0, 0,                   /* reserved */
-       0, 0,                   /* user specified altitude */
-       2,                      /* alt hold mode (disabled, require 3d fixes) */
-       0,                      /* alt hold source (use last computed altitude) */
-       0,                      /* reserved */
-       10,                     /* Degraded time out (10 sec) */
-       10,                     /* Dead Reckoning time out (10 sec) */
-       0,                      /* Track smoothing (disabled) */
-       0x00, 0x8e, 0xb0, 0xb3,
-
-       0xa0, 0xa2, 0x00, 0x08, /* length: 8 bytes */
-       166,                    /* Set message rate */
-       2,                      /* enable/disable all messages */
-       0,                      /* message id (ignored) */
-       0,                      /* update rate (0 = disable) */
-       0, 0, 0, 0,             /* reserved */
-       0x00, 0xa8, 0xb0, 0xb3,
-
-       0xa0, 0xa2, 0x00, 0x02, /* length: 2 bytes */
-       143,                    /* static navigation */
-       0,                      /* disable */
-       0x00, 0x8f, 0xb0, 0xb3,
-};
-
-#define NAV_TYPE_GPS_FIX_TYPE_MASK                     (7 << 0)
-#define NAV_TYPE_NO_FIX                                        (0 << 0)
-#define NAV_TYPE_SV_KF                                 (1 << 0)
-#define NAV_TYPE_2_SV_KF                               (2 << 0)
-#define NAV_TYPE_3_SV_KF                               (3 << 0)
-#define NAV_TYPE_4_SV_KF                               (4 << 0)
-#define NAV_TYPE_2D_LEAST_SQUARES                      (5 << 0)
-#define NAV_TYPE_3D_LEAST_SQUARES                      (6 << 0)
-#define NAV_TYPE_DR                                    (7 << 0)
-#define NAV_TYPE_TRICKLE_POWER                         (1 << 3)
-#define NAV_TYPE_ALTITUDE_HOLD_MASK                    (3 << 4)
-#define NAV_TYPE_ALTITUDE_HOLD_NONE                    (0 << 4)
-#define NAV_TYPE_ALTITUDE_HOLD_KF                      (1 << 4)
-#define NAV_TYPE_ALTITUDE_HOLD_USER                    (2 << 4)
-#define NAV_TYPE_ALTITUDE_HOLD_ALWAYS                  (3 << 4)
-#define NAV_TYPE_DOP_LIMIT_EXCEEDED                    (1 << 6)
-#define NAV_TYPE_DGPS_APPLIED                          (1 << 7)
-#define NAV_TYPE_SENSOR_DR                             (1 << 8)
-#define NAV_TYPE_OVERDETERMINED                                (1 << 9)
-#define NAV_TYPE_DR_TIMEOUT_EXCEEDED                   (1 << 10)
-#define NAV_TYPE_FIX_MI_EDIT                           (1 << 11)
-#define NAV_TYPE_INVALID_VELOCITY                      (1 << 12)
-#define NAV_TYPE_ALTITUDE_HOLD_DISABLED                        (1 << 13)
-#define NAV_TYPE_DR_ERROR_STATUS_MASK                  (3 << 14)
-#define NAV_TYPE_DR_ERROR_STATUS_GPS_ONLY              (0 << 14)
-#define NAV_TYPE_DR_ERROR_STATUS_DR_FROM_GPS           (1 << 14)
-#define NAV_TYPE_DR_ERROR_STATUS_DR_SENSOR_ERROR       (2 << 14)
-#define NAV_TYPE_DR_ERROR_STATUS_DR_IN_TEST            (3 << 14)
-
-struct sirf_geodetic_nav_data {
-       uint16_t        nav_type;
-       uint16_t        utc_year;
-       uint8_t         utc_month;
-       uint8_t         utc_day;
-       uint8_t         utc_hour;
-       uint8_t         utc_minute;
-       uint16_t        utc_second;
-       int32_t         lat;
-       int32_t         lon;
-       int32_t         alt_msl;
-       uint16_t        ground_speed;
-       uint16_t        course;
-       int16_t         climb_rate;
-       uint32_t        h_error;
-       uint32_t        v_error;
-       uint8_t         num_sv;
-       uint8_t         hdop;
-};
-
-static __xdata struct sirf_geodetic_nav_data   ao_sirf_data;
-
-struct sirf_measured_sat_data {
-       uint8_t         svid;
-       uint16_t        state;
-       uint8_t         c_n_1;
-};
-
-struct sirf_measured_tracker_data {
-       int16_t                         gps_week;
-       uint32_t                        gps_tow;
-       uint8_t                         channels;
-       struct sirf_measured_sat_data   sats[12];
-};
-
-static __xdata struct sirf_measured_tracker_data       ao_sirf_tracker_data;
-
-static __pdata uint16_t ao_sirf_cksum;
-static __pdata uint16_t ao_sirf_len;
-
-#define ao_sirf_byte() ((uint8_t) ao_serial_getchar())
-
-static uint8_t data_byte(void)
-{
-       uint8_t c = ao_sirf_byte();
-       --ao_sirf_len;
-       ao_sirf_cksum += c;
-       return c;
-}
-
-static char __xdata *sirf_target;
-
-static void sirf_u16(uint8_t offset)
-{
-       uint16_t __xdata *ptr = (uint16_t __xdata *) (sirf_target + offset);
-       uint16_t val;
-
-       val = data_byte() << 8;
-       val |= data_byte ();
-       *ptr = val;
-}
-
-static void sirf_u8(uint8_t offset)
-{
-       uint8_t __xdata *ptr = (uint8_t __xdata *) (sirf_target + offset);
-       uint8_t val;
-
-       val = data_byte ();
-       *ptr = val;
-}
-
-static void sirf_u32(uint8_t offset) __reentrant
-{
-       uint32_t __xdata *ptr = (uint32_t __xdata *) (sirf_target + offset);
-       uint32_t val;
-
-       val = ((uint32_t) data_byte ()) << 24;
-       val |= ((uint32_t) data_byte ()) << 16;
-       val |= ((uint32_t) data_byte ()) << 8;
-       val |= ((uint32_t) data_byte ());
-       *ptr = val;
-}
-
-static void sirf_discard(uint8_t len)
-{
-       while (len--)
-               data_byte();
-}
-
-#define SIRF_END       0
-#define SIRF_DISCARD   1
-#define SIRF_U8                2
-#define SIRF_U16       3
-#define SIRF_U32       4
-#define SIRF_U8X10     5
-
-struct sirf_packet_parse {
-       uint8_t type;
-       uint8_t offset;
-};
-
-static void
-ao_sirf_parse(void __xdata *target, const struct sirf_packet_parse *parse) __reentrant
-{
-       uint8_t i, offset, j;
-
-       sirf_target = target;
-       for (i = 0; ; i++) {
-               offset = parse[i].offset;
-               switch (parse[i].type) {
-               case SIRF_END:
-                       return;
-               case SIRF_DISCARD:
-                       sirf_discard(offset);
-                       break;
-               case SIRF_U8:
-                       sirf_u8(offset);
-                       break;
-               case SIRF_U16:
-                       sirf_u16(offset);
-                       break;
-               case SIRF_U32:
-                       sirf_u32(offset);
-                       break;
-               case SIRF_U8X10:
-                       for (j = 10; j--;)
-                               sirf_u8(offset++);
-                       break;
-               }
-       }
-}
-
-static const struct sirf_packet_parse geodetic_nav_data_packet[] = {
-       { SIRF_DISCARD, 2 },                                                    /* 1 nav valid */
-       { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, nav_type) },        /* 3 */
-       { SIRF_DISCARD, 6 },                                                    /* 5 week number, time of week */
-       { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_year) },        /* 11 */
-       { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_month) },        /* 13 */
-       { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_day) },          /* 14 */
-       { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_hour) },         /* 15 */
-       { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_minute) },       /* 16 */
-       { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_second) },      /* 17 */
-       { SIRF_DISCARD, 4 },    /* satellite id list */                         /* 19 */
-       { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lat) },             /* 23 */
-       { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lon) },             /* 27 */
-       { SIRF_DISCARD, 4 },    /* altitude from ellipsoid */                   /* 31 */
-       { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, alt_msl) },         /* 35 */
-       { SIRF_DISCARD, 1 },    /* map datum */                                 /* 39 */
-       { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, ground_speed) },    /* 40 */
-       { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, course) },          /* 42 */
-       { SIRF_DISCARD, 2 },    /* magnetic variation */                        /* 44 */
-       { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, climb_rate) },      /* 46 */
-       { SIRF_DISCARD, 2 },    /* turn rate */                                 /* 48 */
-       { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, h_error) },         /* 50 */
-       { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, v_error) },         /* 54 */
-       { SIRF_DISCARD, 30 },   /* time error, h_vel error, clock_bias,
-                                  clock bias error, clock drift,
-                                  clock drift error, distance,
-                                  distance error, heading error */             /* 58 */
-       { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, num_sv) },           /* 88 */
-       { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, hdop) },             /* 89 */
-       { SIRF_DISCARD, 1 },    /* additional mode info */                      /* 90 */
-       { SIRF_END, 0 },                                                        /* 91 */
-};
-
-static void
-ao_sirf_parse_41(void) __reentrant
-{
-       ao_sirf_parse(&ao_sirf_data, geodetic_nav_data_packet);
-}
-
-static const struct sirf_packet_parse measured_tracker_data_packet[] = {
-       { SIRF_U16, offsetof (struct sirf_measured_tracker_data, gps_week) },   /* 1 week */
-       { SIRF_U32, offsetof (struct sirf_measured_tracker_data, gps_tow) },    /* 3 time of week */
-       { SIRF_U8, offsetof (struct sirf_measured_tracker_data, channels) },    /* 7 channels */
-       { SIRF_END, 0 },
-};
-
-static const struct sirf_packet_parse measured_sat_data_packet[] = {
-       { SIRF_U8, offsetof (struct sirf_measured_sat_data, svid) },            /* 0 SV id */
-       { SIRF_DISCARD, 2 },                                                    /* 1 azimuth, 2 elevation */
-       { SIRF_U16, offsetof (struct sirf_measured_sat_data, state) },          /* 2 state */
-       { SIRF_U8, offsetof (struct sirf_measured_sat_data, c_n_1) },           /* C/N0 1 */
-       { SIRF_DISCARD, 9 },                                                    /* C/N0 2-10 */
-       { SIRF_END, 0 },
-};
-
-static void
-ao_sirf_parse_4(void) __reentrant
-{
-       uint8_t i;
-       ao_sirf_parse(&ao_sirf_tracker_data, measured_tracker_data_packet);
-       for (i = 0; i < 12; i++)
-               ao_sirf_parse(&ao_sirf_tracker_data.sats[i], measured_sat_data_packet);
-}
-
-static void
-ao_gps_setup(void) __reentrant
-{
-       uint8_t i, k;
-       ao_serial_set_speed(AO_SERIAL_SPEED_4800);
-       for (i = 0; i < 64; i++)
-               ao_serial_putchar(0x00);
-       for (k = 0; k < 3; k++)
-               for (i = 0; i < sizeof (ao_gps_set_nmea); i++)
-                       ao_serial_putchar(ao_gps_set_nmea[i]);
-       ao_serial_set_speed(AO_SERIAL_SPEED_57600);
-       for (i = 0; i < 64; i++)
-               ao_serial_putchar(0x00);
-}
-
-static const char ao_gps_set_message_rate[] = {
-       0xa0, 0xa2, 0x00, 0x08,
-       166,
-       0,
-};
-
-void
-ao_sirf_set_message_rate(uint8_t msg, uint8_t rate)
-{
-       uint16_t        cksum = 0x00a6;
-       uint8_t         i;
-
-       for (i = 0; i < sizeof (ao_gps_set_message_rate); i++)
-               ao_serial_putchar(ao_gps_set_message_rate[i]);
-       ao_serial_putchar(msg);
-       ao_serial_putchar(rate);
-       cksum = 0xa6 + msg + rate;
-       for (i = 0; i < 4; i++)
-               ao_serial_putchar(0);
-       ao_serial_putchar((cksum >> 8) & 0x7f);
-       ao_serial_putchar(cksum & 0xff);
-       ao_serial_putchar(0xb0);
-       ao_serial_putchar(0xb3);
-}
-
-static const uint8_t sirf_disable[] = {
-       2,
-       9,
-       10,
-       27,
-       50,
-       52,
-};
-
-void
-ao_gps(void) __reentrant
-{
-       uint8_t i, k;
-       uint16_t cksum;
-
-       ao_gps_setup();
-       for (k = 0; k < 5; k++)
-       {
-               for (i = 0; i < sizeof (ao_gps_config); i++)
-                       ao_serial_putchar(ao_gps_config[i]);
-               for (i = 0; i < sizeof (sirf_disable); i++)
-                       ao_sirf_set_message_rate(sirf_disable[i], 0);
-               ao_sirf_set_message_rate(41, 1);
-               ao_sirf_set_message_rate(4, 1);
-       }
-       for (;;) {
-               /* Locate the begining of the next record */
-               while (ao_sirf_byte() != (uint8_t) 0xa0)
-                       ;
-               if (ao_sirf_byte() != (uint8_t) 0xa2)
-                       continue;
-
-               /* Length */
-               ao_sirf_len = ao_sirf_byte() << 8;
-               ao_sirf_len |= ao_sirf_byte();
-               if (ao_sirf_len > 1023)
-                       continue;
-
-               ao_sirf_cksum = 0;
-
-               /* message ID */
-               i = data_byte ();                                                       /* 0 */
-
-               switch (i) {
-               case 41:
-                       if (ao_sirf_len < 90)
-                               break;
-                       ao_sirf_parse_41();
-                       break;
-               case 4:
-                       if (ao_sirf_len < 187)
-                               break;
-                       ao_sirf_parse_4();
-                       break;
-               }
-               if (ao_sirf_len != 0)
-                       continue;
-
-               /* verify checksum and end sequence */
-               ao_sirf_cksum &= 0x7fff;
-               cksum = ao_sirf_byte() << 8;
-               cksum |= ao_sirf_byte();
-               if (ao_sirf_cksum != cksum)
-                       continue;
-               if (ao_sirf_byte() != (uint8_t) 0xb0)
-                       continue;
-               if (ao_sirf_byte() != (uint8_t) 0xb3)
-                       continue;
-
-               switch (i) {
-               case 41:
-                       ao_mutex_get(&ao_gps_mutex);
-                       ao_gps_data.hour = ao_sirf_data.utc_hour;
-                       ao_gps_data.minute = ao_sirf_data.utc_minute;
-                       ao_gps_data.second = ao_sirf_data.utc_second / 1000;
-                       ao_gps_data.flags = ((ao_sirf_data.num_sv << AO_GPS_NUM_SAT_SHIFT) & AO_GPS_NUM_SAT_MASK) | AO_GPS_RUNNING;
-                       if ((ao_sirf_data.nav_type & NAV_TYPE_GPS_FIX_TYPE_MASK) >= NAV_TYPE_4_SV_KF)
-                               ao_gps_data.flags |= AO_GPS_VALID;
-                       ao_gps_data.latitude = ao_sirf_data.lat;
-                       ao_gps_data.longitude = ao_sirf_data.lon;
-                       ao_gps_data.altitude = ao_sirf_data.alt_msl / 100;
-                       ao_gps_data.ground_speed = ao_sirf_data.ground_speed;
-                       ao_gps_data.course = ao_sirf_data.course / 200;
-                       ao_gps_data.hdop = ao_sirf_data.hdop;
-                       ao_gps_data.climb_rate = ao_sirf_data.climb_rate;
-                       if (ao_sirf_data.h_error > 6553500)
-                               ao_gps_data.h_error = 65535;
-                       else
-                               ao_gps_data.h_error = ao_sirf_data.h_error / 100;
-                       if (ao_sirf_data.v_error > 6553500)
-                               ao_gps_data.v_error = 65535;
-                       else
-                               ao_gps_data.v_error = ao_sirf_data.v_error / 100;
-                       ao_mutex_put(&ao_gps_mutex);
-                       ao_wakeup(&ao_gps_data);
-                       break;
-               case 4:
-                       ao_mutex_get(&ao_gps_mutex);
-                       ao_gps_tracking_data.channels = ao_sirf_tracker_data.channels;
-                       for (i = 0; i < 12; i++) {
-                               ao_gps_tracking_data.sats[i].svid = ao_sirf_tracker_data.sats[i].svid;
-                               ao_gps_tracking_data.sats[i].state = (uint8_t) ao_sirf_tracker_data.sats[i].state;
-                               ao_gps_tracking_data.sats[i].c_n_1 = ao_sirf_tracker_data.sats[i].c_n_1;
-                       }
-                       ao_mutex_put(&ao_gps_mutex);
-                       ao_wakeup(&ao_gps_tracking_data);
-                       break;
-               }
-       }
-}
-
-__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);
-       putchar('\n');
-       ao_gps_tracking_print(&ao_gps_tracking_data);
-       putchar('\n');
-       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)
-{
-       ao_add_task(&ao_gps_task, ao_gps, "gps");
-       ao_cmd_register(&ao_gps_cmds[0]);
-}
diff --git a/src/ao_gps_sirf.c b/src/ao_gps_sirf.c
new file mode 100644 (file)
index 0000000..2b3a517
--- /dev/null
@@ -0,0 +1,457 @@
+/*
+ * Copyright © 2009 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef AO_GPS_TEST
+#include "ao.h"
+#endif
+
+__xdata uint8_t ao_gps_mutex;
+__xdata struct ao_gps_data     ao_gps_data;
+__xdata struct ao_gps_tracking_data    ao_gps_tracking_data;
+
+static const char ao_gps_set_nmea[] = "\r\n$PSRF100,0,57600,8,1,0*37\r\n";
+
+const char ao_gps_config[] = {
+
+       0xa0, 0xa2, 0x00, 0x0e, /* length: 14 bytes */
+       136,                    /* mode control */
+       0, 0,                   /* reserved */
+       0,                      /* degraded mode (allow 1-SV navigation) */
+       0, 0,                   /* reserved */
+       0, 0,                   /* user specified altitude */
+       2,                      /* alt hold mode (disabled, require 3d fixes) */
+       0,                      /* alt hold source (use last computed altitude) */
+       0,                      /* reserved */
+       10,                     /* Degraded time out (10 sec) */
+       10,                     /* Dead Reckoning time out (10 sec) */
+       0,                      /* Track smoothing (disabled) */
+       0x00, 0x8e, 0xb0, 0xb3,
+
+       0xa0, 0xa2, 0x00, 0x08, /* length: 8 bytes */
+       166,                    /* Set message rate */
+       2,                      /* enable/disable all messages */
+       0,                      /* message id (ignored) */
+       0,                      /* update rate (0 = disable) */
+       0, 0, 0, 0,             /* reserved */
+       0x00, 0xa8, 0xb0, 0xb3,
+
+       0xa0, 0xa2, 0x00, 0x02, /* length: 2 bytes */
+       143,                    /* static navigation */
+       0,                      /* disable */
+       0x00, 0x8f, 0xb0, 0xb3,
+};
+
+#define NAV_TYPE_GPS_FIX_TYPE_MASK                     (7 << 0)
+#define NAV_TYPE_NO_FIX                                        (0 << 0)
+#define NAV_TYPE_SV_KF                                 (1 << 0)
+#define NAV_TYPE_2_SV_KF                               (2 << 0)
+#define NAV_TYPE_3_SV_KF                               (3 << 0)
+#define NAV_TYPE_4_SV_KF                               (4 << 0)
+#define NAV_TYPE_2D_LEAST_SQUARES                      (5 << 0)
+#define NAV_TYPE_3D_LEAST_SQUARES                      (6 << 0)
+#define NAV_TYPE_DR                                    (7 << 0)
+#define NAV_TYPE_TRICKLE_POWER                         (1 << 3)
+#define NAV_TYPE_ALTITUDE_HOLD_MASK                    (3 << 4)
+#define NAV_TYPE_ALTITUDE_HOLD_NONE                    (0 << 4)
+#define NAV_TYPE_ALTITUDE_HOLD_KF                      (1 << 4)
+#define NAV_TYPE_ALTITUDE_HOLD_USER                    (2 << 4)
+#define NAV_TYPE_ALTITUDE_HOLD_ALWAYS                  (3 << 4)
+#define NAV_TYPE_DOP_LIMIT_EXCEEDED                    (1 << 6)
+#define NAV_TYPE_DGPS_APPLIED                          (1 << 7)
+#define NAV_TYPE_SENSOR_DR                             (1 << 8)
+#define NAV_TYPE_OVERDETERMINED                                (1 << 9)
+#define NAV_TYPE_DR_TIMEOUT_EXCEEDED                   (1 << 10)
+#define NAV_TYPE_FIX_MI_EDIT                           (1 << 11)
+#define NAV_TYPE_INVALID_VELOCITY                      (1 << 12)
+#define NAV_TYPE_ALTITUDE_HOLD_DISABLED                        (1 << 13)
+#define NAV_TYPE_DR_ERROR_STATUS_MASK                  (3 << 14)
+#define NAV_TYPE_DR_ERROR_STATUS_GPS_ONLY              (0 << 14)
+#define NAV_TYPE_DR_ERROR_STATUS_DR_FROM_GPS           (1 << 14)
+#define NAV_TYPE_DR_ERROR_STATUS_DR_SENSOR_ERROR       (2 << 14)
+#define NAV_TYPE_DR_ERROR_STATUS_DR_IN_TEST            (3 << 14)
+
+struct sirf_geodetic_nav_data {
+       uint16_t        nav_type;
+       uint16_t        utc_year;
+       uint8_t         utc_month;
+       uint8_t         utc_day;
+       uint8_t         utc_hour;
+       uint8_t         utc_minute;
+       uint16_t        utc_second;
+       int32_t         lat;
+       int32_t         lon;
+       int32_t         alt_msl;
+       uint16_t        ground_speed;
+       uint16_t        course;
+       int16_t         climb_rate;
+       uint32_t        h_error;
+       uint32_t        v_error;
+       uint8_t         num_sv;
+       uint8_t         hdop;
+};
+
+static __xdata struct sirf_geodetic_nav_data   ao_sirf_data;
+
+struct sirf_measured_sat_data {
+       uint8_t         svid;
+       uint16_t        state;
+       uint8_t         c_n_1;
+};
+
+struct sirf_measured_tracker_data {
+       int16_t                         gps_week;
+       uint32_t                        gps_tow;
+       uint8_t                         channels;
+       struct sirf_measured_sat_data   sats[12];
+};
+
+static __xdata struct sirf_measured_tracker_data       ao_sirf_tracker_data;
+
+static __pdata uint16_t ao_sirf_cksum;
+static __pdata uint16_t ao_sirf_len;
+
+#define ao_sirf_byte() ((uint8_t) ao_serial_getchar())
+
+static uint8_t data_byte(void)
+{
+       uint8_t c = ao_sirf_byte();
+       --ao_sirf_len;
+       ao_sirf_cksum += c;
+       return c;
+}
+
+static char __xdata *sirf_target;
+
+static void sirf_u16(uint8_t offset)
+{
+       uint16_t __xdata *ptr = (uint16_t __xdata *) (sirf_target + offset);
+       uint16_t val;
+
+       val = data_byte() << 8;
+       val |= data_byte ();
+       *ptr = val;
+}
+
+static void sirf_u8(uint8_t offset)
+{
+       uint8_t __xdata *ptr = (uint8_t __xdata *) (sirf_target + offset);
+       uint8_t val;
+
+       val = data_byte ();
+       *ptr = val;
+}
+
+static void sirf_u32(uint8_t offset) __reentrant
+{
+       uint32_t __xdata *ptr = (uint32_t __xdata *) (sirf_target + offset);
+       uint32_t val;
+
+       val = ((uint32_t) data_byte ()) << 24;
+       val |= ((uint32_t) data_byte ()) << 16;
+       val |= ((uint32_t) data_byte ()) << 8;
+       val |= ((uint32_t) data_byte ());
+       *ptr = val;
+}
+
+static void sirf_discard(uint8_t len)
+{
+       while (len--)
+               data_byte();
+}
+
+#define SIRF_END       0
+#define SIRF_DISCARD   1
+#define SIRF_U8                2
+#define SIRF_U16       3
+#define SIRF_U32       4
+#define SIRF_U8X10     5
+
+struct sirf_packet_parse {
+       uint8_t type;
+       uint8_t offset;
+};
+
+static void
+ao_sirf_parse(void __xdata *target, const struct sirf_packet_parse *parse) __reentrant
+{
+       uint8_t i, offset, j;
+
+       sirf_target = target;
+       for (i = 0; ; i++) {
+               offset = parse[i].offset;
+               switch (parse[i].type) {
+               case SIRF_END:
+                       return;
+               case SIRF_DISCARD:
+                       sirf_discard(offset);
+                       break;
+               case SIRF_U8:
+                       sirf_u8(offset);
+                       break;
+               case SIRF_U16:
+                       sirf_u16(offset);
+                       break;
+               case SIRF_U32:
+                       sirf_u32(offset);
+                       break;
+               case SIRF_U8X10:
+                       for (j = 10; j--;)
+                               sirf_u8(offset++);
+                       break;
+               }
+       }
+}
+
+static const struct sirf_packet_parse geodetic_nav_data_packet[] = {
+       { SIRF_DISCARD, 2 },                                                    /* 1 nav valid */
+       { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, nav_type) },        /* 3 */
+       { SIRF_DISCARD, 6 },                                                    /* 5 week number, time of week */
+       { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_year) },        /* 11 */
+       { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_month) },        /* 13 */
+       { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_day) },          /* 14 */
+       { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_hour) },         /* 15 */
+       { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_minute) },       /* 16 */
+       { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_second) },      /* 17 */
+       { SIRF_DISCARD, 4 },    /* satellite id list */                         /* 19 */
+       { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lat) },             /* 23 */
+       { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lon) },             /* 27 */
+       { SIRF_DISCARD, 4 },    /* altitude from ellipsoid */                   /* 31 */
+       { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, alt_msl) },         /* 35 */
+       { SIRF_DISCARD, 1 },    /* map datum */                                 /* 39 */
+       { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, ground_speed) },    /* 40 */
+       { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, course) },          /* 42 */
+       { SIRF_DISCARD, 2 },    /* magnetic variation */                        /* 44 */
+       { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, climb_rate) },      /* 46 */
+       { SIRF_DISCARD, 2 },    /* turn rate */                                 /* 48 */
+       { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, h_error) },         /* 50 */
+       { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, v_error) },         /* 54 */
+       { SIRF_DISCARD, 30 },   /* time error, h_vel error, clock_bias,
+                                  clock bias error, clock drift,
+                                  clock drift error, distance,
+                                  distance error, heading error */             /* 58 */
+       { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, num_sv) },           /* 88 */
+       { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, hdop) },             /* 89 */
+       { SIRF_DISCARD, 1 },    /* additional mode info */                      /* 90 */
+       { SIRF_END, 0 },                                                        /* 91 */
+};
+
+static void
+ao_sirf_parse_41(void) __reentrant
+{
+       ao_sirf_parse(&ao_sirf_data, geodetic_nav_data_packet);
+}
+
+static const struct sirf_packet_parse measured_tracker_data_packet[] = {
+       { SIRF_U16, offsetof (struct sirf_measured_tracker_data, gps_week) },   /* 1 week */
+       { SIRF_U32, offsetof (struct sirf_measured_tracker_data, gps_tow) },    /* 3 time of week */
+       { SIRF_U8, offsetof (struct sirf_measured_tracker_data, channels) },    /* 7 channels */
+       { SIRF_END, 0 },
+};
+
+static const struct sirf_packet_parse measured_sat_data_packet[] = {
+       { SIRF_U8, offsetof (struct sirf_measured_sat_data, svid) },            /* 0 SV id */
+       { SIRF_DISCARD, 2 },                                                    /* 1 azimuth, 2 elevation */
+       { SIRF_U16, offsetof (struct sirf_measured_sat_data, state) },          /* 2 state */
+       { SIRF_U8, offsetof (struct sirf_measured_sat_data, c_n_1) },           /* C/N0 1 */
+       { SIRF_DISCARD, 9 },                                                    /* C/N0 2-10 */
+       { SIRF_END, 0 },
+};
+
+static void
+ao_sirf_parse_4(void) __reentrant
+{
+       uint8_t i;
+       ao_sirf_parse(&ao_sirf_tracker_data, measured_tracker_data_packet);
+       for (i = 0; i < 12; i++)
+               ao_sirf_parse(&ao_sirf_tracker_data.sats[i], measured_sat_data_packet);
+}
+
+static void
+ao_gps_setup(void) __reentrant
+{
+       uint8_t i, k;
+       ao_serial_set_speed(AO_SERIAL_SPEED_4800);
+       for (i = 0; i < 64; i++)
+               ao_serial_putchar(0x00);
+       for (k = 0; k < 3; k++)
+               for (i = 0; i < sizeof (ao_gps_set_nmea); i++)
+                       ao_serial_putchar(ao_gps_set_nmea[i]);
+       ao_serial_set_speed(AO_SERIAL_SPEED_57600);
+       for (i = 0; i < 64; i++)
+               ao_serial_putchar(0x00);
+}
+
+static const char ao_gps_set_message_rate[] = {
+       0xa0, 0xa2, 0x00, 0x08,
+       166,
+       0,
+};
+
+void
+ao_sirf_set_message_rate(uint8_t msg, uint8_t rate)
+{
+       uint16_t        cksum = 0x00a6;
+       uint8_t         i;
+
+       for (i = 0; i < sizeof (ao_gps_set_message_rate); i++)
+               ao_serial_putchar(ao_gps_set_message_rate[i]);
+       ao_serial_putchar(msg);
+       ao_serial_putchar(rate);
+       cksum = 0xa6 + msg + rate;
+       for (i = 0; i < 4; i++)
+               ao_serial_putchar(0);
+       ao_serial_putchar((cksum >> 8) & 0x7f);
+       ao_serial_putchar(cksum & 0xff);
+       ao_serial_putchar(0xb0);
+       ao_serial_putchar(0xb3);
+}
+
+static const uint8_t sirf_disable[] = {
+       2,
+       9,
+       10,
+       27,
+       50,
+       52,
+};
+
+void
+ao_gps(void) __reentrant
+{
+       uint8_t i, k;
+       uint16_t cksum;
+
+       ao_gps_setup();
+       for (k = 0; k < 5; k++)
+       {
+               for (i = 0; i < sizeof (ao_gps_config); i++)
+                       ao_serial_putchar(ao_gps_config[i]);
+               for (i = 0; i < sizeof (sirf_disable); i++)
+                       ao_sirf_set_message_rate(sirf_disable[i], 0);
+               ao_sirf_set_message_rate(41, 1);
+               ao_sirf_set_message_rate(4, 1);
+       }
+       for (;;) {
+               /* Locate the begining of the next record */
+               while (ao_sirf_byte() != (uint8_t) 0xa0)
+                       ;
+               if (ao_sirf_byte() != (uint8_t) 0xa2)
+                       continue;
+
+               /* Length */
+               ao_sirf_len = ao_sirf_byte() << 8;
+               ao_sirf_len |= ao_sirf_byte();
+               if (ao_sirf_len > 1023)
+                       continue;
+
+               ao_sirf_cksum = 0;
+
+               /* message ID */
+               i = data_byte ();                                                       /* 0 */
+
+               switch (i) {
+               case 41:
+                       if (ao_sirf_len < 90)
+                               break;
+                       ao_sirf_parse_41();
+                       break;
+               case 4:
+                       if (ao_sirf_len < 187)
+                               break;
+                       ao_sirf_parse_4();
+                       break;
+               }
+               if (ao_sirf_len != 0)
+                       continue;
+
+               /* verify checksum and end sequence */
+               ao_sirf_cksum &= 0x7fff;
+               cksum = ao_sirf_byte() << 8;
+               cksum |= ao_sirf_byte();
+               if (ao_sirf_cksum != cksum)
+                       continue;
+               if (ao_sirf_byte() != (uint8_t) 0xb0)
+                       continue;
+               if (ao_sirf_byte() != (uint8_t) 0xb3)
+                       continue;
+
+               switch (i) {
+               case 41:
+                       ao_mutex_get(&ao_gps_mutex);
+                       ao_gps_data.hour = ao_sirf_data.utc_hour;
+                       ao_gps_data.minute = ao_sirf_data.utc_minute;
+                       ao_gps_data.second = ao_sirf_data.utc_second / 1000;
+                       ao_gps_data.flags = ((ao_sirf_data.num_sv << AO_GPS_NUM_SAT_SHIFT) & AO_GPS_NUM_SAT_MASK) | AO_GPS_RUNNING;
+                       if ((ao_sirf_data.nav_type & NAV_TYPE_GPS_FIX_TYPE_MASK) >= NAV_TYPE_4_SV_KF)
+                               ao_gps_data.flags |= AO_GPS_VALID;
+                       ao_gps_data.latitude = ao_sirf_data.lat;
+                       ao_gps_data.longitude = ao_sirf_data.lon;
+                       ao_gps_data.altitude = ao_sirf_data.alt_msl / 100;
+                       ao_gps_data.ground_speed = ao_sirf_data.ground_speed;
+                       ao_gps_data.course = ao_sirf_data.course / 200;
+                       ao_gps_data.hdop = ao_sirf_data.hdop;
+                       ao_gps_data.climb_rate = ao_sirf_data.climb_rate;
+                       if (ao_sirf_data.h_error > 6553500)
+                               ao_gps_data.h_error = 65535;
+                       else
+                               ao_gps_data.h_error = ao_sirf_data.h_error / 100;
+                       if (ao_sirf_data.v_error > 6553500)
+                               ao_gps_data.v_error = 65535;
+                       else
+                               ao_gps_data.v_error = ao_sirf_data.v_error / 100;
+                       ao_mutex_put(&ao_gps_mutex);
+                       ao_wakeup(&ao_gps_data);
+                       break;
+               case 4:
+                       ao_mutex_get(&ao_gps_mutex);
+                       ao_gps_tracking_data.channels = ao_sirf_tracker_data.channels;
+                       for (i = 0; i < 12; i++) {
+                               ao_gps_tracking_data.sats[i].svid = ao_sirf_tracker_data.sats[i].svid;
+                               ao_gps_tracking_data.sats[i].state = (uint8_t) ao_sirf_tracker_data.sats[i].state;
+                               ao_gps_tracking_data.sats[i].c_n_1 = ao_sirf_tracker_data.sats[i].c_n_1;
+                       }
+                       ao_mutex_put(&ao_gps_mutex);
+                       ao_wakeup(&ao_gps_tracking_data);
+                       break;
+               }
+       }
+}
+
+__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);
+       putchar('\n');
+       ao_gps_tracking_print(&ao_gps_tracking_data);
+       putchar('\n');
+       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)
+{
+       ao_add_task(&ao_gps_task, ao_gps, "gps");
+       ao_cmd_register(&ao_gps_cmds[0]);
+}
diff --git a/src/ao_gps_skytraq.c b/src/ao_gps_skytraq.c
new file mode 100644 (file)
index 0000000..b397d97
--- /dev/null
@@ -0,0 +1,369 @@
+/*
+ * Copyright © 2009 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#ifndef AO_GPS_TEST
+#include "ao.h"
+#endif
+
+#define AO_GPS_LEADER          3
+
+static const char ao_gps_header[] = "GPG";
+
+__xdata uint8_t ao_gps_mutex;
+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;
+__xdata struct ao_gps_tracking_data    ao_gps_tracking_data;
+
+static __xdata struct ao_gps_data              ao_gps_next;
+static __xdata struct ao_gps_tracking_data     ao_gps_tracking_next;
+
+static const char ao_gps_config[] = {
+       0xa0, 0xa1, 0x00, 0x09,         /* length 9 bytes */
+       0x08,                           /* configure nmea */
+       1,                              /* gga interval */
+       1,                              /* gsa interval */
+       1,                              /* gsv interval */
+       1,                              /* gll interval */
+       1,                              /* rmc interval */
+       1,                              /* vtg interval */
+       1,                              /* zda interval */
+       0,                              /* attributes (0 = update to sram, 1 = update flash too) */
+       0x09, 0x0d, 0x0a,
+};
+
+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)
+{
+       if (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 int32_t
+ao_gps_parse_pos(uint8_t deg_width) __reentrant
+{
+       int32_t d;
+       int32_t m;
+       int32_t f;
+
+       d = ao_gps_decimal(deg_width);
+       m = ao_gps_decimal(2);
+       if (ao_gps_char == '.') {
+               f = ao_gps_decimal(4);
+               while (ao_gps_num_width < 4) {
+                       f *= 10;
+                       ao_gps_num_width++;
+               }
+       } else {
+               f = 0;
+               if (ao_gps_char != ',')
+                       ao_gps_error = 1;
+       }
+       d = d * 10000000l;
+       m = m * 10000l + f;
+       d = d + m * 50 / 3;
+       return d;
+}
+
+static uint8_t
+ao_gps_parse_flag(char no_c, char yes_c) __reentrant
+{
+       uint8_t ret = 0;
+       ao_gps_skip_sep();
+       if (ao_gps_char == yes_c)
+               ret = 1;
+       else if (ao_gps_char == no_c)
+               ret = 0;
+       else
+               ao_gps_error = 1;
+       ao_gps_lexchar();
+       return ret;
+}
+
+
+void
+ao_gps(void) __reentrant
+{
+       char    c;
+       uint8_t i;
+
+       ao_serial_set_speed(AO_SERIAL_SPEED_9600);
+       for (i = 0; i < sizeof (ao_gps_config); i++)
+               ao_serial_putchar(ao_gps_config[i]);
+       for (;;) {
+               /* Locate the begining of the next record */
+               for (;;) {
+                       c = ao_serial_getchar();
+                       if (c == '$')
+                               break;
+               }
+
+               ao_gps_cksum = 0;
+               ao_gps_error = 0;
+
+               /* Skip anything other than GPG */
+               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;
+
+               /* pull the record identifier characters off the link */
+               ao_gps_lexchar();
+               c = ao_gps_char;
+               ao_gps_lexchar();
+               i = ao_gps_char;
+               ao_gps_lexchar();
+               if (ao_gps_char != ',')
+                       continue;
+
+               if (c == (uint8_t) 'G' && i == (uint8_t) 'A') {
+                       /* 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 = AO_GPS_RUNNING;
+                       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_next.latitude = ao_gps_parse_pos(2);
+                       if (ao_gps_parse_flag('N', 'S'))
+                               ao_gps_next.latitude = -ao_gps_next.latitude;
+                       ao_gps_next.longitude = ao_gps_parse_pos(3);
+                       if (ao_gps_parse_flag('E', 'W'))
+                               ao_gps_next.longitude = -ao_gps_next.longitude;
+
+                       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);
+                       }
+               } else if (c == (uint8_t) 'S' && i == (uint8_t) 'V') {
+                       uint8_t done;
+                       /* Now read the data into the GPS tracking data record
+                        *
+                        * $GPGSV,3,1,12,05,54,069,45,12,44,061,44,21,07,184,46,22,78,289,47*72<CR><LF>
+                        *
+                        * Satellites in view data
+                        *
+                        *      3               Total number of GSV messages
+                        *      1               Sequence number of current GSV message
+                        *      12              Total sats in view (0-12)
+                        *      05              SVID
+                        *      54              Elevation
+                        *      069             Azimuth
+                        *      45              C/N0 in dB
+                        *      ...             other SVIDs
+                        *      72              checksum
+                        */
+                       c = ao_gps_decimal(1);  /* total messages */
+                       i = ao_gps_decimal(1);  /* message sequence */
+                       if (i == 1) {
+                               ao_gps_tracking_next.channels = 0;
+                       }
+                       done = (uint8_t) c == i;
+                       ao_gps_lexchar();
+                       ao_gps_skip_field();    /* sats in view */
+                       while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
+                               i = ao_gps_tracking_next.channels;
+                               ao_gps_tracking_next.sats[i].svid = ao_gps_decimal(2);  /* SVID */
+                               ao_gps_lexchar();
+                               ao_gps_skip_field();    /* elevation */
+                               ao_gps_lexchar();
+                               ao_gps_skip_field();    /* azimuth */
+                               if (ao_gps_tracking_next.sats[i].c_n_1 = ao_gps_decimal(2))     /* C/N0 */
+                                       ao_gps_tracking_next.sats[i].state = 0xbf;
+                               else
+                                       ao_gps_tracking_next.sats[i].state = 0;
+                               ao_gps_tracking_next.channels = i + 1;
+                       }
+                       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_gps_tracking_next.channels = 0;
+                       else if (done) {
+                               ao_mutex_get(&ao_gps_mutex);
+                               memcpy(&ao_gps_tracking_data, &ao_gps_tracking_next,
+                                      sizeof(ao_gps_tracking_data));
+                               ao_mutex_put(&ao_gps_mutex);
+                               ao_wakeup(&ao_gps_tracking_data);
+                       }
+               }
+       }
+}
+
+__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);
+       putchar('\n');
+       ao_gps_tracking_print(&ao_gps_tracking_data);
+       putchar('\n');
+       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)
+{
+       ao_add_task(&ao_gps_task, ao_gps, "gps");
+       ao_cmd_register(&ao_gps_cmds[0]);
+}
index c94128d91adc9b0f1c5799dffab77f0917bdcb77..366bca718a76851a31c26434a124b794324f3d10 100644 (file)
@@ -398,7 +398,7 @@ ao_serial_set_speed(uint8_t speed)
 }
 
 #include "ao_gps_print.c"
-#include "ao_gps.c"
+#include "ao_gps_sirf.c"
 
 void
 ao_dump_state(void *wchan)
diff --git a/src/ao_gps_test_skytraq.c b/src/ao_gps_test_skytraq.c
new file mode 100644 (file)
index 0000000..510bc41
--- /dev/null
@@ -0,0 +1,478 @@
+/*
+ * Copyright © 2009 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#define AO_GPS_TEST
+#include "ao_host.h"
+#include <termios.h>
+#include <errno.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#define AO_GPS_NUM_SAT_MASK    (0xf << 0)
+#define AO_GPS_NUM_SAT_SHIFT   (0)
+
+#define AO_GPS_VALID           (1 << 4)
+#define AO_GPS_RUNNING         (1 << 5)
+
+struct ao_gps_data {
+       uint8_t                 hour;
+       uint8_t                 minute;
+       uint8_t                 second;
+       uint8_t                 flags;
+       int32_t                 latitude;       /* degrees * 10⁷ */
+       int32_t                 longitude;      /* degrees * 10⁷ */
+       int16_t                 altitude;       /* m */
+       uint16_t                ground_speed;   /* cm/s */
+       uint8_t                 course;         /* degrees / 2 */
+       uint8_t                 hdop;           /* * 5 */
+       int16_t                 climb_rate;     /* cm/s */
+       uint16_t                h_error;        /* m */
+       uint16_t                v_error;        /* m */
+};
+
+#define SIRF_SAT_STATE_ACQUIRED                        (1 << 0)
+#define SIRF_SAT_STATE_CARRIER_PHASE_VALID     (1 << 1)
+#define SIRF_SAT_BIT_SYNC_COMPLETE             (1 << 2)
+#define SIRF_SAT_SUBFRAME_SYNC_COMPLETE                (1 << 3)
+#define SIRF_SAT_CARRIER_PULLIN_COMPLETE       (1 << 4)
+#define SIRF_SAT_CODE_LOCKED                   (1 << 5)
+#define SIRF_SAT_ACQUISITION_FAILED            (1 << 6)
+#define SIRF_SAT_EPHEMERIS_AVAILABLE           (1 << 7)
+
+struct ao_gps_sat_data {
+       uint8_t         svid;
+       uint8_t         state;
+       uint8_t         c_n_1;
+};
+
+struct ao_gps_tracking_data {
+       uint8_t                 channels;
+       struct ao_gps_sat_data  sats[12];
+};
+
+void
+ao_mutex_get(uint8_t *mutex)
+{
+}
+
+void
+ao_mutex_put(uint8_t *mutex)
+{
+}
+
+static int
+ao_gps_fd;
+
+static void
+ao_dbg_char(char c)
+{
+       char    line[128];
+       line[0] = '\0';
+       if (c < ' ') {
+               if (c == '\n')
+                       sprintf (line, "\n");
+               else
+                       sprintf (line, "\\%02x", ((int) c) & 0xff);
+       } else {
+               sprintf (line, "%c", c);
+       }
+       write(1, line, strlen(line));
+}
+
+#define QUEUE_LEN      4096
+
+static char    input_queue[QUEUE_LEN];
+int            input_head, input_tail;
+
+#include <sys/time.h>
+
+int
+get_millis(void)
+{
+       struct timeval  tv;
+       gettimeofday(&tv, NULL);
+       return tv.tv_sec * 1000 + tv.tv_usec / 1000;
+}
+
+static void
+check_skytraq_message(char *from, uint8_t *msg, int len)
+{
+       uint16_t        encoded_len, encoded_cksum;
+       uint16_t        cksum;
+       uint8_t         id;
+       int             i;
+
+//     fwrite(msg, 1, len, stdout);
+       return;
+       if (msg[0] != 0xa0 || msg[1] != 0xa2) {
+               printf ("bad header\n");
+               return;
+       }
+       if (len < 7) {
+               printf("short\n");
+               return;
+       }
+       if (msg[len-1] != 0xb3 || msg[len-2] != 0xb0) {
+               printf ("bad trailer\n");
+               return;
+       }
+       encoded_len = (msg[2] << 8) | msg[3];
+       id = msg[4];
+/*     printf ("%9d: %3d\n", get_millis(), id); */
+       if (encoded_len != len - 8) {
+               if (id != 52)
+                       printf ("length mismatch (got %d, wanted %d)\n",
+                               len - 8, encoded_len);
+               return;
+       }
+       encoded_cksum = (msg[len - 4] << 8) | msg[len-3];
+       cksum = 0;
+       for (i = 4; i < len - 4; i++)
+               cksum = (cksum + msg[i]) & 0x7fff;
+       if (encoded_cksum != cksum) {
+               printf ("cksum mismatch (got %04x wanted %04x)\n",
+                       cksum, encoded_cksum);
+               return;
+       }
+       id = msg[4];
+       switch (id) {
+       case 41:{
+               int     off = 4;
+
+               uint8_t         id;
+               uint16_t        nav_valid;
+               uint16_t        nav_type;
+               uint16_t        week;
+               uint32_t        tow;
+               uint16_t        year;
+               uint8_t         month;
+               uint8_t         day;
+               uint8_t         hour;
+               uint8_t         minute;
+               uint16_t        second;
+               uint32_t        sat_list;
+               int32_t         lat;
+               int32_t         lon;
+               int32_t         alt_ell;
+               int32_t         alt_msl;
+               int8_t          datum;
+               uint16_t        sog;
+               uint16_t        cog;
+               int16_t         mag_var;
+               int16_t         climb_rate;
+               int16_t         heading_rate;
+               uint32_t        h_error;
+               uint32_t        v_error;
+               uint32_t        t_error;
+               uint16_t        h_v_error;
+
+#define get_u8(u)      u = (msg[off]); off+= 1
+#define get_u16(u)     u = (msg[off] << 8) | (msg[off + 1]); off+= 2
+#define get_u32(u)     u = (msg[off] << 24) | (msg[off + 1] << 16) | (msg[off+2] << 8) | (msg[off+3]); off+= 4
+
+               get_u8(id);
+               get_u16(nav_valid);
+               get_u16(nav_type);
+               get_u16(week);
+               get_u32(tow);
+               get_u16(year);
+               get_u8(month);
+               get_u8(day);
+               get_u8(hour);
+               get_u8(minute);
+               get_u16(second);
+               get_u32(sat_list);
+               get_u32(lat);
+               get_u32(lon);
+               get_u32(alt_ell);
+               get_u32(alt_msl);
+               get_u8(datum);
+               get_u16(sog);
+               get_u16(cog);
+               get_u16(mag_var);
+               get_u16(climb_rate);
+               get_u16(heading_rate);
+               get_u32(h_error);
+               get_u32(v_error);
+               get_u32(t_error);
+               get_u16(h_v_error);
+
+
+               printf ("Geodetic Navigation Data (41):\n");
+               printf ("\tNav valid %04x\n", nav_valid);
+               printf ("\tNav type %04x\n", nav_type);
+               printf ("\tWeek %5d", week);
+               printf (" TOW %9d", tow);
+               printf (" %4d-%2d-%2d %02d:%02d:%07.4f\n",
+                       year, month, day,
+                       hour, minute, second / 1000.0);
+               printf ("\tsats: %08x\n", sat_list);
+               printf ("\tlat: %g", lat / 1.0e7);
+               printf (" lon: %g", lon / 1.0e7);
+               printf (" alt_ell: %g", alt_ell / 100.0);
+               printf (" alt_msll: %g", alt_msl / 100.0);
+               printf (" datum: %d\n", datum);
+               printf ("\tground speed: %g", sog / 100.0);
+               printf (" course: %g", cog / 100.0);
+               printf (" climb: %g", climb_rate / 100.0);
+               printf (" heading rate: %g\n", heading_rate / 100.0);
+               printf ("\th error: %g", h_error / 100.0);
+               printf (" v error: %g", v_error / 100.0);
+               printf (" t error: %g", t_error / 100.0);
+               printf (" h vel error: %g\n", h_v_error / 100.0);
+               break;
+       }
+       case 4: {
+               int off = 4;
+               uint8_t         id;
+               int16_t         gps_week;
+               uint32_t        gps_tow;
+               uint8_t         channels;
+               int             j, k;
+
+               get_u8(id);
+               get_u16(gps_week);
+               get_u32(gps_tow);
+               get_u8(channels);
+
+               printf ("Measured Tracker Data (4):\n");
+               printf ("GPS week: %d\n", gps_week);
+               printf ("GPS time of week: %d\n", gps_tow);
+               printf ("channels: %d\n", channels);
+               for (j = 0; j < 12; j++) {
+                       uint8_t svid, azimuth, elevation;
+                       uint16_t state;
+                       uint8_t c_n[10];
+                       get_u8(svid);
+                       get_u8(azimuth);
+                       get_u8(elevation);
+                       get_u16(state);
+                       for (k = 0; k < 10; k++) {
+                               get_u8(c_n[k]);
+                       }
+                       printf ("Sat %3d:", svid);
+                       printf (" aziumuth: %6.1f", azimuth * 1.5);
+                       printf (" elevation: %6.1f", elevation * 0.5);
+                       printf (" state: 0x%02x", state);
+                       printf (" c_n:");
+                       for (k = 0; k < 10; k++)
+                               printf(" %3d", c_n[k]);
+                       if (state & SIRF_SAT_STATE_ACQUIRED)
+                               printf(" acq,");
+                       if (state & SIRF_SAT_STATE_CARRIER_PHASE_VALID)
+                               printf(" car,");
+                       if (state & SIRF_SAT_BIT_SYNC_COMPLETE)
+                               printf(" bit,");
+                       if (state & SIRF_SAT_SUBFRAME_SYNC_COMPLETE)
+                               printf(" sub,");
+                       if (state & SIRF_SAT_CARRIER_PULLIN_COMPLETE)
+                               printf(" pullin,");
+                       if (state & SIRF_SAT_CODE_LOCKED)
+                               printf(" code,");
+                       if (state & SIRF_SAT_ACQUISITION_FAILED)
+                               printf(" fail,");
+                       if (state & SIRF_SAT_EPHEMERIS_AVAILABLE)
+                               printf(" ephem,");
+                       printf ("\n");
+               }
+               break;
+       }
+       default:
+               return;
+               printf ("%s %4d:", from, encoded_len);
+               for (i = 4; i < len - 4; i++) {
+                       if (((i - 4) & 0xf) == 0)
+                               printf("\n   ");
+                       printf (" %3d", msg[i]);
+               }
+               printf ("\n");
+       }
+}
+
+static uint8_t skytraq_message[4096];
+static int     skytraq_message_len;
+static uint8_t skytraq_in_message[4096];
+static int     skytraq_in_len;
+
+char
+ao_serial_getchar(void)
+{
+       char    c;
+       uint8_t uc;
+
+       while (input_head == input_tail) {
+               for (;;) {
+                       input_tail = read(ao_gps_fd, input_queue, QUEUE_LEN);
+                       if (input_tail < 0) {
+                               if (errno == EINTR || errno == EAGAIN)
+                                       continue;
+                               perror ("getchar");
+                               exit (1);
+                       }
+                       input_head = 0;
+                       break;
+               }
+       }
+       c = input_queue[input_head];
+       input_head = (input_head + 1) % QUEUE_LEN;
+       uc = c;
+//     printf ("c: %02x %c\n", uc, uc);
+       if (skytraq_in_len || uc == '$') {
+               if (skytraq_in_len < 4096)
+                       skytraq_in_message[skytraq_in_len++] = uc;
+               if (uc == 0x0a) {
+                       check_skytraq_message("recv", skytraq_in_message, skytraq_in_len);
+                       skytraq_in_len = 0;
+               }
+       }
+       return c;
+}
+
+
+void
+ao_serial_putchar(char c)
+{
+       int     i;
+       uint8_t uc = (uint8_t) c;
+
+       if (skytraq_message_len || uc == 0xa0) {
+               if (skytraq_message_len < 4096)
+                       skytraq_message[skytraq_message_len++] = uc;
+               if (uc == 0x0a) {
+                       check_skytraq_message("send", skytraq_message, skytraq_message_len);
+                       skytraq_message_len = 0;
+               }
+       }
+       for (;;) {
+               i = write(ao_gps_fd, &c, 1);
+               if (i == 1) {
+                       if ((uint8_t) c == 0xb3 || c == '\r') {
+                               static const struct timespec delay = {
+                                       .tv_sec = 0,
+                                       .tv_nsec = 100 * 1000 * 1000
+                               };
+                               tcdrain(ao_gps_fd);
+//                             nanosleep(&delay, NULL);
+                       }
+                       break;
+               }
+               if (i < 0 && (errno == EINTR || errno == EAGAIN))
+                       continue;
+               perror("putchar");
+               exit(1);
+       }
+}
+
+#define AO_SERIAL_SPEED_4800   0
+#define AO_SERIAL_SPEED_9600   1
+#define AO_SERIAL_SPEED_57600  2
+
+static void
+ao_serial_set_speed(uint8_t speed)
+{
+       int     fd = ao_gps_fd;
+       struct termios  termios;
+
+       tcdrain(fd);
+       tcgetattr(fd, &termios);
+       switch (speed) {
+       case AO_SERIAL_SPEED_4800:
+               cfsetspeed(&termios, B4800);
+               break;
+       case AO_SERIAL_SPEED_9600:
+               cfsetspeed(&termios, B38400);
+               break;
+       case AO_SERIAL_SPEED_57600:
+               cfsetspeed(&termios, B57600);
+               break;
+       }
+       tcsetattr(fd, TCSAFLUSH, &termios);
+       tcflush(fd, TCIFLUSH);
+}
+
+#include "ao_gps_print.c"
+#include "ao_gps_skytraq.c"
+
+void
+ao_dump_state(void *wchan)
+{
+       double  lat, lon;
+       int     i;
+       if (wchan == &ao_gps_data)
+               ao_gps_print(&ao_gps_data);
+       else
+               ao_gps_tracking_print(&ao_gps_tracking_data);
+       putchar('\n');
+       return;
+}
+
+int
+ao_gps_open(const char *tty)
+{
+       struct termios  termios;
+       int fd;
+
+       fd = open (tty, O_RDWR);
+       if (fd < 0)
+               return -1;
+
+       tcgetattr(fd, &termios);
+       cfmakeraw(&termios);
+       cfsetspeed(&termios, B4800);
+       tcsetattr(fd, TCSAFLUSH, &termios);
+
+       tcdrain(fd);
+       tcflush(fd, TCIFLUSH);
+       return fd;
+}
+
+#include <getopt.h>
+
+static const struct option options[] = {
+       { .name = "tty", .has_arg = 1, .val = 'T' },
+       { 0, 0, 0, 0},
+};
+
+static void usage(char *program)
+{
+       fprintf(stderr, "usage: %s [--tty <tty-name>]\n", program);
+       exit(1);
+}
+
+int
+main (int argc, char **argv)
+{
+       char    *tty = "/dev/ttyUSB0";
+       int     c;
+
+       while ((c = getopt_long(argc, argv, "T:", options, NULL)) != -1) {
+               switch (c) {
+               case 'T':
+                       tty = optarg;
+                       break;
+               default:
+                       usage(argv[0]);
+                       break;
+               }
+       }
+       ao_gps_fd = ao_gps_open(tty);
+       if (ao_gps_fd < 0) {
+               perror (tty);
+               exit (1);
+       }
+       ao_gps();
+}
index 59110354efd3651192c7a4dd7199d03b7333152f..1e3ea3e39b2cd26852d30b18f5a8c5c095070f18 100644 (file)
@@ -60,7 +60,10 @@ ao_serial_getchar(void) __critical
                ao_sleep(&ao_usart1_rx_fifo);
        ao_fifo_remove(ao_usart1_rx_fifo, c);
        if (serial_echo) {
-               printf("%02x\n", ((int) c) & 0xff);
+               printf("%02x ", ((int) c) & 0xff);
+               if (c >= ' ')
+                       putchar(c);
+               putchar('\n');
                flush();
        }
        return c;
@@ -121,6 +124,10 @@ static const struct {
                /* .baud = */ 163,
                /* .gcr  = */ (7 << UxGCR_BAUD_E_SHIFT) | UxGCR_ORDER_LSB
        },
+       /* [AO_SERIAL_SPEED_9600] = */ {
+               /* .baud = */ 163,
+               /* .gcr  = */ (8 << UxGCR_BAUD_E_SHIFT) | UxGCR_ORDER_LSB
+       },
        /* [AO_SERIAL_SPEED_57600] = */ {
                /* .baud = */ 59,
                /* .gcr =  */ (11 << UxGCR_BAUD_E_SHIFT) | UxGCR_ORDER_LSB
index deb63597f91845dec91a69a2158c0f80df9b2ba8..d696b914e88c845a70d1789424b7e2ec21553281 100644 (file)
@@ -31,7 +31,6 @@ main(void)
        ao_cmd_init();
        ao_usb_init();
        ao_serial_init();
-       ao_gps_init();
        ao_monitor_init(AO_LED_GREEN, TRUE);
        ao_radio_init();
        ao_config_init();
diff --git a/src/skytraq-cksum b/src/skytraq-cksum
new file mode 100644 (file)
index 0000000..e4960bf
--- /dev/null
@@ -0,0 +1,44 @@
+#!/usr/bin/env nickle
+
+int checksum(int[] msg)
+{
+       int sum = 0;
+       for (int i = 0; i < dim(msg); i++) {
+               sum ^= msg[i];
+               sum &= 0xff;
+       }
+       return sum;
+}
+
+void main()
+{
+       string[...]     input;
+       int[...]        msg;
+
+       setdim(input, 0);
+       while (!File::end(stdin)) {
+               input[dim(input)] = gets();
+       }
+
+       setdim(msg, 0);
+       for (int i = 0; i < dim(input); i++) {
+               string[*] words = String::wordsplit(input[i], " ,\t");
+               for (int j = 0; j < dim(words); j++) {
+                       if (words[j] == "/" + "*")
+                               break;
+                       if (String::length(words[j]) > 0 &&
+                           Ctype::isdigit(words[j][0])) {
+                               msg[dim(msg)] = string_to_integer(words[j]);
+                       }
+                }
+       }
+       printf("\t0xa0, 0xa1, 0x02x, 0x%02x,\t\t/* length: %d bytes */\n",
+              dim(msg) >> 8, dim(msg) & 0xff, dim(msg));
+       for (int i = 0; i < dim(input); i++)
+               printf("%s\n", input[i]);
+       int csum = checksum(msg);
+       printf ("\t0x%02x, 0x0d, 0x0a,\n",
+               csum);
+}
+
+main();