X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fao_gps_test.c;h=0ed51d163cf06d9e79031448e77a8f188ef030d6;hp=7489e788f0db611f6369036baaddb3acfe75ab1d;hb=34f148500df427c148188c0ada20bf914a7c74ba;hpb=0b35447d05a0c7eaf4fefcbcf0065fe3320bba82 diff --git a/src/ao_gps_test.c b/src/ao_gps_test.c index 7489e788..0ed51d16 100644 --- a/src/ao_gps_test.c +++ b/src/ao_gps_test.c @@ -22,8 +22,6 @@ #include #include #include -#include -#include #define AO_GPS_NUM_SAT_MASK (0xf << 0) #define AO_GPS_NUM_SAT_SHIFT (0) @@ -79,62 +77,209 @@ ao_dbg_char(char c) static char input_queue[QUEUE_LEN]; int input_head, input_tail; -static sem_t input_semaphore; -char -ao_serial_getchar(void) +static void +check_sirf_message(char *from, uint8_t *msg, int len) { - char c; - int value; - char line[100]; + uint16_t encoded_len, encoded_cksum; + uint16_t cksum; + uint8_t id; + int i; - sem_getvalue(&input_semaphore, &value); -// printf ("ao_serial_getchar %d\n", value); - sem_wait(&input_semaphore); - c = input_queue[input_head]; - input_head = (input_head + 1) % QUEUE_LEN; -// sprintf (line, "%02x\n", ((int) c) & 0xff); -// write(1, line, strlen(line)); - return c; + 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"); + } } -void * -ao_gps_input(void *arg) +static uint8_t sirf_message[4096]; +static int sirf_message_len; +static uint8_t sirf_in_message[4096]; +static int sirf_in_len; + +char +ao_serial_getchar(void) { - int i; char c; + uint8_t uc; - printf("ao_gps_input\n"); - for (;;) { - i = read(ao_gps_fd, &c, 1); - if (i == 1) { - int v; - - input_queue[input_tail] = c; - input_tail = (input_tail + 1) % QUEUE_LEN; - sem_post(&input_semaphore); - sem_getvalue(&input_semaphore, &v); -// printf ("ao_gps_input %02x %d\n", ((int) c) & 0xff, v); - fflush(stdout); - continue; + 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; + 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; } - if (i < 0 && (errno == EINTR || errno == EAGAIN)) - continue; - perror("getchar"); - exit(1); } + return c; } + 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') { + 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"); @@ -142,15 +287,25 @@ ao_serial_putchar(char c) } } +#define AO_SERIAL_SPEED_4800 0 +#define AO_SERIAL_SPEED_57600 1 + static void -ao_serial_set_speed(uint8_t fast) +ao_serial_set_speed(uint8_t speed) { int fd = ao_gps_fd; struct termios termios; tcdrain(fd); tcgetattr(fd, &termios); - cfsetspeed(&termios, fast ? B9600 : B4800); + switch (speed) { + case AO_SERIAL_SPEED_4800: + cfsetspeed(&termios, B4800); + break; + case AO_SERIAL_SPEED_57600: + cfsetspeed(&termios, B57600); + break; + } tcsetattr(fd, TCSAFLUSH, &termios); tcflush(fd, TCIFLUSH); } @@ -161,7 +316,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 @@ -184,8 +357,6 @@ ao_gps_open(const char *tty) return fd; } -pthread_t input_thread; - int main (int argc, char **argv) { @@ -197,8 +368,5 @@ main (int argc, char **argv) exit (1); } ao_gps_setup(); - sem_init(&input_semaphore, 0, 0); - if (pthread_create(&input_thread, NULL, ao_gps_input, NULL) != 0) - perror("pthread_create"); ao_gps(); }