From 34f148500df427c148188c0ada20bf914a7c74ba Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 17 Jul 2009 19:23:10 -0700 Subject: [PATCH] Use 57600 baud for GPS. Clean up gps init. Assume GPS is either in 4800 NMEA or 57600 SiRF mode, send just the sequence to get from 4800 NMEA to 5760 SiRF. Also, eliminate threads from the gps test program. Signed-off-by: Keith Packard --- src/Makefile | 2 +- src/ao.h | 6 +++ src/ao_gps.c | 62 +++++++++------------------ src/ao_gps_test.c | 104 ++++++++++++++++++++-------------------------- src/ao_serial.c | 24 ++++++++++- 5 files changed, 93 insertions(+), 105 deletions(-) diff --git a/src/Makefile b/src/Makefile index 812b1955..297f676b 100644 --- a/src/Makefile +++ b/src/Makefile @@ -264,4 +264,4 @@ ao_flight_test: ao_flight.c ao_flight_test.c cc -g -o $@ ao_flight_test.c ao_gps_test: ao_gps.c ao_gps_test.c ao_host.h - cc -g -o $@ ao_gps_test.c -lpthread + cc -g -o $@ ao_gps_test.c diff --git a/src/ao.h b/src/ao.h index 30511c8c..2e582577 100644 --- a/src/ao.h +++ b/src/ao.h @@ -661,6 +661,12 @@ ao_serial_getchar(void) __critical; void ao_serial_putchar(char c) __critical; +#define AO_SERIAL_SPEED_4800 0 +#define AO_SERIAL_SPEED_57600 1 + +void +ao_serial_set_speed(uint8_t speed); + void ao_serial_init(void); diff --git a/src/ao_gps.c b/src/ao_gps.c index 4d278832..811ac2a8 100644 --- a/src/ao_gps.c +++ b/src/ao_gps.c @@ -22,23 +22,7 @@ __xdata uint8_t ao_gps_mutex; __xdata struct ao_gps_data ao_gps_data; -static const char ao_gps_set_nmea[] = { - - '$', 'P', 'S', 'R', 'F', '1', '0', '0', ',', '0', ',', - '9', '6', '0', '0', ',', '8', ',', '1', ',', '0', '*', - '0', 'C', '\r','\n', -}; - -static const char ao_gps_set_sirf[] = { - 0xa0, 0xa2, 0x00, 0x09, /* length 9 bytes */ - 134, /* Set binary serial port */ - 0, 0, 0x25, 0x80, /* 9600 baud */ - 8, /* data bits */ - 1, /* stop bits */ - 0, /* parity */ - 0, /* pad */ - 0x01, 0x34, 0xb0, 0xb3, -}; +static const char ao_gps_set_nmea[] = "$PSRF100,0,57600,8,1,0*37\r\n"; const char ao_gps_config[] = { 0xa0, 0xa2, 0x00, 0x0e, /* length: 14 bytes */ @@ -243,31 +227,21 @@ ao_sirf_parse_41(void) void ao_gps_setup(void) __reentrant { - uint8_t i, j, k; - for (j = 0; j < 2; j++) { -#ifdef AO_GPS_TEST - ao_serial_set_speed(j); -#endif - for (i = 0; i < 128; i++) - ao_serial_putchar(0x55); - for (k = 0; k < 4; k++) - for (i = 0; i < sizeof (ao_gps_set_nmea); i++) - ao_serial_putchar(ao_gps_set_nmea[i]); - for (i = 0; i < 128; i++) - ao_serial_putchar(0x55); - for (k = 0; k < 4; k++) - for (i = 0; i < sizeof (ao_gps_set_sirf); i++) - ao_serial_putchar(ao_gps_set_sirf[i]); - } + uint8_t i; + ao_serial_set_speed(AO_SERIAL_SPEED_4800); + for (i = 0; i < 16; i++) + ao_serial_putchar(0x00); + for (i = 0; i < sizeof (ao_gps_set_nmea) - 1; i++) + ao_serial_putchar(ao_gps_set_nmea[i]); + ao_serial_set_speed(AO_SERIAL_SPEED_57600); + for (i = 0; i < 16; i++) + ao_serial_putchar(0x00); } 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 @@ -302,14 +276,17 @@ static const uint8_t sirf_disable[] = { void ao_gps(void) __reentrant { - uint8_t i; + uint8_t i, k; uint16_t cksum; - 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 (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); + } for (;;) { /* Locate the begining of the next record */ while (ao_sirf_byte() != 0xa0) @@ -327,7 +304,6 @@ ao_gps(void) __reentrant /* message ID */ i = data_byte (); /* 0 */ - printf ("message %d len %d\n", i, ao_sirf_len); switch (i) { case 41: diff --git a/src/ao_gps_test.c b/src/ao_gps_test.c index 35cce7de..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,24 +77,6 @@ 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) -{ - char c; - int value; - char line[100]; - - 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; -} static void check_sirf_message(char *from, uint8_t *msg, int len) @@ -234,47 +214,44 @@ check_sirf_message(char *from, uint8_t *msg, int len) } } +static uint8_t sirf_message[4096]; +static int sirf_message_len; static uint8_t sirf_in_message[4096]; static int sirf_in_len; -void * -ao_gps_input(void *arg) +char +ao_serial_getchar(void) { - int i; char c; - - printf("ao_gps_input\n"); - for (;;) { - 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; - } + 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_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; + input_head = 0; + break; } - if (i < 0 && (errno == EINTR || errno == EAGAIN)) - continue; - perror("getchar"); - exit(1); } + 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; + } + } + return c; } -static uint8_t sirf_message[4096]; -static int sirf_message_len; void ao_serial_putchar(char c) @@ -294,8 +271,12 @@ ao_serial_putchar(char c) 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); - usleep (1000 * 100); +// nanosleep(&delay, NULL); } break; } @@ -306,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); } @@ -366,8 +357,6 @@ ao_gps_open(const char *tty) return fd; } -pthread_t input_thread; - int main (int argc, char **argv) { @@ -379,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(); } diff --git a/src/ao_serial.c b/src/ao_serial.c index ce116940..e926b4e4 100644 --- a/src/ao_serial.c +++ b/src/ao_serial.c @@ -84,6 +84,27 @@ __code struct ao_cmds ao_serial_cmds[] = { { 0, send_serial, NULL }, }; +static const struct { + uint8_t baud; + uint8_t gcr; +} ao_serial_speeds[] = { + /* [AO_SERIAL_SPEED_4800] = */ { + /* .baud = */ 163, + /* .gcr = */ (7 << UxGCR_BAUD_E_SHIFT) | UxGCR_ORDER_LSB + }, + /* [AO_SERIAL_SPEED_57600] = */ { + /* .baud = */ 59, + /* .gcr = */ (11 << UxGCR_BAUD_E_SHIFT) | UxGCR_ORDER_LSB + }, +}; + +void +ao_serial_set_speed(uint8_t speed) +{ + U1BAUD = ao_serial_speeds[speed].baud; + U1GCR = ao_serial_speeds[speed].gcr; +} + void ao_serial_init(void) { @@ -99,8 +120,7 @@ ao_serial_init(void) U1CSR = (UxCSR_MODE_UART | UxCSR_RE); /* Pick a 4800 baud rate */ - U1BAUD = 163; /* 4800 */ - U1GCR = 7 << UxGCR_BAUD_E_SHIFT; /* 4800 */ + ao_serial_set_speed(AO_SERIAL_SPEED_4800); /* Reasonable serial parameters */ U1UCR = (UxUCR_FLUSH | -- 2.30.2