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
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);
__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 */
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
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)
/* message ID */
i = data_byte (); /* 0 */
- printf ("message %d len %d\n", i, ao_sirf_len);
switch (i) {
case 41:
#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)
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)
}
}
+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)
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;
}
}
}
+#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);
}
return fd;
}
-pthread_t input_thread;
-
int
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();
}
{ 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)
{
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 |
projects / fw / altos / commitdiff