diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/Makefile | 2 | ||||
| -rw-r--r-- | src/ao.h | 6 | ||||
| -rw-r--r-- | src/ao_gps.c | 62 | ||||
| -rw-r--r-- | src/ao_gps_test.c | 104 | ||||
| -rw-r--r-- | 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 @@ -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 <sys/types.h> #include <sys/stat.h> #include <fcntl.h> -#include <pthread.h> -#include <semaphore.h> #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 | |