From 5f0179652e8bb85add9067e5253e981c60f2c51e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 17 Jul 2009 16:03:35 -0700 Subject: [PATCH] Fix up SiRF parsing and test code so that it actually works Signed-off-by: Keith Packard --- src/ao_gps.c | 68 +++++++++++------ src/ao_gps_test.c | 188 +++++++++++++++++++++++++++++++++++++++++++++- 2 files changed, 230 insertions(+), 26 deletions(-) diff --git a/src/ao_gps.c b/src/ao_gps.c index f83361eb..b6d7182d 100644 --- a/src/ao_gps.c +++ b/src/ao_gps.c @@ -56,22 +56,6 @@ const char ao_gps_config[] = { 143, /* static navigation */ 0, /* disable */ 0x00, 0x8f, 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, 0x08, /* length: 8 bytes */ - 166, /* Set message rate */ - 0, /* enable/disable one message */ - 41, /* message 41 */ - 1, /* once per second */ - 0, 0, 0, 0, /* reserved */ - 0x00, 0xd0, 0xb0, 0xb3, }; #define NAV_TYPE_GPS_FIX_TYPE_MASK (7 << 0) @@ -224,27 +208,21 @@ ao_sirf_parse_41(void) { uint8_t i, offset; - printf("parse 41\n"); for (i = 0; ; i++) { offset = geodetic_nav_data_packet[i].offset; switch (geodetic_nav_data_packet[i].type) { case SIRF_END: - printf("parse 41 done\n"); return; case SIRF_DISCARD: - printf("parse 41 discard %d\n", offset); sirf_discard(offset); break; case SIRF_U8: - printf("parse 41 u8 %d\n", offset); sirf_u8(offset); break; case SIRF_U16: - printf("parse 41 u16 %d\n", offset); sirf_u16(offset); break; case SIRF_U32: - printf("parse 41 u32 %d\n", offset); sirf_u32(offset); break; } @@ -259,11 +237,53 @@ ao_gps_setup(void) __reentrant #ifdef AO_GPS_TEST ao_serial_set_speed(j); #endif + for (i = 255; i != 0; i--) + ao_serial_putchar(0); + for (i = 0; i < sizeof (ao_gps_set_binary); i++) ao_serial_putchar(ao_gps_set_binary[i]); } } +static const char ao_gps_set_message_rate[] = { + 0xa0, 0xa2, 0x00, 0x08, + 166, + 0, +#define SET_MESSAGE_RATE_ID 6 +#define SET_MESSAGE_RATE 7 + +}; + +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, + 4, + 9, + 10, + 27, + 50, + 52, + 4, +}; + void ao_gps(void) __reentrant { @@ -272,6 +292,9 @@ ao_gps(void) __reentrant 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); for (;;) { /* Locate the begining of the next record */ while (ao_sirf_byte() != 0xa0) @@ -336,7 +359,6 @@ ao_gps(void) __reentrant ao_gps_data.v_error = 65535; else ao_gps_data.v_error = ao_sirf_data.v_error / 100; - ao_gps_data.h_error = ao_sirf_data.h_error; ao_mutex_put(&ao_gps_mutex); ao_wakeup(&ao_gps_data); break; diff --git a/src/ao_gps_test.c b/src/ao_gps_test.c index 7489e788..35cce7de 100644 --- a/src/ao_gps_test.c +++ b/src/ao_gps_test.c @@ -98,6 +98,145 @@ ao_serial_getchar(void) return c; } +static void +check_sirf_message(char *from, uint8_t *msg, int len) +{ + uint16_t encoded_len, encoded_cksum; + uint16_t cksum; + uint8_t id; + int i; + + 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]; + if (encoded_len != len - 8) { + 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]; + if (id == 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 %d\n", week); + printf ("\tTOW %d\n", tow); + printf ("\tyear %d\n", year); + printf ("\tmonth %d\n", month); + printf ("\tday %d\n", day); + printf ("\thour %d\n", hour); + printf ("\tminute %d\n", minute); + printf ("\tsecond %g\n", second / 1000.0); + printf ("\tsats: %08x\n", sat_list); + printf ("\tlat: %g\n", lat / 1.0e7); + printf ("\tlon: %g\n", lon / 1.0e7); + printf ("\talt_ell: %g\n", alt_ell / 100.0); + printf ("\talt_msll: %g\n", alt_msl / 100.0); + printf ("\tdatum: %d\n", datum); + printf ("\tground speed: %g\n", sog / 100.0); + printf ("\tcourse: %g\n", cog / 100.0); + printf ("\tclimb: %g\n", climb_rate / 100.0); + printf ("\theading rate: %g\n", heading_rate / 100.0); + printf ("\th error: %g\n", h_error / 100.0); + printf ("\tv error: %g\n", v_error / 100.0); + printf ("\tt error: %g\n", t_error / 100.0); + printf ("\th vel error: %g\n", h_v_error / 100.0); + } else { + 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 sirf_in_message[4096]; +static int sirf_in_len; + void * ao_gps_input(void *arg) { @@ -109,7 +248,16 @@ ao_gps_input(void *arg) i = read(ao_gps_fd, &c, 1); if (i == 1) { int v; + uint8_t uc = c; + if (sirf_in_len || uc == 0xa0) { + if (sirf_in_len < 4096) + sirf_in_message[sirf_in_len++] = uc; + if (uc == 0xb3) { + check_sirf_message("recv", sirf_in_message, sirf_in_len); + sirf_in_len = 0; + } + } input_queue[input_tail] = c; input_tail = (input_tail + 1) % QUEUE_LEN; sem_post(&input_semaphore); @@ -125,16 +273,32 @@ ao_gps_input(void *arg) } } +static uint8_t sirf_message[4096]; +static int sirf_message_len; + void ao_serial_putchar(char c) { int i; + uint8_t uc = (uint8_t) c; - ao_dbg_char(c); + if (sirf_message_len || uc == 0xa0) { + if (sirf_message_len < 4096) + sirf_message[sirf_message_len++] = uc; + if (uc == 0xb3) { + check_sirf_message("send", sirf_message, sirf_message_len); + sirf_message_len = 0; + } + } for (;;) { i = write(ao_gps_fd, &c, 1); - if (i == 1) + if (i == 1) { + if ((uint8_t) c == 0xb3 || c == '\r') { + tcdrain(ao_gps_fd); + usleep (1000 * 100); + } break; + } if (i < 0 && (errno == EINTR || errno == EAGAIN)) continue; perror("putchar"); @@ -161,7 +325,25 @@ ao_serial_set_speed(uint8_t fast) void ao_dump_state(void) { - printf ("dump state\n"); + double lat, lon; + printf ("%02d:%02d:%02d", + ao_gps_data.hour, ao_gps_data.minute, + ao_gps_data.second); + printf (" nsat %d %svalid", + ao_gps_data.flags & AO_GPS_NUM_SAT_MASK, + ao_gps_data.flags & AO_GPS_VALID ? "" : "not "); + printf (" lat %g lon %g alt %d", + ao_gps_data.latitude / 1.0e7, + ao_gps_data.longitude / 1.0e7, + ao_gps_data.altitude); + printf (" speed %g climb %g course %d", + ao_gps_data.ground_speed / 100.0, + ao_gps_data.climb_rate / 100.0, + ao_gps_data.course * 2); + printf (" hdop %g h_error %d v_error %d", + ao_gps_data.hdop / 5.0, + ao_gps_data.h_error, ao_gps_data.v_error); + printf("\n"); } int -- 2.30.2