aclocal.m4
src/ao_flight_test
src/ao_gps_test
+src/ao_gps_test_skytraq
ao-teledongle.h
ao-telemetrum.h
ao-teleterra.h
stamp-h1
tags
teledongle
-telemetrum
+telemetrum-sirf
+telemetrum-sky
teleterra
tidongle
TELE_DRIVER_SRC = \
ao_convert.c \
- ao_gps.c \
ao_serial.c
#
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
#
$(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
$(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
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)
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)
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
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);
+++ /dev/null
-/*
- * 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]);
-}
--- /dev/null
+/*
+ * 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]);
+}
--- /dev/null
+/*
+ * 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]);
+}
}
#include "ao_gps_print.c"
-#include "ao_gps.c"
+#include "ao_gps_sirf.c"
void
ao_dump_state(void *wchan)
--- /dev/null
+/*
+ * 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();
+}
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;
/* .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
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();
--- /dev/null
+#!/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();
projects / fw / altos / commitdiff