diff options
| -rw-r--r-- | src/Makefile | 5 | ||||
| -rw-r--r-- | src/ao_gps.c | 47 | ||||
| -rw-r--r-- | src/ao_gps_print.c | 2 | ||||
| -rw-r--r-- | src/ao_gps_test.c | 204 | ||||
| -rw-r--r-- | src/ao_host.h | 151 |
5 files changed, 400 insertions, 9 deletions
diff --git a/src/Makefile b/src/Makefile index 29ee4b12..812b1955 100644 --- a/src/Makefile +++ b/src/Makefile @@ -181,7 +181,7 @@ SYM=$(REL:.rel=.sym) PROGS= telemetrum.ihx tidongle.ihx \ teleterra.ihx teledongle.ihx -HOST_PROGS=ao_flight_test +HOST_PROGS=ao_flight_test ao_gps_test PCDB=$(PROGS:.ihx=.cdb) PLNK=$(PROGS:.ihx=.lnk) @@ -262,3 +262,6 @@ install: 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 diff --git a/src/ao_gps.c b/src/ao_gps.c index e7f0693c..f83361eb 100644 --- a/src/ao_gps.c +++ b/src/ao_gps.c @@ -15,17 +15,29 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. */ +#ifndef AO_GPS_TEST #include "ao.h" +#endif __xdata uint8_t ao_gps_mutex; __xdata struct ao_gps_data ao_gps_data; -const char ao_gps_config[] = { - /* Serial param - binary, 4800 baud, 8 bits, 1 stop, no parity */ +const char ao_gps_set_binary[] = { '$', 'P', 'S', 'R', 'F', '1', '0', '0', ',', '0', ',', - '4', '8', '0', '0', ',', '8', ',', '1', ',', '0', '*', - '0', 'F', '\r','\n', + '9', '6', '0', '0', ',', '8', ',', '1', ',', '0', '*', + '0', 'C', '\r','\n', + + 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, +}; +const char ao_gps_config[] = { 0xa0, 0xa2, 0x00, 0x0e, /* length: 14 bytes */ 136, /* mode control */ 0, 0, /* reserved */ @@ -212,21 +224,27 @@ 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; } @@ -234,14 +252,26 @@ ao_sirf_parse_41(void) } void +ao_gps_setup(void) __reentrant +{ + uint8_t i, j; + for (j = 0; j < 2; j++) { +#ifdef AO_GPS_TEST + ao_serial_set_speed(j); +#endif + for (i = 0; i < sizeof (ao_gps_set_binary); i++) + ao_serial_putchar(ao_gps_set_binary[i]); + } +} + +void ao_gps(void) __reentrant { - char c; uint8_t i; uint16_t cksum; - for (i = 0; (c = ao_gps_config[i]); i++) - ao_serial_putchar(c); + for (i = 0; i < sizeof (ao_gps_config); i++) + ao_serial_putchar(ao_gps_config[i]); for (;;) { /* Locate the begining of the next record */ while (ao_sirf_byte() != 0xa0) @@ -259,10 +289,11 @@ ao_gps(void) __reentrant /* message ID */ i = data_byte (); /* 0 */ + printf ("message %d len %d\n", i, ao_sirf_len); switch (i) { case 41: - if (ao_sirf_len < 91) + if (ao_sirf_len < 90) break; ao_sirf_parse_41(); break; diff --git a/src/ao_gps_print.c b/src/ao_gps_print.c index 46521b10..5ad8d022 100644 --- a/src/ao_gps_print.c +++ b/src/ao_gps_print.c @@ -15,7 +15,9 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. */ +#ifndef AO_GPS_TEST #include "ao.h" +#endif struct ao_gps_split { uint8_t positive; diff --git a/src/ao_gps_test.c b/src/ao_gps_test.c new file mode 100644 index 00000000..7489e788 --- /dev/null +++ b/src/ao_gps_test.c @@ -0,0 +1,204 @@ +/* + * Copyright © 2009 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#define AO_GPS_TEST +#include "ao_host.h" +#include <termios.h> +#include <errno.h> +#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) + +#define AO_GPS_VALID (1 << 4) + +struct ao_gps_data { + uint8_t hour; + uint8_t minute; + uint8_t second; + uint8_t flags; + int32_t latitude; /* degrees * 10⁷ */ + int32_t longitude; /* degrees * 10⁷ */ + int16_t altitude; /* m */ + uint16_t ground_speed; /* cm/s */ + uint8_t course; /* degrees / 2 */ + uint8_t hdop; /* * 5 */ + int16_t climb_rate; /* cm/s */ + uint16_t h_error; /* m */ + uint16_t v_error; /* m */ +}; + +void +ao_mutex_get(uint8_t *mutex) +{ +} + +void +ao_mutex_put(uint8_t *mutex) +{ +} + +static int +ao_gps_fd; + +static void +ao_dbg_char(char c) +{ + char line[128]; + line[0] = '\0'; + if (c < ' ') { + if (c == '\n') + sprintf (line, "\n"); + else + sprintf (line, "\\%02x", ((int) c) & 0xff); + } else { + sprintf (line, "%c", c); + } + write(1, line, strlen(line)); +} + +#define QUEUE_LEN 4096 + +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; +} + +void * +ao_gps_input(void *arg) +{ + int i; + char c; + + 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; + } + if (i < 0 && (errno == EINTR || errno == EAGAIN)) + continue; + perror("getchar"); + exit(1); + } +} + +void +ao_serial_putchar(char c) +{ + int i; + + ao_dbg_char(c); + for (;;) { + i = write(ao_gps_fd, &c, 1); + if (i == 1) + break; + if (i < 0 && (errno == EINTR || errno == EAGAIN)) + continue; + perror("putchar"); + exit(1); + } +} + +static void +ao_serial_set_speed(uint8_t fast) +{ + int fd = ao_gps_fd; + struct termios termios; + + tcdrain(fd); + tcgetattr(fd, &termios); + cfsetspeed(&termios, fast ? B9600 : B4800); + tcsetattr(fd, TCSAFLUSH, &termios); + tcflush(fd, TCIFLUSH); +} + +#include "ao_gps_print.c" +#include "ao_gps.c" + +void +ao_dump_state(void) +{ + printf ("dump state\n"); +} + +int +ao_gps_open(const char *tty) +{ + struct termios termios; + int fd; + + fd = open (tty, O_RDWR); + if (fd < 0) + return -1; + + tcgetattr(fd, &termios); + cfmakeraw(&termios); + cfsetspeed(&termios, B4800); + tcsetattr(fd, TCSAFLUSH, &termios); + + tcdrain(fd); + tcflush(fd, TCIFLUSH); + return fd; +} + +pthread_t input_thread; + +int +main (int argc, char **argv) +{ + char *gps_file = "/dev/ttyUSB0"; + + ao_gps_fd = ao_gps_open(gps_file); + if (ao_gps_fd < 0) { + perror (gps_file); + 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_host.h b/src/ao_host.h new file mode 100644 index 00000000..38ff84ac --- /dev/null +++ b/src/ao_host.h @@ -0,0 +1,151 @@ +/* + * Copyright © 2009 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#define _GNU_SOURCE + +#include <stddef.h> +#include <stdint.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> + +#define AO_ADC_RING 64 +#define ao_adc_ring_next(n) (((n) + 1) & (AO_ADC_RING - 1)) +#define ao_adc_ring_prev(n) (((n) - 1) & (AO_ADC_RING - 1)) + +/* + * One set of samples read from the A/D converter + */ +struct ao_adc { + uint16_t tick; /* tick when the sample was read */ + int16_t accel; /* accelerometer */ + int16_t pres; /* pressure sensor */ + int16_t temp; /* temperature sensor */ + int16_t v_batt; /* battery voltage */ + int16_t sense_d; /* drogue continuity sense */ + int16_t sense_m; /* main continuity sense */ +}; + +#define __pdata +#define __data +#define __xdata +#define __code +#define __reentrant + +enum ao_flight_state { + ao_flight_startup = 0, + ao_flight_idle = 1, + ao_flight_pad = 2, + ao_flight_boost = 3, + ao_flight_fast = 4, + ao_flight_coast = 5, + ao_flight_drogue = 6, + ao_flight_main = 7, + ao_flight_landed = 8, + ao_flight_invalid = 9 +}; + +struct ao_adc ao_adc_ring[AO_ADC_RING]; +uint8_t ao_adc_head; + +#define ao_led_on(l) +#define ao_led_off(l) +#define ao_timer_set_adc_interval(i) +#define ao_wakeup(wchan) ao_dump_state() +#define ao_cmd_register(c) +#define ao_usb_disable() +#define ao_telemetry_set_interval(x) + +enum ao_igniter { + ao_igniter_drogue = 0, + ao_igniter_main = 1 +}; + +void +ao_ignite(enum ao_igniter igniter) +{ + printf ("ignite %s\n", igniter == ao_igniter_drogue ? "drogue" : "main"); +} + +struct ao_task { + int dummy; +}; + +#define ao_add_task(t,f,n) + +#define ao_log_start() +#define ao_log_stop() + +#define AO_MS_TO_TICKS(ms) ((ms) / 10) +#define AO_SEC_TO_TICKS(s) ((s) * 100) + +#define AO_FLIGHT_TEST + +struct ao_adc ao_adc_static; + +FILE *emulator_in; + +void +ao_dump_state(void); + +void +ao_sleep(void *wchan); + +const char const * const ao_state_names[] = { + "startup", "idle", "pad", "boost", "fast", + "coast", "drogue", "main", "landed", "invalid" +}; + +struct ao_cmds { + char cmd; + void (*func)(void); + const char *help; +}; + + +static int16_t altitude_table[2048] = { +#include "altitude.h" +}; + +int16_t +ao_pres_to_altitude(int16_t pres) __reentrant +{ + pres = pres >> 4; + if (pres < 0) pres = 0; + if (pres > 2047) pres = 2047; + return altitude_table[pres]; +} + +int16_t +ao_altitude_to_pres(int16_t alt) __reentrant +{ + int16_t pres; + + for (pres = 0; pres < 2047; pres++) + if (altitude_table[pres] <= alt) + break; + return pres << 4; +} + +struct ao_config { + uint16_t main_deploy; + int16_t accel_zero_g; +}; + +#define ao_config_get() + +struct ao_config ao_config = { 250, 16000 }; |