diff options
| -rw-r--r-- | .gitignore | 4 | ||||
| -rw-r--r-- | src/Makefile | 48 | ||||
| -rw-r--r-- | src/ao.h | 3 | ||||
| -rw-r--r-- | src/ao_gps_sirf.c (renamed from src/ao_gps.c) | 0 | ||||
| -rw-r--r-- | src/ao_gps_skytraq.c | 369 | ||||
| -rw-r--r-- | src/ao_gps_test.c | 2 | ||||
| -rw-r--r-- | src/ao_gps_test_skytraq.c | 478 | ||||
| -rw-r--r-- | src/ao_serial.c | 9 | ||||
| -rw-r--r-- | src/ao_teleterra.c | 1 | ||||
| -rw-r--r-- | src/skytraq-cksum | 44 |
10 files changed, 944 insertions, 14 deletions
@@ -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 diff --git a/src/Makefile b/src/Makefile index 828c48bd..9891cdad 100644 --- a/src/Makefile +++ b/src/Makefile @@ -59,7 +59,6 @@ TELE_RECEIVER_SRC =\ TELE_DRIVER_SRC = \ ao_convert.c \ - ao_gps.c \ ao_serial.c # @@ -85,6 +84,17 @@ TM_DRIVER_SRC = \ 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 = \ @@ -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 @@ -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_sirf.c index 2b3a5178..2b3a5178 100644 --- a/src/ao_gps.c +++ b/src/ao_gps_sirf.c diff --git a/src/ao_gps_skytraq.c b/src/ao_gps_skytraq.c new file mode 100644 index 00000000..b397d975 --- /dev/null +++ b/src/ao_gps_skytraq.c @@ -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]); +} diff --git a/src/ao_gps_test.c b/src/ao_gps_test.c index c94128d9..366bca71 100644 --- a/src/ao_gps_test.c +++ b/src/ao_gps_test.c @@ -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 index 00000000..510bc419 --- /dev/null +++ b/src/ao_gps_test_skytraq.c @@ -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(); +} diff --git a/src/ao_serial.c b/src/ao_serial.c index 59110354..1e3ea3e3 100644 --- a/src/ao_serial.c +++ b/src/ao_serial.c @@ -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 diff --git a/src/ao_teleterra.c b/src/ao_teleterra.c index deb63597..d696b914 100644 --- a/src/ao_teleterra.c +++ b/src/ao_teleterra.c @@ -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 index 00000000..e4960bff --- /dev/null +++ b/src/skytraq-cksum @@ -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(); |