From: Keith Packard Date: Mon, 2 Nov 2009 23:47:40 +0000 (-0800) Subject: Merge remote branch 'origin/master' X-Git-Tag: 0.6^0 X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=7db9d86178ecfd58cc1c17ac9fcbdcfd2f13aaec;hp=f9de20000794c97a04d5bc2476191864bd2af371 Merge remote branch 'origin/master' --- diff --git a/.gitignore b/.gitignore index 0ca4bed4..a6f94439 100644 --- a/.gitignore +++ b/.gitignore @@ -16,6 +16,7 @@ TAGS aclocal.m4 src/ao_flight_test src/ao_gps_test +src/ao_gps_test_skytraq ao-teledongle.h ao-telemetrum.h ao-teleterra.h @@ -47,6 +48,7 @@ missing stamp-h1 tags teledongle -telemetrum +telemetrum-sirf +telemetrum-sky teleterra tidongle diff --git a/README b/README index 1244f849..282b9ce5 100644 --- a/README +++ b/README @@ -17,18 +17,18 @@ Copyright and License with this program; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. -Parameters: +Tasking API: - * Multi-tasking - * Non-preemptive - * Unix-style sleep/wakeup scheduling - * Strict round-robin, no priorities + * Multi-tasking + * Non-preemptive + * Unix-style sleep/wakeup scheduling + * Strict round-robin, no priorities -API: + uint8_t ao_sleep(void *wchan) - void ao_sleep(void *wchan) - - Puts current task to sleep. Will wake up when wchan is signalled + Puts current task to sleep. Will wake up when wchan is + signalled (returning 0), or when an alarm has expired + (returning 1). void ao_wakeup(void *wchan) @@ -52,3 +52,63 @@ API: Any fatal error should call this function, which can display the error code in some cryptic fashion. + + void ao_wake_task(struct ao_task *task) + + Wake the task as if the wchan it is waiting on had + been signalled. + + void ao_alarm(uint16_t delay) + + Queue an alarm to expire 'delay' ticks in the future. + The alarm will cause the task to wake from ao_sleep + and return 1. Alarms are cancelled if the task is + awoken by ao_wakeup or ao_wake_task. + + void ao_exit(void) + + Stop the current task and remove it from the list of + available tasks. + +Timer API + + * Regular timer ticks (at 100Hz, by default) + * All time values are in ticks + + uint16_t ao_time(void) + + Returns the curent tick count + + void ao_delay(uint16_t delay) + + Suspend task execution for 'delay' ticks. + +DMA API + + * Static DMA entry allocation + + uint8_t ao_dma_alloc(uint8_t *done) + + Allocates one of the 5 DMA units on the cc1111 + processor. When this DMA unit is finished, it will + set *done to 1 and wakeup anyone waiting on that. + + void ao_dma_set_transfer(uint8_t id, void *src, void *dst, + uint16_t count, uint8_t cfg0, uint8_t cfg1) + + Prepares the specified DMA unit for operation. + + void ao_dma_start(uint8_t id) + + Start DMA on the specified channel. + + void ao_dma_trigger(uint8_t id) + + Manually trigger a DMA channel + + void ao_dma_abort(uint8_t id) + + Abort a pending DMA transfer, signalling + that by setting the associated 'done' to + AO_DMA_ABORTED and waking any task + sleeping on that. diff --git a/ao-tools/ao-postflight/ao-postflight.c b/ao-tools/ao-postflight/ao-postflight.c index cc9f64b4..60d8c036 100644 --- a/ao-tools/ao-postflight/ao-postflight.c +++ b/ao-tools/ao-postflight/ao-postflight.c @@ -161,8 +161,33 @@ merge_data(struct cc_perioddata *first, struct cc_perioddata *last, double split return pd; } +static const char kml_header[] = + "\n" + "\n" + " \n" + " gps\n" + " \n" + " \n" + " \n" + " 1\n" + " absolute\n" + " \n"; + +static const char kml_footer[] = + "\n" + " \n" + " \n" + "\n" + "\n"; + static void -analyse_flight(struct cc_flightraw *f, FILE *summary_file, FILE *detail_file, FILE *raw_file, char *plot_name, FILE *gps_file) +analyse_flight(struct cc_flightraw *f, FILE *summary_file, FILE *detail_file, + FILE *raw_file, char *plot_name, FILE *gps_file, FILE *kml_file) { double height; double accel; @@ -339,6 +364,37 @@ analyse_flight(struct cc_flightraw *f, FILE *summary_file, FILE *detail_file, FI fprintf(gps_file, " %d\n", nsat); } } + if (kml_file) { + int j = 0; + + fprintf(kml_file, "%s", kml_header); + for (i = 0; i < f->gps.num; i++) { + int nsat = 0; + int k; + while (j < f->gps.numsats - 1) { + if (f->gps.sats[j].sat[0].time <= f->gps.data[i].time && + f->gps.data[i].time < f->gps.sats[j+1].sat[0].time) + break; + j++; + } + nsat = 0; + for (k = 0; k < f->gps.sats[j].nsat; k++) + if (f->gps.sats[j].sat[k].state == 0xbf) + nsat++; + + fprintf(kml_file, "%12.7f, %12.7f, %12.7f ", + f->gps.data[i].lon, + f->gps.data[i].lat, + f->gps.data[i].alt, + (f->gps.data[i].time - boost_start) / 100.0, + nsat); + if (i < f->gps.num - 1) + fprintf(kml_file, ",\n"); + else + fprintf(kml_file, "\n"); + } + fprintf(kml_file, "%s", kml_footer); + } if (cooked && plot_name) { struct cc_perioddata *speed; plsdev("svgcairo"); @@ -378,6 +434,7 @@ static const struct option options[] = { { .name = "plot", .has_arg = 1, .val = 'p' }, { .name = "raw", .has_arg = 1, .val = 'r' }, { .name = "gps", .has_arg = 1, .val = 'g' }, + { .name = "kml", .has_arg = 1, .val = 'k' }, { 0, 0, 0, 0}, }; @@ -389,6 +446,7 @@ static void usage(char *program) "\t[--raw= -r -p ]\n" "\t[--gps= -g ]\n" + "\t[--kml= -k ]\n" "\t{flight-log} ...\n", program); exit(1); } @@ -401,6 +459,7 @@ main (int argc, char **argv) FILE *detail_file = NULL; FILE *raw_file = NULL; FILE *gps_file = NULL; + FILE *kml_file = NULL; int i; int ret = 0; struct cc_flightraw *raw; @@ -412,8 +471,9 @@ main (int argc, char **argv) char *raw_name = NULL; char *plot_name = NULL; char *gps_name = NULL; + char *kml_name = NULL; - while ((c = getopt_long(argc, argv, "s:d:p:r:g:", options, NULL)) != -1) { + while ((c = getopt_long(argc, argv, "s:d:p:r:g:k:", options, NULL)) != -1) { switch (c) { case 's': summary_name = optarg; @@ -430,6 +490,9 @@ main (int argc, char **argv) case 'g': gps_name = optarg; break; + case 'k': + kml_name = optarg; + break; default: usage(argv[0]); break; @@ -468,6 +531,13 @@ main (int argc, char **argv) exit(1); } } + if (kml_name) { + kml_file = fopen(kml_name, "w"); + if (!kml_file) { + perror(kml_name); + exit(1); + } + } for (i = optind; i < argc; i++) { file = fopen(argv[i], "r"); if (!file) { @@ -488,7 +558,7 @@ main (int argc, char **argv) } if (!raw->serial) raw->serial = serial; - analyse_flight(raw, summary_file, detail_file, raw_file, plot_name, gps_file); + analyse_flight(raw, summary_file, detail_file, raw_file, plot_name, gps_file, kml_file); cc_flightraw_free(raw); } return ret; diff --git a/src/Makefile b/src/Makefile index 828c48bd..d984e9dc 100644 --- a/src/Makefile +++ b/src/Makefile @@ -44,6 +44,8 @@ ALTOS_DRIVER_SRC = \ TELE_COMMON_SRC = \ ao_gps_print.c \ + ao_packet.c \ + ao_packet_slave.c \ ao_state.c # @@ -51,6 +53,7 @@ TELE_COMMON_SRC = \ # TELE_RECEIVER_SRC =\ ao_monitor.c \ + ao_packet_master.c \ ao_rssi.c # @@ -59,7 +62,6 @@ TELE_RECEIVER_SRC =\ TELE_DRIVER_SRC = \ ao_convert.c \ - ao_gps.c \ ao_serial.c # @@ -84,6 +86,17 @@ TM_DRIVER_SRC = \ ao_gps_report.c \ ao_ignite.c +# +# Drivers only on TeleMetrum +# +TM_SIRF_DRIVER_SRC = \ + ao_gps_sirf.c +# +# Drivers only on TeleMetrum +# +TM_SKY_DRIVER_SRC = \ + ao_gps_skytraq.c + # # Tasks run on TeleMetrum # @@ -108,6 +121,14 @@ TM_SRC = \ $(TM_TASK_SRC) \ $(TM_MAIN_SRC) +TM_SIRF_SRC = \ + $(TM_SRC) \ + $(TM_SIRF_DRIVER_SRC) + +TM_SKY_SRC = \ + $(TM_SRC) \ + $(TM_SKY_DRIVER_SRC) + TI_MAIN_SRC = \ ao_tidongle.c @@ -161,13 +182,16 @@ SRC = \ $(TELE_COMMON_SRC) \ $(TELE_FAKE_SRC) \ $(TM_DRIVER_SRC) \ + $(TM_SIRF_DRIVER_SRC) \ + $(TM_SKY_DRIVER_SRC) \ $(TM_TASK_SRC) \ $(TM_MAIN_SRC) \ $(TI_MAIN_SRC) \ $(TD_MAIN_SRC) \ $(TT_MAIN_SRC) -TM_REL=$(TM_SRC:.c=.rel) ao_product-telemetrum.rel +TM_SIRF_REL=$(TM_SIRF_SRC:.c=.rel) ao_product-telemetrum.rel +TM_SKY_REL=$(TM_SKY_SRC:.c=.rel) ao_product-telemetrum.rel TI_REL=$(TI_SRC:.c=.rel) ao_product-tidongle.rel TT_REL=$(TT_SRC:.c=.rel) ao_product-teleterra.rel TD_REL=$(TD_SRC:.c=.rel) ao_product-teledongle.rel @@ -186,10 +210,10 @@ LST=$(REL:.rel=.lst) RST=$(REL:.rel=.rst) SYM=$(REL:.rel=.sym) -PROGS= telemetrum.ihx tidongle.ihx \ +PROGS= telemetrum-sirf.ihx telemetrum-sky.ihx tidongle.ihx \ teleterra.ihx teledongle.ihx -HOST_PROGS=ao_flight_test ao_gps_test +HOST_PROGS=ao_flight_test ao_gps_test ao_gps_test_skytraq PCDB=$(PROGS:.ihx=.cdb) PLNK=$(PROGS:.ihx=.lnk) @@ -202,15 +226,21 @@ PAOM=$(PROGS:.ihx=) all: $(PROGS) $(HOST_PROGS) -telemetrum.ihx: $(TM_REL) Makefile - $(CC) $(LDFLAGS) $(CFLAGS) -o $@ $(TM_REL) - sh check-stack ao.h telemetrum.mem +telemetrum-sirf.ihx: $(TM_SIRF_REL) Makefile + $(CC) $(LDFLAGS) $(CFLAGS) -o $@ $(TM_SIRF_REL) + sh check-stack ao.h telemetrum-sirf.mem + +telemetrum-sky.ihx: $(TM_SKY_REL) Makefile + $(CC) $(LDFLAGS) $(CFLAGS) -o $@ $(TM_SKY_REL) + sh check-stack ao.h telemetrum-sky.mem + +telemetrum-sky.ihx: telemetrum-sirf.ihx tidongle.ihx: $(TI_REL) Makefile $(CC) $(LDFLAGS) $(CFLAGS) -o $@ $(TI_REL) sh check-stack ao.h tidongle.mem -tidongle.ihx: telemetrum.ihx +tidongle.ihx: telemetrum-sky.ihx teleterra.ihx: $(TT_REL) Makefile $(CC) $(LDFLAGS) $(CFLAGS) -o $@ $(TT_REL) @@ -271,5 +301,8 @@ install: ao_flight_test: ao_flight.c ao_flight_test.c ao_host.h cc -g -o $@ ao_flight_test.c -ao_gps_test: ao_gps.c ao_gps_test.c ao_gps_print.c ao_host.h +ao_gps_test: ao_gps_sirf.c ao_gps_test.c ao_gps_print.c ao_host.h cc -g -o $@ ao_gps_test.c + +ao_gps_test_skytraq: ao_gps_skytraq.c ao_gps_test_skytraq.c ao_gps_print.c ao_host.h + cc -g -o $@ ao_gps_test_skytraq.c diff --git a/src/ao.h b/src/ao.h index f89568f2..4cceefe1 100644 --- a/src/ao.h +++ b/src/ao.h @@ -40,6 +40,7 @@ /* An AltOS task */ struct ao_task { __xdata void *wchan; /* current wait channel (NULL if running) */ + uint16_t alarm; /* abort ao_sleep time */ uint8_t stack_count; /* amount of saved stack */ uint8_t task_id; /* index in the task array */ __code char *name; /* task name */ @@ -55,14 +56,26 @@ extern __xdata struct ao_task *__data ao_cur_task; ao_task.c */ -/* Suspend the current task until wchan is awoken */ -void +/* Suspend the current task until wchan is awoken. + * returns: + * 0 on normal wake + * 1 on alarm + */ +uint8_t ao_sleep(__xdata void *wchan); /* Wake all tasks sleeping on wchan */ void ao_wakeup(__xdata void *wchan); +/* Wake up a specific task */ +void +ao_wake_task(__xdata struct ao_task *task); + +/* set an alarm to go off in 'delay' ticks */ +void +ao_alarm(uint16_t delay); + /* Yield the processor to another task */ void ao_yield(void) _naked; @@ -71,6 +84,10 @@ ao_yield(void) _naked; void ao_add_task(__xdata struct ao_task * task, void (*start)(void), __code char *name) __reentrant; +/* Terminate the current task */ +void +ao_exit(void); + /* Dump task info to console */ void ao_task_info(void); @@ -89,6 +106,7 @@ ao_start_scheduler(void); #define AO_PANIC_EE 4 /* Mis-using eeprom API */ #define AO_PANIC_LOG 5 /* Failing to read/write log data */ #define AO_PANIC_CMD 6 /* Too many command sets registered */ +#define AO_PANIC_STDIO 7 /* Too many stdio handlers registered */ /* Stop the operating system, beeping and blinking the reason */ void @@ -130,7 +148,7 @@ ao_clock_init(void); * ao_adc.c */ -#define AO_ADC_RING 64 +#define AO_ADC_RING 32 #define ao_adc_ring_next(n) (((n) + 1) & (AO_ADC_RING - 1)) #define ao_adc_ring_prev(n) (((n) - 1) & (AO_ADC_RING - 1)) @@ -273,6 +291,12 @@ ao_usb_putchar(char c); char ao_usb_getchar(void); +/* Poll for a charcter on the USB input queue. + * returns AO_READ_AGAIN if none are available + */ +char +ao_usb_pollchar(void); + /* Flush the USB output queue */ void ao_usb_flush(void); @@ -341,9 +365,14 @@ ao_cmd_init(void); * ao_dma.c */ -/* Allocate a DMA channel. the 'done' parameter will be set to 1 - * when the dma is finished and will be used to wakeup any waiters +/* Allocate a DMA channel. the 'done' parameter will be set + * when the dma is finished or aborted and will be used to + * wakeup any waiters */ + +#define AO_DMA_DONE 1 +#define AO_DMA_ABORTED 2 + uint8_t ao_dma_alloc(__xdata uint8_t * done); @@ -668,7 +697,8 @@ void ao_serial_putchar(char c) __critical; #define AO_SERIAL_SPEED_4800 0 -#define AO_SERIAL_SPEED_57600 1 +#define AO_SERIAL_SPEED_9600 1 +#define AO_SERIAL_SPEED_57600 2 void ao_serial_set_speed(uint8_t speed); @@ -787,6 +817,23 @@ ao_telemetry_init(void); * ao_radio.c */ +extern __xdata uint8_t ao_radio_dma; +extern __xdata uint8_t ao_radio_dma_done; +extern __xdata uint8_t ao_radio_done; +extern __xdata uint8_t ao_radio_mutex; + +void +ao_radio_general_isr(void) interrupt 16; + +void +ao_radio_set_telemetry(void); + +void +ao_radio_set_packet(void); + +void +ao_radio_set_rdf(void); + void ao_radio_send(__xdata struct ao_telemetry *telemetry) __reentrant; @@ -796,15 +843,21 @@ struct ao_radio_recv { uint8_t status; }; -void +uint8_t ao_radio_recv(__xdata struct ao_radio_recv *recv) __reentrant; void ao_radio_rdf(int ms); +void +ao_radio_abort(void); + void ao_radio_rdf_abort(void); +void +ao_radio_idle(void); + void ao_radio_init(void); @@ -827,9 +880,24 @@ ao_monitor_init(uint8_t led, uint8_t monitoring) __reentrant; * ao_stdio.c */ +#define AO_READ_AGAIN ((char) -1) + +struct ao_stdio { + char (*pollchar)(void); + void (*putchar)(char c) __reentrant; + void (*flush)(void); +}; + void flush(void); +extern __xdata uint8_t ao_stdin_ready; + +void +ao_add_stdio(char (*pollchar)(void), + void (*putchar)(char) __reentrant, + void (*flush)(void)); + /* * ao_ignite.c */ @@ -934,37 +1002,59 @@ struct ao_fifo { * Packet-based command interface */ -#define AO_PACKET_MAX 32 -#define AO_PACKET_WIN 256 - -#define AO_PACKET_FIN (1 << 0) -#define AO_PACKET_SYN (1 << 1) -#define AO_PACKET_RST (1 << 2) -#define AO_PACKET_ACK (1 << 3) +#define AO_PACKET_MAX 8 +#define AO_PACKET_SYN (uint8_t) 0xff struct ao_packet { uint8_t addr; - uint8_t flags; - uint16_t seq; - uint16_t ack; - uint16_t window; uint8_t len; + uint8_t seq; + uint8_t ack; uint8_t d[AO_PACKET_MAX]; }; -uint8_t -ao_packet_connect(uint8_t dest); +struct ao_packet_recv { + struct ao_packet packet; + int8_t rssi; + uint8_t status; +}; + +extern __xdata struct ao_packet_recv ao_rx_packet; +extern __xdata struct ao_packet ao_tx_packet; +extern __xdata struct ao_task ao_packet_task; +extern __xdata uint8_t ao_packet_enable; +extern __xdata uint8_t ao_packet_master_sleeping; +extern __pdata uint8_t ao_packet_rx_len, ao_packet_rx_used, ao_packet_tx_used; + +void +ao_packet_send(void); uint8_t -ao_packet_accept(void); +ao_packet_recv(void); + +void +ao_packet_flush(void); + +void +ao_packet_putchar(char c) __reentrant; + +char +ao_packet_pollchar(void) __critical; -int -ao_packet_send(uint8_t *data, int len); +/* ao_packet_master.c */ -int -ao_packet_recv(uint8_t *data, int len); +void +ao_packet_master_init(void); + +/* ao_packet_slave.c */ + +void +ao_packet_slave_start(void); + +void +ao_packet_slave_stop(void); void -ao_packet_init(void); +ao_packet_slave_init(void); #endif /* _AO_H_ */ diff --git a/src/ao_adc.c b/src/ao_adc.c index 26209dcf..d9672671 100644 --- a/src/ao_adc.c +++ b/src/ao_adc.c @@ -61,9 +61,9 @@ ao_adc_isr(void) interrupt 1 } static void -ao_adc_dump(void) +ao_adc_dump(void) __reentrant { - __xdata struct ao_adc packet; + static __xdata struct ao_adc packet; ao_adc_get(&packet); printf("tick: %5u accel: %4d pres: %4d temp: %4d batt: %4d drogue: %4d main: %4d\n", packet.tick, packet.accel >> 4, packet.pres >> 4, packet.temp >> 4, diff --git a/src/ao_dma.c b/src/ao_dma.c index a4d45f14..110138b5 100644 --- a/src/ao_dma.c +++ b/src/ao_dma.c @@ -107,6 +107,8 @@ ao_dma_abort(uint8_t id) uint8_t mask = (1 << id); DMAARM = 0x80 | mask; DMAIRQ &= ~mask; + *(ao_dma_done[id]) |= AO_DMA_ABORTED; + ao_wakeup(ao_dma_done[id]); } void @@ -122,7 +124,7 @@ ao_dma_isr(void) interrupt 8 DMAIF = 0; /* Clear the completed ID */ DMAIRQ = ~mask; - *(ao_dma_done[id]) = 1; + *(ao_dma_done[id]) |= AO_DMA_DONE; ao_wakeup(ao_dma_done[id]); break; } diff --git a/src/ao_ee.c b/src/ao_ee.c index 9b6db234..26cfb7fd 100644 --- a/src/ao_ee.c +++ b/src/ao_ee.c @@ -351,11 +351,11 @@ ao_ee_read_config(uint8_t *buf, uint16_t len) __reentrant } static void -ee_dump(void) +ee_dump(void) __reentrant { - __xdata uint8_t b; - __xdata uint16_t block; - __xdata uint8_t i; + uint8_t b; + uint16_t block; + uint8_t i; ao_cmd_hex(); block = ao_cmd_lex_i; @@ -377,13 +377,13 @@ ee_dump(void) } static void -ee_store(void) +ee_store(void) __reentrant { - __xdata uint16_t block; - __xdata uint8_t i; - __xdata uint16_t len; - __xdata uint8_t b; - __xdata uint32_t addr; + uint16_t block; + uint8_t i; + uint16_t len; + uint8_t b; + uint32_t addr; ao_cmd_hex(); block = ao_cmd_lex_i; diff --git a/src/ao_flight.c b/src/ao_flight.c index ec89e7c2..e91a5daa 100644 --- a/src/ao_flight.c +++ b/src/ao_flight.c @@ -235,9 +235,9 @@ ao_flight(void) } else { ao_flight_state = ao_flight_idle; - /* Turn on the Green LED in idle mode + /* Turn on packet system in idle mode */ - ao_led_on(AO_LED_GREEN); + ao_packet_slave_start(); ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); } /* signal successful initialization by turning off the LED */ @@ -458,7 +458,7 @@ ao_flight(void) #define AO_VEL_COUNT_TO_MS(count) ((int16_t) ((count) / 2700)) static void -ao_flight_status(void) +ao_flight_status(void) __reentrant { printf("STATE: %7s accel: %d speed: %d altitude: %d main: %d\n", ao_state_names[ao_flight_state], diff --git a/src/ao_flight_test.c b/src/ao_flight_test.c index 9fcb00c2..83c63016 100644 --- a/src/ao_flight_test.c +++ b/src/ao_flight_test.c @@ -69,6 +69,7 @@ uint8_t ao_adc_head; #define ao_usb_disable() #define ao_telemetry_set_interval(x) #define ao_rdf_set(rdf) +#define ao_packet_slave_start() enum ao_igniter { ao_igniter_drogue = 0, diff --git a/src/ao_gps.c b/src/ao_gps.c deleted file mode 100644 index 2b3a5178..00000000 --- a/src/ao_gps.c +++ /dev/null @@ -1,457 +0,0 @@ -/* - * Copyright © 2009 Keith Packard - * - * 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. - */ - -#ifndef AO_GPS_TEST -#include "ao.h" -#endif - -__xdata uint8_t ao_gps_mutex; -__xdata struct ao_gps_data ao_gps_data; -__xdata struct ao_gps_tracking_data ao_gps_tracking_data; - -static const char ao_gps_set_nmea[] = "\r\n$PSRF100,0,57600,8,1,0*37\r\n"; - -const char ao_gps_config[] = { - - 0xa0, 0xa2, 0x00, 0x0e, /* length: 14 bytes */ - 136, /* mode control */ - 0, 0, /* reserved */ - 0, /* degraded mode (allow 1-SV navigation) */ - 0, 0, /* reserved */ - 0, 0, /* user specified altitude */ - 2, /* alt hold mode (disabled, require 3d fixes) */ - 0, /* alt hold source (use last computed altitude) */ - 0, /* reserved */ - 10, /* Degraded time out (10 sec) */ - 10, /* Dead Reckoning time out (10 sec) */ - 0, /* Track smoothing (disabled) */ - 0x00, 0x8e, 0xb0, 0xb3, - - 0xa0, 0xa2, 0x00, 0x08, /* length: 8 bytes */ - 166, /* Set message rate */ - 2, /* enable/disable all messages */ - 0, /* message id (ignored) */ - 0, /* update rate (0 = disable) */ - 0, 0, 0, 0, /* reserved */ - 0x00, 0xa8, 0xb0, 0xb3, - - 0xa0, 0xa2, 0x00, 0x02, /* length: 2 bytes */ - 143, /* static navigation */ - 0, /* disable */ - 0x00, 0x8f, 0xb0, 0xb3, -}; - -#define NAV_TYPE_GPS_FIX_TYPE_MASK (7 << 0) -#define NAV_TYPE_NO_FIX (0 << 0) -#define NAV_TYPE_SV_KF (1 << 0) -#define NAV_TYPE_2_SV_KF (2 << 0) -#define NAV_TYPE_3_SV_KF (3 << 0) -#define NAV_TYPE_4_SV_KF (4 << 0) -#define NAV_TYPE_2D_LEAST_SQUARES (5 << 0) -#define NAV_TYPE_3D_LEAST_SQUARES (6 << 0) -#define NAV_TYPE_DR (7 << 0) -#define NAV_TYPE_TRICKLE_POWER (1 << 3) -#define NAV_TYPE_ALTITUDE_HOLD_MASK (3 << 4) -#define NAV_TYPE_ALTITUDE_HOLD_NONE (0 << 4) -#define NAV_TYPE_ALTITUDE_HOLD_KF (1 << 4) -#define NAV_TYPE_ALTITUDE_HOLD_USER (2 << 4) -#define NAV_TYPE_ALTITUDE_HOLD_ALWAYS (3 << 4) -#define NAV_TYPE_DOP_LIMIT_EXCEEDED (1 << 6) -#define NAV_TYPE_DGPS_APPLIED (1 << 7) -#define NAV_TYPE_SENSOR_DR (1 << 8) -#define NAV_TYPE_OVERDETERMINED (1 << 9) -#define NAV_TYPE_DR_TIMEOUT_EXCEEDED (1 << 10) -#define NAV_TYPE_FIX_MI_EDIT (1 << 11) -#define NAV_TYPE_INVALID_VELOCITY (1 << 12) -#define NAV_TYPE_ALTITUDE_HOLD_DISABLED (1 << 13) -#define NAV_TYPE_DR_ERROR_STATUS_MASK (3 << 14) -#define NAV_TYPE_DR_ERROR_STATUS_GPS_ONLY (0 << 14) -#define NAV_TYPE_DR_ERROR_STATUS_DR_FROM_GPS (1 << 14) -#define NAV_TYPE_DR_ERROR_STATUS_DR_SENSOR_ERROR (2 << 14) -#define NAV_TYPE_DR_ERROR_STATUS_DR_IN_TEST (3 << 14) - -struct sirf_geodetic_nav_data { - uint16_t nav_type; - uint16_t utc_year; - uint8_t utc_month; - uint8_t utc_day; - uint8_t utc_hour; - uint8_t utc_minute; - uint16_t utc_second; - int32_t lat; - int32_t lon; - int32_t alt_msl; - uint16_t ground_speed; - uint16_t course; - int16_t climb_rate; - uint32_t h_error; - uint32_t v_error; - uint8_t num_sv; - uint8_t hdop; -}; - -static __xdata struct sirf_geodetic_nav_data ao_sirf_data; - -struct sirf_measured_sat_data { - uint8_t svid; - uint16_t state; - uint8_t c_n_1; -}; - -struct sirf_measured_tracker_data { - int16_t gps_week; - uint32_t gps_tow; - uint8_t channels; - struct sirf_measured_sat_data sats[12]; -}; - -static __xdata struct sirf_measured_tracker_data ao_sirf_tracker_data; - -static __pdata uint16_t ao_sirf_cksum; -static __pdata uint16_t ao_sirf_len; - -#define ao_sirf_byte() ((uint8_t) ao_serial_getchar()) - -static uint8_t data_byte(void) -{ - uint8_t c = ao_sirf_byte(); - --ao_sirf_len; - ao_sirf_cksum += c; - return c; -} - -static char __xdata *sirf_target; - -static void sirf_u16(uint8_t offset) -{ - uint16_t __xdata *ptr = (uint16_t __xdata *) (sirf_target + offset); - uint16_t val; - - val = data_byte() << 8; - val |= data_byte (); - *ptr = val; -} - -static void sirf_u8(uint8_t offset) -{ - uint8_t __xdata *ptr = (uint8_t __xdata *) (sirf_target + offset); - uint8_t val; - - val = data_byte (); - *ptr = val; -} - -static void sirf_u32(uint8_t offset) __reentrant -{ - uint32_t __xdata *ptr = (uint32_t __xdata *) (sirf_target + offset); - uint32_t val; - - val = ((uint32_t) data_byte ()) << 24; - val |= ((uint32_t) data_byte ()) << 16; - val |= ((uint32_t) data_byte ()) << 8; - val |= ((uint32_t) data_byte ()); - *ptr = val; -} - -static void sirf_discard(uint8_t len) -{ - while (len--) - data_byte(); -} - -#define SIRF_END 0 -#define SIRF_DISCARD 1 -#define SIRF_U8 2 -#define SIRF_U16 3 -#define SIRF_U32 4 -#define SIRF_U8X10 5 - -struct sirf_packet_parse { - uint8_t type; - uint8_t offset; -}; - -static void -ao_sirf_parse(void __xdata *target, const struct sirf_packet_parse *parse) __reentrant -{ - uint8_t i, offset, j; - - sirf_target = target; - for (i = 0; ; i++) { - offset = parse[i].offset; - switch (parse[i].type) { - case SIRF_END: - return; - case SIRF_DISCARD: - sirf_discard(offset); - break; - case SIRF_U8: - sirf_u8(offset); - break; - case SIRF_U16: - sirf_u16(offset); - break; - case SIRF_U32: - sirf_u32(offset); - break; - case SIRF_U8X10: - for (j = 10; j--;) - sirf_u8(offset++); - break; - } - } -} - -static const struct sirf_packet_parse geodetic_nav_data_packet[] = { - { SIRF_DISCARD, 2 }, /* 1 nav valid */ - { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, nav_type) }, /* 3 */ - { SIRF_DISCARD, 6 }, /* 5 week number, time of week */ - { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_year) }, /* 11 */ - { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_month) }, /* 13 */ - { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_day) }, /* 14 */ - { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_hour) }, /* 15 */ - { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_minute) }, /* 16 */ - { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_second) }, /* 17 */ - { SIRF_DISCARD, 4 }, /* satellite id list */ /* 19 */ - { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lat) }, /* 23 */ - { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lon) }, /* 27 */ - { SIRF_DISCARD, 4 }, /* altitude from ellipsoid */ /* 31 */ - { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, alt_msl) }, /* 35 */ - { SIRF_DISCARD, 1 }, /* map datum */ /* 39 */ - { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, ground_speed) }, /* 40 */ - { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, course) }, /* 42 */ - { SIRF_DISCARD, 2 }, /* magnetic variation */ /* 44 */ - { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, climb_rate) }, /* 46 */ - { SIRF_DISCARD, 2 }, /* turn rate */ /* 48 */ - { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, h_error) }, /* 50 */ - { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, v_error) }, /* 54 */ - { SIRF_DISCARD, 30 }, /* time error, h_vel error, clock_bias, - clock bias error, clock drift, - clock drift error, distance, - distance error, heading error */ /* 58 */ - { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, num_sv) }, /* 88 */ - { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, hdop) }, /* 89 */ - { SIRF_DISCARD, 1 }, /* additional mode info */ /* 90 */ - { SIRF_END, 0 }, /* 91 */ -}; - -static void -ao_sirf_parse_41(void) __reentrant -{ - ao_sirf_parse(&ao_sirf_data, geodetic_nav_data_packet); -} - -static const struct sirf_packet_parse measured_tracker_data_packet[] = { - { SIRF_U16, offsetof (struct sirf_measured_tracker_data, gps_week) }, /* 1 week */ - { SIRF_U32, offsetof (struct sirf_measured_tracker_data, gps_tow) }, /* 3 time of week */ - { SIRF_U8, offsetof (struct sirf_measured_tracker_data, channels) }, /* 7 channels */ - { SIRF_END, 0 }, -}; - -static const struct sirf_packet_parse measured_sat_data_packet[] = { - { SIRF_U8, offsetof (struct sirf_measured_sat_data, svid) }, /* 0 SV id */ - { SIRF_DISCARD, 2 }, /* 1 azimuth, 2 elevation */ - { SIRF_U16, offsetof (struct sirf_measured_sat_data, state) }, /* 2 state */ - { SIRF_U8, offsetof (struct sirf_measured_sat_data, c_n_1) }, /* C/N0 1 */ - { SIRF_DISCARD, 9 }, /* C/N0 2-10 */ - { SIRF_END, 0 }, -}; - -static void -ao_sirf_parse_4(void) __reentrant -{ - uint8_t i; - ao_sirf_parse(&ao_sirf_tracker_data, measured_tracker_data_packet); - for (i = 0; i < 12; i++) - ao_sirf_parse(&ao_sirf_tracker_data.sats[i], measured_sat_data_packet); -} - -static void -ao_gps_setup(void) __reentrant -{ - uint8_t i, k; - ao_serial_set_speed(AO_SERIAL_SPEED_4800); - for (i = 0; i < 64; i++) - ao_serial_putchar(0x00); - for (k = 0; k < 3; k++) - for (i = 0; i < sizeof (ao_gps_set_nmea); i++) - ao_serial_putchar(ao_gps_set_nmea[i]); - ao_serial_set_speed(AO_SERIAL_SPEED_57600); - for (i = 0; i < 64; i++) - ao_serial_putchar(0x00); -} - -static const char ao_gps_set_message_rate[] = { - 0xa0, 0xa2, 0x00, 0x08, - 166, - 0, -}; - -void -ao_sirf_set_message_rate(uint8_t msg, uint8_t rate) -{ - uint16_t cksum = 0x00a6; - uint8_t i; - - for (i = 0; i < sizeof (ao_gps_set_message_rate); i++) - ao_serial_putchar(ao_gps_set_message_rate[i]); - ao_serial_putchar(msg); - ao_serial_putchar(rate); - cksum = 0xa6 + msg + rate; - for (i = 0; i < 4; i++) - ao_serial_putchar(0); - ao_serial_putchar((cksum >> 8) & 0x7f); - ao_serial_putchar(cksum & 0xff); - ao_serial_putchar(0xb0); - ao_serial_putchar(0xb3); -} - -static const uint8_t sirf_disable[] = { - 2, - 9, - 10, - 27, - 50, - 52, -}; - -void -ao_gps(void) __reentrant -{ - uint8_t i, k; - uint16_t cksum; - - ao_gps_setup(); - 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); - ao_sirf_set_message_rate(4, 1); - } - for (;;) { - /* Locate the begining of the next record */ - while (ao_sirf_byte() != (uint8_t) 0xa0) - ; - if (ao_sirf_byte() != (uint8_t) 0xa2) - continue; - - /* Length */ - ao_sirf_len = ao_sirf_byte() << 8; - ao_sirf_len |= ao_sirf_byte(); - if (ao_sirf_len > 1023) - continue; - - ao_sirf_cksum = 0; - - /* message ID */ - i = data_byte (); /* 0 */ - - switch (i) { - case 41: - if (ao_sirf_len < 90) - break; - ao_sirf_parse_41(); - break; - case 4: - if (ao_sirf_len < 187) - break; - ao_sirf_parse_4(); - break; - } - if (ao_sirf_len != 0) - continue; - - /* verify checksum and end sequence */ - ao_sirf_cksum &= 0x7fff; - cksum = ao_sirf_byte() << 8; - cksum |= ao_sirf_byte(); - if (ao_sirf_cksum != cksum) - continue; - if (ao_sirf_byte() != (uint8_t) 0xb0) - continue; - if (ao_sirf_byte() != (uint8_t) 0xb3) - continue; - - switch (i) { - case 41: - ao_mutex_get(&ao_gps_mutex); - ao_gps_data.hour = ao_sirf_data.utc_hour; - ao_gps_data.minute = ao_sirf_data.utc_minute; - ao_gps_data.second = ao_sirf_data.utc_second / 1000; - ao_gps_data.flags = ((ao_sirf_data.num_sv << AO_GPS_NUM_SAT_SHIFT) & AO_GPS_NUM_SAT_MASK) | AO_GPS_RUNNING; - if ((ao_sirf_data.nav_type & NAV_TYPE_GPS_FIX_TYPE_MASK) >= NAV_TYPE_4_SV_KF) - ao_gps_data.flags |= AO_GPS_VALID; - ao_gps_data.latitude = ao_sirf_data.lat; - ao_gps_data.longitude = ao_sirf_data.lon; - ao_gps_data.altitude = ao_sirf_data.alt_msl / 100; - ao_gps_data.ground_speed = ao_sirf_data.ground_speed; - ao_gps_data.course = ao_sirf_data.course / 200; - ao_gps_data.hdop = ao_sirf_data.hdop; - ao_gps_data.climb_rate = ao_sirf_data.climb_rate; - if (ao_sirf_data.h_error > 6553500) - ao_gps_data.h_error = 65535; - else - ao_gps_data.h_error = ao_sirf_data.h_error / 100; - if (ao_sirf_data.v_error > 6553500) - ao_gps_data.v_error = 65535; - else - ao_gps_data.v_error = ao_sirf_data.v_error / 100; - ao_mutex_put(&ao_gps_mutex); - ao_wakeup(&ao_gps_data); - break; - case 4: - ao_mutex_get(&ao_gps_mutex); - ao_gps_tracking_data.channels = ao_sirf_tracker_data.channels; - for (i = 0; i < 12; i++) { - ao_gps_tracking_data.sats[i].svid = ao_sirf_tracker_data.sats[i].svid; - ao_gps_tracking_data.sats[i].state = (uint8_t) ao_sirf_tracker_data.sats[i].state; - ao_gps_tracking_data.sats[i].c_n_1 = ao_sirf_tracker_data.sats[i].c_n_1; - } - ao_mutex_put(&ao_gps_mutex); - ao_wakeup(&ao_gps_tracking_data); - break; - } - } -} - -__xdata struct ao_task ao_gps_task; - -static void -gps_dump(void) __reentrant -{ - ao_mutex_get(&ao_gps_mutex); - ao_gps_print(&ao_gps_data); - putchar('\n'); - ao_gps_tracking_print(&ao_gps_tracking_data); - putchar('\n'); - ao_mutex_put(&ao_gps_mutex); -} - -__code struct ao_cmds ao_gps_cmds[] = { - { 'g', gps_dump, "g Display current GPS values" }, - { 0, gps_dump, NULL }, -}; - -void -ao_gps_init(void) -{ - ao_add_task(&ao_gps_task, ao_gps, "gps"); - ao_cmd_register(&ao_gps_cmds[0]); -} diff --git a/src/ao_gps_sirf.c b/src/ao_gps_sirf.c new file mode 100644 index 00000000..58438760 --- /dev/null +++ b/src/ao_gps_sirf.c @@ -0,0 +1,457 @@ +/* + * Copyright © 2009 Keith Packard + * + * 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. + */ + +#ifndef AO_GPS_TEST +#include "ao.h" +#endif + +__xdata uint8_t ao_gps_mutex; +__xdata struct ao_gps_data ao_gps_data; +__xdata struct ao_gps_tracking_data ao_gps_tracking_data; + +static const char ao_gps_set_nmea[] = "\r\n$PSRF100,0,57600,8,1,0*37\r\n"; + +const char ao_gps_config[] = { + + 0xa0, 0xa2, 0x00, 0x0e, /* length: 14 bytes */ + 136, /* mode control */ + 0, 0, /* reserved */ + 0, /* degraded mode (allow 1-SV navigation) */ + 0, 0, /* reserved */ + 0, 0, /* user specified altitude */ + 2, /* alt hold mode (disabled, require 3d fixes) */ + 0, /* alt hold source (use last computed altitude) */ + 0, /* reserved */ + 10, /* Degraded time out (10 sec) */ + 10, /* Dead Reckoning time out (10 sec) */ + 0, /* Track smoothing (disabled) */ + 0x00, 0x8e, 0xb0, 0xb3, + + 0xa0, 0xa2, 0x00, 0x08, /* length: 8 bytes */ + 166, /* Set message rate */ + 2, /* enable/disable all messages */ + 0, /* message id (ignored) */ + 0, /* update rate (0 = disable) */ + 0, 0, 0, 0, /* reserved */ + 0x00, 0xa8, 0xb0, 0xb3, + + 0xa0, 0xa2, 0x00, 0x02, /* length: 2 bytes */ + 143, /* static navigation */ + 0, /* disable */ + 0x00, 0x8f, 0xb0, 0xb3, +}; + +#define NAV_TYPE_GPS_FIX_TYPE_MASK (7 << 0) +#define NAV_TYPE_NO_FIX (0 << 0) +#define NAV_TYPE_SV_KF (1 << 0) +#define NAV_TYPE_2_SV_KF (2 << 0) +#define NAV_TYPE_3_SV_KF (3 << 0) +#define NAV_TYPE_4_SV_KF (4 << 0) +#define NAV_TYPE_2D_LEAST_SQUARES (5 << 0) +#define NAV_TYPE_3D_LEAST_SQUARES (6 << 0) +#define NAV_TYPE_DR (7 << 0) +#define NAV_TYPE_TRICKLE_POWER (1 << 3) +#define NAV_TYPE_ALTITUDE_HOLD_MASK (3 << 4) +#define NAV_TYPE_ALTITUDE_HOLD_NONE (0 << 4) +#define NAV_TYPE_ALTITUDE_HOLD_KF (1 << 4) +#define NAV_TYPE_ALTITUDE_HOLD_USER (2 << 4) +#define NAV_TYPE_ALTITUDE_HOLD_ALWAYS (3 << 4) +#define NAV_TYPE_DOP_LIMIT_EXCEEDED (1 << 6) +#define NAV_TYPE_DGPS_APPLIED (1 << 7) +#define NAV_TYPE_SENSOR_DR (1 << 8) +#define NAV_TYPE_OVERDETERMINED (1 << 9) +#define NAV_TYPE_DR_TIMEOUT_EXCEEDED (1 << 10) +#define NAV_TYPE_FIX_MI_EDIT (1 << 11) +#define NAV_TYPE_INVALID_VELOCITY (1 << 12) +#define NAV_TYPE_ALTITUDE_HOLD_DISABLED (1 << 13) +#define NAV_TYPE_DR_ERROR_STATUS_MASK (3 << 14) +#define NAV_TYPE_DR_ERROR_STATUS_GPS_ONLY (0 << 14) +#define NAV_TYPE_DR_ERROR_STATUS_DR_FROM_GPS (1 << 14) +#define NAV_TYPE_DR_ERROR_STATUS_DR_SENSOR_ERROR (2 << 14) +#define NAV_TYPE_DR_ERROR_STATUS_DR_IN_TEST (3 << 14) + +struct sirf_geodetic_nav_data { + uint16_t nav_type; + uint16_t utc_year; + uint8_t utc_month; + uint8_t utc_day; + uint8_t utc_hour; + uint8_t utc_minute; + uint16_t utc_second; + int32_t lat; + int32_t lon; + int32_t alt_msl; + uint16_t ground_speed; + uint16_t course; + int16_t climb_rate; + uint32_t h_error; + uint32_t v_error; + uint8_t num_sv; + uint8_t hdop; +}; + +static __xdata struct sirf_geodetic_nav_data ao_sirf_data; + +struct sirf_measured_sat_data { + uint8_t svid; + uint16_t state; + uint8_t c_n_1; +}; + +struct sirf_measured_tracker_data { + int16_t gps_week; + uint32_t gps_tow; + uint8_t channels; + struct sirf_measured_sat_data sats[12]; +}; + +static __xdata struct sirf_measured_tracker_data ao_sirf_tracker_data; + +static __pdata uint16_t ao_sirf_cksum; +static __pdata uint16_t ao_sirf_len; + +#define ao_sirf_byte() ((uint8_t) ao_serial_getchar()) + +static uint8_t data_byte(void) +{ + uint8_t c = ao_sirf_byte(); + --ao_sirf_len; + ao_sirf_cksum += c; + return c; +} + +static char __xdata *sirf_target; + +static void sirf_u16(uint8_t offset) +{ + uint16_t __xdata *ptr = (uint16_t __xdata *) (sirf_target + offset); + uint16_t val; + + val = data_byte() << 8; + val |= data_byte (); + *ptr = val; +} + +static void sirf_u8(uint8_t offset) +{ + uint8_t __xdata *ptr = (uint8_t __xdata *) (sirf_target + offset); + uint8_t val; + + val = data_byte (); + *ptr = val; +} + +static void sirf_u32(uint8_t offset) __reentrant +{ + uint32_t __xdata *ptr = (uint32_t __xdata *) (sirf_target + offset); + uint32_t val; + + val = ((uint32_t) data_byte ()) << 24; + val |= ((uint32_t) data_byte ()) << 16; + val |= ((uint32_t) data_byte ()) << 8; + val |= ((uint32_t) data_byte ()); + *ptr = val; +} + +static void sirf_discard(uint8_t len) +{ + while (len--) + data_byte(); +} + +#define SIRF_END 0 +#define SIRF_DISCARD 1 +#define SIRF_U8 2 +#define SIRF_U16 3 +#define SIRF_U32 4 +#define SIRF_U8X10 5 + +struct sirf_packet_parse { + uint8_t type; + uint8_t offset; +}; + +static void +ao_sirf_parse(void __xdata *target, const struct sirf_packet_parse *parse) __reentrant +{ + uint8_t i, offset, j; + + sirf_target = target; + for (i = 0; ; i++) { + offset = parse[i].offset; + switch (parse[i].type) { + case SIRF_END: + return; + case SIRF_DISCARD: + sirf_discard(offset); + break; + case SIRF_U8: + sirf_u8(offset); + break; + case SIRF_U16: + sirf_u16(offset); + break; + case SIRF_U32: + sirf_u32(offset); + break; + case SIRF_U8X10: + for (j = 10; j--;) + sirf_u8(offset++); + break; + } + } +} + +static const struct sirf_packet_parse geodetic_nav_data_packet[] = { + { SIRF_DISCARD, 2 }, /* 1 nav valid */ + { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, nav_type) }, /* 3 */ + { SIRF_DISCARD, 6 }, /* 5 week number, time of week */ + { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_year) }, /* 11 */ + { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_month) }, /* 13 */ + { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_day) }, /* 14 */ + { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_hour) }, /* 15 */ + { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, utc_minute) }, /* 16 */ + { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, utc_second) }, /* 17 */ + { SIRF_DISCARD, 4 }, /* satellite id list */ /* 19 */ + { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lat) }, /* 23 */ + { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, lon) }, /* 27 */ + { SIRF_DISCARD, 4 }, /* altitude from ellipsoid */ /* 31 */ + { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, alt_msl) }, /* 35 */ + { SIRF_DISCARD, 1 }, /* map datum */ /* 39 */ + { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, ground_speed) }, /* 40 */ + { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, course) }, /* 42 */ + { SIRF_DISCARD, 2 }, /* magnetic variation */ /* 44 */ + { SIRF_U16, offsetof(struct sirf_geodetic_nav_data, climb_rate) }, /* 46 */ + { SIRF_DISCARD, 2 }, /* turn rate */ /* 48 */ + { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, h_error) }, /* 50 */ + { SIRF_U32, offsetof(struct sirf_geodetic_nav_data, v_error) }, /* 54 */ + { SIRF_DISCARD, 30 }, /* time error, h_vel error, clock_bias, + clock bias error, clock drift, + clock drift error, distance, + distance error, heading error */ /* 58 */ + { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, num_sv) }, /* 88 */ + { SIRF_U8, offsetof(struct sirf_geodetic_nav_data, hdop) }, /* 89 */ + { SIRF_DISCARD, 1 }, /* additional mode info */ /* 90 */ + { SIRF_END, 0 }, /* 91 */ +}; + +static void +ao_sirf_parse_41(void) __reentrant +{ + ao_sirf_parse(&ao_sirf_data, geodetic_nav_data_packet); +} + +static const struct sirf_packet_parse measured_tracker_data_packet[] = { + { SIRF_U16, offsetof (struct sirf_measured_tracker_data, gps_week) }, /* 1 week */ + { SIRF_U32, offsetof (struct sirf_measured_tracker_data, gps_tow) }, /* 3 time of week */ + { SIRF_U8, offsetof (struct sirf_measured_tracker_data, channels) }, /* 7 channels */ + { SIRF_END, 0 }, +}; + +static const struct sirf_packet_parse measured_sat_data_packet[] = { + { SIRF_U8, offsetof (struct sirf_measured_sat_data, svid) }, /* 0 SV id */ + { SIRF_DISCARD, 2 }, /* 1 azimuth, 2 elevation */ + { SIRF_U16, offsetof (struct sirf_measured_sat_data, state) }, /* 2 state */ + { SIRF_U8, offsetof (struct sirf_measured_sat_data, c_n_1) }, /* C/N0 1 */ + { SIRF_DISCARD, 9 }, /* C/N0 2-10 */ + { SIRF_END, 0 }, +}; + +static void +ao_sirf_parse_4(void) __reentrant +{ + uint8_t i; + ao_sirf_parse(&ao_sirf_tracker_data, measured_tracker_data_packet); + for (i = 0; i < 12; i++) + ao_sirf_parse(&ao_sirf_tracker_data.sats[i], measured_sat_data_packet); +} + +static void +ao_gps_setup(void) __reentrant +{ + uint8_t i, k; + ao_serial_set_speed(AO_SERIAL_SPEED_4800); + for (i = 0; i < 64; i++) + ao_serial_putchar(0x00); + for (k = 0; k < 3; k++) + for (i = 0; i < sizeof (ao_gps_set_nmea); i++) + ao_serial_putchar(ao_gps_set_nmea[i]); + ao_serial_set_speed(AO_SERIAL_SPEED_57600); + for (i = 0; i < 64; i++) + ao_serial_putchar(0x00); +} + +static const char ao_gps_set_message_rate[] = { + 0xa0, 0xa2, 0x00, 0x08, + 166, + 0, +}; + +void +ao_sirf_set_message_rate(uint8_t msg, uint8_t rate) __reentrant +{ + uint16_t cksum = 0x00a6; + uint8_t i; + + for (i = 0; i < sizeof (ao_gps_set_message_rate); i++) + ao_serial_putchar(ao_gps_set_message_rate[i]); + ao_serial_putchar(msg); + ao_serial_putchar(rate); + cksum = 0xa6 + msg + rate; + for (i = 0; i < 4; i++) + ao_serial_putchar(0); + ao_serial_putchar((cksum >> 8) & 0x7f); + ao_serial_putchar(cksum & 0xff); + ao_serial_putchar(0xb0); + ao_serial_putchar(0xb3); +} + +static const uint8_t sirf_disable[] = { + 2, + 9, + 10, + 27, + 50, + 52, +}; + +void +ao_gps(void) __reentrant +{ + uint8_t i, k; + uint16_t cksum; + + ao_gps_setup(); + 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); + ao_sirf_set_message_rate(4, 1); + } + for (;;) { + /* Locate the begining of the next record */ + while (ao_sirf_byte() != (uint8_t) 0xa0) + ; + if (ao_sirf_byte() != (uint8_t) 0xa2) + continue; + + /* Length */ + ao_sirf_len = ao_sirf_byte() << 8; + ao_sirf_len |= ao_sirf_byte(); + if (ao_sirf_len > 1023) + continue; + + ao_sirf_cksum = 0; + + /* message ID */ + i = data_byte (); /* 0 */ + + switch (i) { + case 41: + if (ao_sirf_len < 90) + break; + ao_sirf_parse_41(); + break; + case 4: + if (ao_sirf_len < 187) + break; + ao_sirf_parse_4(); + break; + } + if (ao_sirf_len != 0) + continue; + + /* verify checksum and end sequence */ + ao_sirf_cksum &= 0x7fff; + cksum = ao_sirf_byte() << 8; + cksum |= ao_sirf_byte(); + if (ao_sirf_cksum != cksum) + continue; + if (ao_sirf_byte() != (uint8_t) 0xb0) + continue; + if (ao_sirf_byte() != (uint8_t) 0xb3) + continue; + + switch (i) { + case 41: + ao_mutex_get(&ao_gps_mutex); + ao_gps_data.hour = ao_sirf_data.utc_hour; + ao_gps_data.minute = ao_sirf_data.utc_minute; + ao_gps_data.second = ao_sirf_data.utc_second / 1000; + ao_gps_data.flags = ((ao_sirf_data.num_sv << AO_GPS_NUM_SAT_SHIFT) & AO_GPS_NUM_SAT_MASK) | AO_GPS_RUNNING; + if ((ao_sirf_data.nav_type & NAV_TYPE_GPS_FIX_TYPE_MASK) >= NAV_TYPE_4_SV_KF) + ao_gps_data.flags |= AO_GPS_VALID; + ao_gps_data.latitude = ao_sirf_data.lat; + ao_gps_data.longitude = ao_sirf_data.lon; + ao_gps_data.altitude = ao_sirf_data.alt_msl / 100; + ao_gps_data.ground_speed = ao_sirf_data.ground_speed; + ao_gps_data.course = ao_sirf_data.course / 200; + ao_gps_data.hdop = ao_sirf_data.hdop; + ao_gps_data.climb_rate = ao_sirf_data.climb_rate; + if (ao_sirf_data.h_error > 6553500) + ao_gps_data.h_error = 65535; + else + ao_gps_data.h_error = ao_sirf_data.h_error / 100; + if (ao_sirf_data.v_error > 6553500) + ao_gps_data.v_error = 65535; + else + ao_gps_data.v_error = ao_sirf_data.v_error / 100; + ao_mutex_put(&ao_gps_mutex); + ao_wakeup(&ao_gps_data); + break; + case 4: + ao_mutex_get(&ao_gps_mutex); + ao_gps_tracking_data.channels = ao_sirf_tracker_data.channels; + for (i = 0; i < 12; i++) { + ao_gps_tracking_data.sats[i].svid = ao_sirf_tracker_data.sats[i].svid; + ao_gps_tracking_data.sats[i].state = (uint8_t) ao_sirf_tracker_data.sats[i].state; + ao_gps_tracking_data.sats[i].c_n_1 = ao_sirf_tracker_data.sats[i].c_n_1; + } + ao_mutex_put(&ao_gps_mutex); + ao_wakeup(&ao_gps_tracking_data); + break; + } + } +} + +__xdata struct ao_task ao_gps_task; + +static void +gps_dump(void) __reentrant +{ + ao_mutex_get(&ao_gps_mutex); + ao_gps_print(&ao_gps_data); + putchar('\n'); + ao_gps_tracking_print(&ao_gps_tracking_data); + putchar('\n'); + ao_mutex_put(&ao_gps_mutex); +} + +__code struct ao_cmds ao_gps_cmds[] = { + { 'g', gps_dump, "g Display current GPS values" }, + { 0, gps_dump, NULL }, +}; + +void +ao_gps_init(void) +{ + ao_add_task(&ao_gps_task, ao_gps, "gps"); + ao_cmd_register(&ao_gps_cmds[0]); +} diff --git a/src/ao_gps_skytraq.c b/src/ao_gps_skytraq.c new file mode 100644 index 00000000..cd5f78b9 --- /dev/null +++ b/src/ao_gps_skytraq.c @@ -0,0 +1,375 @@ +/* + * Copyright © 2009 Keith Packard + * + * 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. + */ + +#ifndef AO_GPS_TEST +#include "ao.h" +#endif + +#define AO_GPS_LEADER 3 + +static const char ao_gps_header[] = "GPG"; + +__xdata uint8_t ao_gps_mutex; +static __xdata char ao_gps_char; +static __xdata uint8_t ao_gps_cksum; +static __xdata uint8_t ao_gps_error; + +__xdata struct ao_gps_data ao_gps_data; +__xdata struct ao_gps_tracking_data ao_gps_tracking_data; + +static __xdata struct ao_gps_data ao_gps_next; +static __xdata struct ao_gps_tracking_data ao_gps_tracking_next; + +static const char ao_gps_config[] = { + 0xa0, 0xa1, 0x00, 0x09, /* length 9 bytes */ + 0x08, /* configure nmea */ + 1, /* gga interval */ + 1, /* gsa interval */ + 1, /* gsv interval */ + 1, /* gll interval */ + 1, /* rmc interval */ + 1, /* vtg interval */ + 1, /* zda interval */ + 0, /* attributes (0 = update to sram, 1 = update flash too) */ + 0x09, 0x0d, 0x0a, + + 0xa0, 0xa1, 0x00, 0x03, /* length: 3 bytes */ + 0x3c, /* configure navigation mode */ + 0x00, /* 0 = car, 1 = pedestrian */ + 0x00, /* 0 = update to sram, 1 = update sram + flash */ + 0x3c, 0x0d, 0x0a, +}; + +static void +ao_gps_lexchar(void) +{ + if (ao_gps_error) + ao_gps_char = '\n'; + else + ao_gps_char = ao_serial_getchar(); + ao_gps_cksum ^= ao_gps_char; +} + +void +ao_gps_skip(void) +{ + while (ao_gps_char >= '0') + ao_gps_lexchar(); +} + +void +ao_gps_skip_field(void) +{ + while (ao_gps_char != ',' && ao_gps_char != '*' && ao_gps_char != '\n') + ao_gps_lexchar(); +} + +void +ao_gps_skip_sep(void) +{ + if (ao_gps_char == ',' || ao_gps_char == '.' || ao_gps_char == '*') + ao_gps_lexchar(); +} + +__xdata static uint8_t ao_gps_num_width; + +static int16_t +ao_gps_decimal(uint8_t max_width) +{ + int16_t v; + __xdata uint8_t neg = 0; + + ao_gps_skip_sep(); + if (ao_gps_char == '-') { + neg = 1; + ao_gps_lexchar(); + } + v = 0; + ao_gps_num_width = 0; + while (ao_gps_num_width < max_width) { + if (ao_gps_char < '0' || '9' < ao_gps_char) + break; + v = v * (int16_t) 10 + ao_gps_char - '0'; + ao_gps_num_width++; + ao_gps_lexchar(); + } + if (neg) + v = -v; + return v; +} + +static uint8_t +ao_gps_hex(uint8_t max_width) +{ + uint8_t v, d; + + ao_gps_skip_sep(); + v = 0; + ao_gps_num_width = 0; + while (ao_gps_num_width < max_width) { + if ('0' <= ao_gps_char && ao_gps_char <= '9') + d = ao_gps_char - '0'; + else if ('A' <= ao_gps_char && ao_gps_char <= 'F') + d = ao_gps_char - 'A' + 10; + else if ('a' <= ao_gps_char && ao_gps_char <= 'f') + d = ao_gps_char - 'a' + 10; + else + break; + v = (v << 4) | d; + ao_gps_num_width++; + ao_gps_lexchar(); + } + return v; +} + +static int32_t +ao_gps_parse_pos(uint8_t deg_width) __reentrant +{ + int32_t d; + int32_t m; + int32_t f; + + d = ao_gps_decimal(deg_width); + m = ao_gps_decimal(2); + if (ao_gps_char == '.') { + f = ao_gps_decimal(4); + while (ao_gps_num_width < 4) { + f *= 10; + ao_gps_num_width++; + } + } else { + f = 0; + if (ao_gps_char != ',') + ao_gps_error = 1; + } + d = d * 10000000l; + m = m * 10000l + f; + d = d + m * 50 / 3; + return d; +} + +static uint8_t +ao_gps_parse_flag(char no_c, char yes_c) __reentrant +{ + uint8_t ret = 0; + ao_gps_skip_sep(); + if (ao_gps_char == yes_c) + ret = 1; + else if (ao_gps_char == no_c) + ret = 0; + else + ao_gps_error = 1; + ao_gps_lexchar(); + return ret; +} + + +void +ao_gps(void) __reentrant +{ + char c; + uint8_t i; + + ao_serial_set_speed(AO_SERIAL_SPEED_9600); + for (i = 0; i < sizeof (ao_gps_config); i++) + ao_serial_putchar(ao_gps_config[i]); + for (;;) { + /* Locate the begining of the next record */ + for (;;) { + c = ao_serial_getchar(); + if (c == '$') + break; + } + + ao_gps_cksum = 0; + ao_gps_error = 0; + + /* Skip anything other than GPG */ + for (i = 0; i < AO_GPS_LEADER; i++) { + ao_gps_lexchar(); + if (ao_gps_char != ao_gps_header[i]) + break; + } + if (i != AO_GPS_LEADER) + continue; + + /* pull the record identifier characters off the link */ + ao_gps_lexchar(); + c = ao_gps_char; + ao_gps_lexchar(); + i = ao_gps_char; + ao_gps_lexchar(); + if (ao_gps_char != ',') + continue; + + if (c == (uint8_t) 'G' && i == (uint8_t) 'A') { + /* Now read the data into the gps data record + * + * $GPGGA,025149.000,4528.1723,N,12244.2480,W,1,05,2.0,103.5,M,-19.5,M,,0000*66 + * + * Essential fix data + * + * 025149.000 time (02:51:49.000 GMT) + * 4528.1723,N Latitude 45°28.1723' N + * 12244.2480,W Longitude 122°44.2480' W + * 1 Fix quality: + * 0 = invalid + * 1 = GPS fix (SPS) + * 2 = DGPS fix + * 3 = PPS fix + * 4 = Real Time Kinematic + * 5 = Float RTK + * 6 = estimated (dead reckoning) + * 7 = Manual input mode + * 8 = Simulation mode + * 05 Number of satellites (5) + * 2.0 Horizontal dilution + * 103.5,M Altitude, 103.5M above msl + * -19.5,M Height of geoid above WGS84 ellipsoid + * ? time in seconds since last DGPS update + * 0000 DGPS station ID + * *66 checksum + */ + + ao_gps_next.flags = AO_GPS_RUNNING; + ao_gps_next.hour = ao_gps_decimal(2); + ao_gps_next.minute = ao_gps_decimal(2); + ao_gps_next.second = ao_gps_decimal(2); + ao_gps_skip_field(); /* skip seconds fraction */ + + ao_gps_next.latitude = ao_gps_parse_pos(2); + if (ao_gps_parse_flag('N', 'S')) + ao_gps_next.latitude = -ao_gps_next.latitude; + ao_gps_next.longitude = ao_gps_parse_pos(3); + if (ao_gps_parse_flag('E', 'W')) + ao_gps_next.longitude = -ao_gps_next.longitude; + + i = ao_gps_decimal(0xff); + if (i == 1) + ao_gps_next.flags |= AO_GPS_VALID; + + i = ao_gps_decimal(0xff) << AO_GPS_NUM_SAT_SHIFT; + if (i > AO_GPS_NUM_SAT_MASK) + i = AO_GPS_NUM_SAT_MASK; + ao_gps_next.flags |= i; + + ao_gps_lexchar(); + ao_gps_skip_field(); /* Horizontal dilution */ + + ao_gps_next.altitude = ao_gps_decimal(0xff); + ao_gps_skip_field(); /* skip any fractional portion */ + + /* Skip remaining fields */ + while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') { + ao_gps_lexchar(); + ao_gps_skip_field(); + } + if (ao_gps_char == '*') { + uint8_t cksum = ao_gps_cksum ^ '*'; + if (cksum != ao_gps_hex(2)) + ao_gps_error = 1; + } else + ao_gps_error = 1; + if (!ao_gps_error) { + ao_mutex_get(&ao_gps_mutex); + memcpy(&ao_gps_data, &ao_gps_next, sizeof (struct ao_gps_data)); + ao_mutex_put(&ao_gps_mutex); + ao_wakeup(&ao_gps_data); + } + } else if (c == (uint8_t) 'S' && i == (uint8_t) 'V') { + uint8_t done; + /* Now read the data into the GPS tracking data record + * + * $GPGSV,3,1,12,05,54,069,45,12,44,061,44,21,07,184,46,22,78,289,47*72 + * + * Satellites in view data + * + * 3 Total number of GSV messages + * 1 Sequence number of current GSV message + * 12 Total sats in view (0-12) + * 05 SVID + * 54 Elevation + * 069 Azimuth + * 45 C/N0 in dB + * ... other SVIDs + * 72 checksum + */ + c = ao_gps_decimal(1); /* total messages */ + i = ao_gps_decimal(1); /* message sequence */ + if (i == 1) { + ao_gps_tracking_next.channels = 0; + } + done = (uint8_t) c == i; + ao_gps_lexchar(); + ao_gps_skip_field(); /* sats in view */ + while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') { + i = ao_gps_tracking_next.channels; + ao_gps_tracking_next.sats[i].svid = ao_gps_decimal(2); /* SVID */ + ao_gps_lexchar(); + ao_gps_skip_field(); /* elevation */ + ao_gps_lexchar(); + ao_gps_skip_field(); /* azimuth */ + if (ao_gps_tracking_next.sats[i].c_n_1 = ao_gps_decimal(2)) /* C/N0 */ + ao_gps_tracking_next.sats[i].state = 0xbf; + else + ao_gps_tracking_next.sats[i].state = 0; + ao_gps_tracking_next.channels = i + 1; + } + if (ao_gps_char == '*') { + uint8_t cksum = ao_gps_cksum ^ '*'; + if (cksum != ao_gps_hex(2)) + ao_gps_error = 1; + } + else + ao_gps_error = 1; + if (ao_gps_error) + ao_gps_tracking_next.channels = 0; + else if (done) { + ao_mutex_get(&ao_gps_mutex); + memcpy(&ao_gps_tracking_data, &ao_gps_tracking_next, + sizeof(ao_gps_tracking_data)); + ao_mutex_put(&ao_gps_mutex); + ao_wakeup(&ao_gps_tracking_data); + } + } + } +} + +__xdata struct ao_task ao_gps_task; + +static void +gps_dump(void) __reentrant +{ + ao_mutex_get(&ao_gps_mutex); + ao_gps_print(&ao_gps_data); + putchar('\n'); + ao_gps_tracking_print(&ao_gps_tracking_data); + putchar('\n'); + ao_mutex_put(&ao_gps_mutex); +} + +__code struct ao_cmds ao_gps_cmds[] = { + { 'g', gps_dump, "g Display current GPS values" }, + { 0, gps_dump, NULL }, +}; + +void +ao_gps_init(void) +{ + ao_add_task(&ao_gps_task, ao_gps, "gps"); + ao_cmd_register(&ao_gps_cmds[0]); +} diff --git a/src/ao_gps_test.c b/src/ao_gps_test.c index c94128d9..366bca71 100644 --- a/src/ao_gps_test.c +++ b/src/ao_gps_test.c @@ -398,7 +398,7 @@ ao_serial_set_speed(uint8_t speed) } #include "ao_gps_print.c" -#include "ao_gps.c" +#include "ao_gps_sirf.c" void ao_dump_state(void *wchan) diff --git a/src/ao_gps_test_skytraq.c b/src/ao_gps_test_skytraq.c new file mode 100644 index 00000000..510bc419 --- /dev/null +++ b/src/ao_gps_test_skytraq.c @@ -0,0 +1,478 @@ +/* + * Copyright © 2009 Keith Packard + * + * 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 +#include +#include +#include +#include +#define AO_GPS_NUM_SAT_MASK (0xf << 0) +#define AO_GPS_NUM_SAT_SHIFT (0) + +#define AO_GPS_VALID (1 << 4) +#define AO_GPS_RUNNING (1 << 5) + +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 */ +}; + +#define SIRF_SAT_STATE_ACQUIRED (1 << 0) +#define SIRF_SAT_STATE_CARRIER_PHASE_VALID (1 << 1) +#define SIRF_SAT_BIT_SYNC_COMPLETE (1 << 2) +#define SIRF_SAT_SUBFRAME_SYNC_COMPLETE (1 << 3) +#define SIRF_SAT_CARRIER_PULLIN_COMPLETE (1 << 4) +#define SIRF_SAT_CODE_LOCKED (1 << 5) +#define SIRF_SAT_ACQUISITION_FAILED (1 << 6) +#define SIRF_SAT_EPHEMERIS_AVAILABLE (1 << 7) + +struct ao_gps_sat_data { + uint8_t svid; + uint8_t state; + uint8_t c_n_1; +}; + +struct ao_gps_tracking_data { + uint8_t channels; + struct ao_gps_sat_data sats[12]; +}; + +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; + +#include + +int +get_millis(void) +{ + struct timeval tv; + gettimeofday(&tv, NULL); + return tv.tv_sec * 1000 + tv.tv_usec / 1000; +} + +static void +check_skytraq_message(char *from, uint8_t *msg, int len) +{ + uint16_t encoded_len, encoded_cksum; + uint16_t cksum; + uint8_t id; + int i; + +// fwrite(msg, 1, len, stdout); + return; + 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]; +/* printf ("%9d: %3d\n", get_millis(), id); */ + if (encoded_len != len - 8) { + if (id != 52) + 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]; + switch (id) { + case 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 %5d", week); + printf (" TOW %9d", tow); + printf (" %4d-%2d-%2d %02d:%02d:%07.4f\n", + year, month, day, + hour, minute, second / 1000.0); + printf ("\tsats: %08x\n", sat_list); + printf ("\tlat: %g", lat / 1.0e7); + printf (" lon: %g", lon / 1.0e7); + printf (" alt_ell: %g", alt_ell / 100.0); + printf (" alt_msll: %g", alt_msl / 100.0); + printf (" datum: %d\n", datum); + printf ("\tground speed: %g", sog / 100.0); + printf (" course: %g", cog / 100.0); + printf (" climb: %g", climb_rate / 100.0); + printf (" heading rate: %g\n", heading_rate / 100.0); + printf ("\th error: %g", h_error / 100.0); + printf (" v error: %g", v_error / 100.0); + printf (" t error: %g", t_error / 100.0); + printf (" h vel error: %g\n", h_v_error / 100.0); + break; + } + case 4: { + int off = 4; + uint8_t id; + int16_t gps_week; + uint32_t gps_tow; + uint8_t channels; + int j, k; + + get_u8(id); + get_u16(gps_week); + get_u32(gps_tow); + get_u8(channels); + + printf ("Measured Tracker Data (4):\n"); + printf ("GPS week: %d\n", gps_week); + printf ("GPS time of week: %d\n", gps_tow); + printf ("channels: %d\n", channels); + for (j = 0; j < 12; j++) { + uint8_t svid, azimuth, elevation; + uint16_t state; + uint8_t c_n[10]; + get_u8(svid); + get_u8(azimuth); + get_u8(elevation); + get_u16(state); + for (k = 0; k < 10; k++) { + get_u8(c_n[k]); + } + printf ("Sat %3d:", svid); + printf (" aziumuth: %6.1f", azimuth * 1.5); + printf (" elevation: %6.1f", elevation * 0.5); + printf (" state: 0x%02x", state); + printf (" c_n:"); + for (k = 0; k < 10; k++) + printf(" %3d", c_n[k]); + if (state & SIRF_SAT_STATE_ACQUIRED) + printf(" acq,"); + if (state & SIRF_SAT_STATE_CARRIER_PHASE_VALID) + printf(" car,"); + if (state & SIRF_SAT_BIT_SYNC_COMPLETE) + printf(" bit,"); + if (state & SIRF_SAT_SUBFRAME_SYNC_COMPLETE) + printf(" sub,"); + if (state & SIRF_SAT_CARRIER_PULLIN_COMPLETE) + printf(" pullin,"); + if (state & SIRF_SAT_CODE_LOCKED) + printf(" code,"); + if (state & SIRF_SAT_ACQUISITION_FAILED) + printf(" fail,"); + if (state & SIRF_SAT_EPHEMERIS_AVAILABLE) + printf(" ephem,"); + printf ("\n"); + } + break; + } + default: + 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"); + } +} + +static uint8_t skytraq_message[4096]; +static int skytraq_message_len; +static uint8_t skytraq_in_message[4096]; +static int skytraq_in_len; + +char +ao_serial_getchar(void) +{ + char c; + 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_head = 0; + break; + } + } + c = input_queue[input_head]; + input_head = (input_head + 1) % QUEUE_LEN; + uc = c; +// printf ("c: %02x %c\n", uc, uc); + if (skytraq_in_len || uc == '$') { + if (skytraq_in_len < 4096) + skytraq_in_message[skytraq_in_len++] = uc; + if (uc == 0x0a) { + check_skytraq_message("recv", skytraq_in_message, skytraq_in_len); + skytraq_in_len = 0; + } + } + return c; +} + + +void +ao_serial_putchar(char c) +{ + int i; + uint8_t uc = (uint8_t) c; + + if (skytraq_message_len || uc == 0xa0) { + if (skytraq_message_len < 4096) + skytraq_message[skytraq_message_len++] = uc; + if (uc == 0x0a) { + check_skytraq_message("send", skytraq_message, skytraq_message_len); + skytraq_message_len = 0; + } + } + for (;;) { + 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); +// nanosleep(&delay, NULL); + } + break; + } + if (i < 0 && (errno == EINTR || errno == EAGAIN)) + continue; + perror("putchar"); + exit(1); + } +} + +#define AO_SERIAL_SPEED_4800 0 +#define AO_SERIAL_SPEED_9600 1 +#define AO_SERIAL_SPEED_57600 2 + +static void +ao_serial_set_speed(uint8_t speed) +{ + int fd = ao_gps_fd; + struct termios termios; + + tcdrain(fd); + tcgetattr(fd, &termios); + switch (speed) { + case AO_SERIAL_SPEED_4800: + cfsetspeed(&termios, B4800); + break; + case AO_SERIAL_SPEED_9600: + cfsetspeed(&termios, B38400); + break; + case AO_SERIAL_SPEED_57600: + cfsetspeed(&termios, B57600); + break; + } + tcsetattr(fd, TCSAFLUSH, &termios); + tcflush(fd, TCIFLUSH); +} + +#include "ao_gps_print.c" +#include "ao_gps_skytraq.c" + +void +ao_dump_state(void *wchan) +{ + double lat, lon; + int i; + if (wchan == &ao_gps_data) + ao_gps_print(&ao_gps_data); + else + ao_gps_tracking_print(&ao_gps_tracking_data); + putchar('\n'); + return; +} + +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; +} + +#include + +static const struct option options[] = { + { .name = "tty", .has_arg = 1, .val = 'T' }, + { 0, 0, 0, 0}, +}; + +static void usage(char *program) +{ + fprintf(stderr, "usage: %s [--tty ]\n", program); + exit(1); +} + +int +main (int argc, char **argv) +{ + char *tty = "/dev/ttyUSB0"; + int c; + + while ((c = getopt_long(argc, argv, "T:", options, NULL)) != -1) { + switch (c) { + case 'T': + tty = optarg; + break; + default: + usage(argv[0]); + break; + } + } + ao_gps_fd = ao_gps_open(tty); + if (ao_gps_fd < 0) { + perror (tty); + exit (1); + } + ao_gps(); +} diff --git a/src/ao_log.c b/src/ao_log.c index 7945ace4..b2bfbd6f 100644 --- a/src/ao_log.c +++ b/src/ao_log.c @@ -192,7 +192,7 @@ ao_log_stop(void) } static void -dump_log(void) +dump_log(void) __reentrant { uint8_t more; diff --git a/src/ao_monitor.c b/src/ao_monitor.c index e57ea145..d0c1da34 100644 --- a/src/ao_monitor.c +++ b/src/ao_monitor.c @@ -30,7 +30,8 @@ ao_monitor(void) for (;;) { __critical while (!ao_monitoring) ao_sleep(&ao_monitoring); - ao_radio_recv(&recv); + if (!ao_radio_recv(&recv)) + continue; state = recv.telemetry.flight_state; memcpy(callsign, recv.telemetry.callsign, AO_MAX_CALLSIGN); if (state > ao_flight_invalid) @@ -74,6 +75,7 @@ ao_set_monitor(uint8_t monitoring) { ao_monitoring = monitoring; ao_wakeup(&ao_monitoring); + ao_radio_abort(); } static void diff --git a/src/ao_packet.c b/src/ao_packet.c new file mode 100644 index 00000000..3ce7e9ab --- /dev/null +++ b/src/ao_packet.c @@ -0,0 +1,152 @@ +/* + * Copyright © 2009 Keith Packard + * + * 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. + */ + +#include "ao.h" + +__xdata struct ao_packet_recv ao_rx_packet; +__xdata struct ao_packet ao_tx_packet; +__pdata uint8_t ao_packet_rx_len, ao_packet_rx_used, ao_packet_tx_used; + +static __xdata char tx_data[AO_PACKET_MAX]; +static __xdata char rx_data[AO_PACKET_MAX]; +static __pdata uint8_t rx_seq; + +__xdata struct ao_task ao_packet_task; +__xdata uint8_t ao_packet_enable; +__xdata uint8_t ao_packet_master_sleeping; + +void +ao_packet_send(void) +{ + ao_led_on(AO_LED_RED); + ao_config_get(); + ao_mutex_get(&ao_radio_mutex); + if (ao_packet_tx_used && ao_tx_packet.len == 0) { + memcpy(&ao_tx_packet.d, tx_data, ao_packet_tx_used); + ao_tx_packet.len = ao_packet_tx_used; + ao_tx_packet.seq++; + ao_packet_tx_used = 0; + ao_wakeup(&tx_data); + } + ao_radio_idle(); + ao_radio_done = 0; + RF_CHANNR = ao_config.radio_channel; + ao_dma_set_transfer(ao_radio_dma, + &ao_tx_packet, + &RFDXADDR, + sizeof (struct ao_packet), + DMA_CFG0_WORDSIZE_8 | + DMA_CFG0_TMODE_SINGLE | + DMA_CFG0_TRIGGER_RADIO, + DMA_CFG1_SRCINC_1 | + DMA_CFG1_DESTINC_0 | + DMA_CFG1_PRIORITY_HIGH); + ao_dma_start(ao_radio_dma); + RFST = RFST_STX; + __critical while (!ao_radio_done) + ao_sleep(&ao_radio_done); + ao_mutex_put(&ao_radio_mutex); + ao_led_off(AO_LED_RED); +} + +uint8_t +ao_packet_recv(void) +{ + uint8_t dma_done; + + ao_led_on(AO_LED_GREEN); + ao_config_get(); + ao_mutex_get(&ao_radio_mutex); + ao_radio_idle(); + RF_CHANNR = ao_config.radio_channel; + ao_dma_set_transfer(ao_radio_dma, + &RFDXADDR, + &ao_rx_packet, + sizeof (struct ao_packet_recv), + DMA_CFG0_WORDSIZE_8 | + DMA_CFG0_TMODE_SINGLE | + DMA_CFG0_TRIGGER_RADIO, + DMA_CFG1_SRCINC_0 | + DMA_CFG1_DESTINC_1 | + DMA_CFG1_PRIORITY_HIGH); + ao_dma_start(ao_radio_dma); + RFST = RFST_SRX; + __critical while (!ao_radio_dma_done) + if (ao_sleep(&ao_radio_dma_done) != 0) + ao_radio_abort(); + dma_done = ao_radio_dma_done; + ao_mutex_put(&ao_radio_mutex); + ao_led_off(AO_LED_GREEN); + + if (dma_done & AO_DMA_DONE) { + if (!(ao_rx_packet.status & PKT_APPEND_STATUS_1_CRC_OK)) + return AO_DMA_ABORTED; + if (ao_rx_packet.packet.len == AO_PACKET_SYN) { + rx_seq = ao_rx_packet.packet.seq; + ao_tx_packet.seq = ao_rx_packet.packet.ack; + ao_tx_packet.ack = rx_seq; + } else if (ao_rx_packet.packet.len) { + if (ao_rx_packet.packet.seq == (uint8_t) (rx_seq + (uint8_t) 1) && ao_packet_rx_used == ao_packet_rx_len) { + memcpy(rx_data, ao_rx_packet.packet.d, ao_rx_packet.packet.len); + ao_packet_rx_used = 0; + ao_packet_rx_len = ao_rx_packet.packet.len; + rx_seq = ao_rx_packet.packet.seq; + ao_tx_packet.ack = rx_seq; + ao_wakeup(&ao_stdin_ready); + } + } + if (ao_rx_packet.packet.ack == ao_tx_packet.seq) { + ao_tx_packet.len = 0; + ao_wakeup(&ao_tx_packet); + } + } + return dma_done; +} + +void +ao_packet_flush(void) +{ + /* If there is data to send, and this is the master, + * then poke the master to send all queued data + */ + if (ao_packet_tx_used && ao_packet_master_sleeping) + ao_wake_task(&ao_packet_task); +} + +void +ao_packet_putchar(char c) __reentrant +{ + while (ao_packet_tx_used == AO_PACKET_MAX && ao_packet_enable) { + ao_packet_flush(); + ao_sleep(&tx_data); + } + + if (ao_packet_enable) + tx_data[ao_packet_tx_used++] = c; +} + +char +ao_packet_pollchar(void) __critical +{ + if (!ao_packet_enable) + return AO_READ_AGAIN; + + if (ao_packet_rx_used == ao_packet_rx_len) + return AO_READ_AGAIN; + + return rx_data[ao_packet_rx_used++]; +} diff --git a/src/ao_packet_master.c b/src/ao_packet_master.c new file mode 100644 index 00000000..2751f414 --- /dev/null +++ b/src/ao_packet_master.c @@ -0,0 +1,142 @@ +/* + * Copyright © 2009 Keith Packard + * + * 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. + */ + +#include "ao.h" + +static char +ao_packet_getchar(void) +{ + char c; + while ((c = ao_packet_pollchar()) == AO_READ_AGAIN) + { + if (!ao_packet_enable) + break; + if (ao_packet_master_sleeping) + ao_wake_task(&ao_packet_task); + ao_sleep(&ao_stdin_ready); + } + return c; +} + +static void +ao_packet_echo(void) __reentrant +{ + uint8_t c; + while (ao_packet_enable) { + c = ao_packet_getchar(); + if (ao_packet_enable) + putchar(c); + } + ao_exit(); +} + +static __xdata struct ao_task ao_packet_echo_task; +static __xdata uint16_t ao_packet_master_delay; +static __xdata uint16_t ao_packet_master_time; + +#define AO_PACKET_MASTER_DELAY_SHORT AO_MS_TO_TICKS(100) +#define AO_PACKET_MASTER_DELAY_LONG AO_MS_TO_TICKS(1000) +#define AO_PACKET_MASTER_DELAY_TIMEOUT AO_MS_TO_TICKS(2000) + +static void +ao_packet_master_busy(void) +{ + ao_packet_master_delay = AO_PACKET_MASTER_DELAY_SHORT; + ao_packet_master_time = ao_time(); +} + +static void +ao_packet_master_check_busy(void) +{ + int16_t idle; + if (ao_packet_master_delay != AO_PACKET_MASTER_DELAY_SHORT) + return; + idle = (int16_t) (ao_time() - ao_packet_master_time); + + if (idle > AO_PACKET_MASTER_DELAY_TIMEOUT) + ao_packet_master_delay = AO_PACKET_MASTER_DELAY_LONG; +} + +void +ao_packet_master(void) +{ + uint8_t status; + + ao_radio_set_packet(); + ao_tx_packet.addr = ao_serial_number; + ao_tx_packet.len = AO_PACKET_SYN; + ao_packet_master_time = ao_time(); + ao_packet_master_delay = AO_PACKET_MASTER_DELAY_SHORT; + while (ao_packet_enable) { + ao_packet_send(); + if (ao_tx_packet.len) + ao_packet_master_busy(); + ao_packet_master_check_busy(); + ao_alarm(ao_packet_master_delay); + status = ao_packet_recv(); + if (status & AO_DMA_DONE) { + /* if we can transmit data, do so */ + if (ao_packet_tx_used && ao_tx_packet.len == 0) + continue; + if (ao_rx_packet.packet.len) + ao_packet_master_busy(); + else + flush(); + ao_packet_master_sleeping = 1; + ao_delay(ao_packet_master_delay); + ao_packet_master_sleeping = 0; + } + } + ao_exit(); +} + +static void +ao_packet_forward(void) __reentrant +{ + char c; + ao_packet_enable = 1; + ao_cmd_white(); + + flush(); + ao_set_monitor(0); + ao_add_task(&ao_packet_task, ao_packet_master, "master"); + ao_add_task(&ao_packet_echo_task, ao_packet_echo, "echo"); + while ((c = getchar()) != '~') { + if (c == '\r') c = '\n'; + ao_packet_putchar(c); + } + ao_packet_enable = 0; + ao_radio_abort(); + while (ao_packet_echo_task.wchan || ao_packet_task.wchan) { + ao_wake_task(&ao_packet_echo_task); + ao_wake_task(&ao_packet_task); + ao_yield(); + } +} + + + +__code struct ao_cmds ao_packet_master_cmds[] = { + { 'p', ao_packet_forward, "p Remote packet link." }, + { 0, ao_packet_forward, NULL }, +}; + +void +ao_packet_master_init(void) +{ + ao_cmd_register(&ao_packet_master_cmds[0]); +} diff --git a/src/ao_packet_slave.c b/src/ao_packet_slave.c new file mode 100644 index 00000000..ba5ad1c1 --- /dev/null +++ b/src/ao_packet_slave.c @@ -0,0 +1,58 @@ +/* + * Copyright © 2009 Keith Packard + * + * 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. + */ + +#include "ao.h" + +void +ao_packet_slave(void) +{ + ao_radio_set_packet(); + ao_tx_packet.addr = ao_serial_number; + ao_tx_packet.len = AO_PACKET_SYN; + while (ao_packet_enable) { + ao_packet_recv(); + ao_packet_send(); + } + ao_exit(); +} + +void +ao_packet_slave_start(void) +{ + ao_packet_enable = 1; + ao_add_task(&ao_packet_task, ao_packet_slave, "slave"); +} + +void +ao_packet_slave_stop(void) +{ + ao_packet_enable = 0; + ao_radio_abort(); + while (ao_packet_task.wchan) { + ao_wake_task(&ao_packet_task); + ao_yield(); + } + ao_radio_set_telemetry(); +} + +void +ao_packet_slave_init(void) +{ + ao_add_stdio(ao_packet_pollchar, + ao_packet_putchar, + ao_packet_flush); +} diff --git a/src/ao_radio.c b/src/ao_radio.c index a7fa682e..c7c8dc8d 100644 --- a/src/ao_radio.c +++ b/src/ao_radio.c @@ -255,11 +255,76 @@ static __code uint8_t telemetry_setup[] = { RF_PKTCTRL0_LENGTH_CONFIG_FIXED), }; +static __code uint8_t packet_setup[] = { + RF_MDMCFG4_OFF, ((CHANBW_E << RF_MDMCFG4_CHANBW_E_SHIFT) | + (CHANBW_M << RF_MDMCFG4_CHANBW_M_SHIFT) | + (DRATE_E << RF_MDMCFG4_DRATE_E_SHIFT)), + RF_MDMCFG3_OFF, (DRATE_M << RF_MDMCFG3_DRATE_M_SHIFT), + RF_MDMCFG2_OFF, (RF_MDMCFG2_DEM_DCFILT_OFF | + RF_MDMCFG2_MOD_FORMAT_GFSK | + RF_MDMCFG2_SYNC_MODE_15_16_THRES), + RF_MDMCFG1_OFF, (RF_MDMCFG1_FEC_EN | + RF_MDMCFG1_NUM_PREAMBLE_4 | + (2 << RF_MDMCFG1_CHANSPC_E_SHIFT)), + + RF_DEVIATN_OFF, ((DEVIATION_E << RF_DEVIATN_DEVIATION_E_SHIFT) | + (DEVIATION_M << RF_DEVIATN_DEVIATION_M_SHIFT)), + + /* max packet length */ + RF_PKTLEN_OFF, sizeof (struct ao_packet), + RF_PKTCTRL1_OFF, ((1 << PKTCTRL1_PQT_SHIFT)| + PKTCTRL1_APPEND_STATUS| + PKTCTRL1_ADR_CHK_NONE), + RF_PKTCTRL0_OFF, (RF_PKTCTRL0_WHITE_DATA| + RF_PKTCTRL0_PKT_FORMAT_NORMAL| + RF_PKTCTRL0_CRC_EN| + RF_PKTCTRL0_LENGTH_CONFIG_FIXED), +}; + __xdata uint8_t ao_radio_dma; __xdata uint8_t ao_radio_dma_done; +__xdata uint8_t ao_radio_done; __xdata uint8_t ao_radio_mutex; -static void +void +ao_radio_general_isr(void) interrupt 16 +{ + S1CON &= ~0x03; + if (RFIF & RFIF_IM_TIMEOUT) { + ao_dma_abort(ao_radio_dma); + RFIF &= ~ RFIF_IM_TIMEOUT; + } else if (RFIF & RFIF_IM_DONE) { + ao_radio_done = 1; + ao_wakeup(&ao_radio_done); + RFIF &= ~RFIF_IM_DONE; + } +} + +void +ao_radio_set_telemetry(void) +{ + uint8_t i; + for (i = 0; i < sizeof (telemetry_setup); i += 2) + RF[telemetry_setup[i]] = telemetry_setup[i+1]; +} + +void +ao_radio_set_packet(void) +{ + uint8_t i; + for (i = 0; i < sizeof (packet_setup); i += 2) + RF[packet_setup[i]] = packet_setup[i+1]; +} + +void +ao_radio_set_rdf(void) +{ + uint8_t i; + for (i = 0; i < sizeof (rdf_setup); i += 2) + RF[rdf_setup[i]] = rdf_setup[i+1]; +} + +void ao_radio_idle(void) { if (RF_MARCSTATE != RF_MARCSTATE_IDLE) @@ -277,6 +342,7 @@ ao_radio_send(__xdata struct ao_telemetry *telemetry) __reentrant ao_config_get(); ao_mutex_get(&ao_radio_mutex); ao_radio_idle(); + ao_radio_done = 0; RF_CHANNR = ao_config.radio_channel; ao_dma_set_transfer(ao_radio_dma, telemetry, @@ -290,12 +356,12 @@ ao_radio_send(__xdata struct ao_telemetry *telemetry) __reentrant DMA_CFG1_PRIORITY_HIGH); ao_dma_start(ao_radio_dma); RFST = RFST_STX; - __critical while (!ao_radio_dma_done) - ao_sleep(&ao_radio_dma_done); + __critical while (!ao_radio_done) + ao_sleep(&ao_radio_done); ao_mutex_put(&ao_radio_mutex); } -void +uint8_t ao_radio_recv(__xdata struct ao_radio_recv *radio) __reentrant { ao_config_get(); @@ -317,6 +383,7 @@ ao_radio_recv(__xdata struct ao_radio_recv *radio) __reentrant __critical while (!ao_radio_dma_done) ao_sleep(&ao_radio_dma_done); ao_mutex_put(&ao_radio_mutex); + return (ao_radio_dma_done & AO_DMA_DONE); } __xdata ao_radio_rdf_running; @@ -367,21 +434,51 @@ ao_radio_rdf(int ms) ao_mutex_put(&ao_radio_mutex); } +void +ao_radio_abort(void) +{ + ao_dma_abort(ao_radio_dma); + ao_radio_idle(); +} + void ao_radio_rdf_abort(void) { - if (ao_radio_rdf_running) { - ao_dma_abort(ao_radio_dma); - ao_radio_idle(); - } + if (ao_radio_rdf_running) + ao_radio_abort(); +} + +/* Output carrier */ +void +ao_radio_test(void) +{ + ao_config_get(); + ao_mutex_get(&ao_radio_mutex); + ao_radio_idle(); + printf ("Hit a character to stop..."); flush(); + RFST = RFST_STX; + getchar(); + ao_radio_idle(); + ao_mutex_put(&ao_radio_mutex); + putchar('\n'); } +__code struct ao_cmds ao_radio_cmds[] = { + { 'C', ao_radio_test, "C Radio carrier test" }, + { 0, ao_radio_test, NULL }, +}; + void ao_radio_init(void) { uint8_t i; for (i = 0; i < sizeof (radio_setup); i += 2) RF[radio_setup[i]] = radio_setup[i+1]; + ao_radio_set_telemetry(); ao_radio_dma_done = 1; ao_radio_dma = ao_dma_alloc(&ao_radio_dma_done); + RFIF = 0; + RFIM = RFIM_IM_TIMEOUT|RFIM_IM_DONE; + IEN2 |= IEN2_RFIE; + ao_cmd_register(&ao_radio_cmds[0]); } diff --git a/src/ao_serial.c b/src/ao_serial.c index 59110354..1e3ea3e3 100644 --- a/src/ao_serial.c +++ b/src/ao_serial.c @@ -60,7 +60,10 @@ ao_serial_getchar(void) __critical ao_sleep(&ao_usart1_rx_fifo); ao_fifo_remove(ao_usart1_rx_fifo, c); if (serial_echo) { - printf("%02x\n", ((int) c) & 0xff); + printf("%02x ", ((int) c) & 0xff); + if (c >= ' ') + putchar(c); + putchar('\n'); flush(); } return c; @@ -121,6 +124,10 @@ static const struct { /* .baud = */ 163, /* .gcr = */ (7 << UxGCR_BAUD_E_SHIFT) | UxGCR_ORDER_LSB }, + /* [AO_SERIAL_SPEED_9600] = */ { + /* .baud = */ 163, + /* .gcr = */ (8 << UxGCR_BAUD_E_SHIFT) | UxGCR_ORDER_LSB + }, /* [AO_SERIAL_SPEED_57600] = */ { /* .baud = */ 59, /* .gcr = */ (11 << UxGCR_BAUD_E_SHIFT) | UxGCR_ORDER_LSB diff --git a/src/ao_stdio.c b/src/ao_stdio.c index fb8ce093..7bc416e1 100644 --- a/src/ao_stdio.c +++ b/src/ao_stdio.c @@ -21,22 +21,56 @@ * Basic I/O functions to support SDCC stdio package */ +#define AO_NUM_STDIOS 2 + +static __xdata struct ao_stdio stdios[AO_NUM_STDIOS]; +static __data int8_t ao_cur_stdio; +static __data int8_t ao_num_stdios; + void putchar(char c) { if (c == '\n') - ao_usb_putchar('\r'); - ao_usb_putchar(c); + (*stdios[ao_cur_stdio].putchar)('\r'); + (*stdios[ao_cur_stdio].putchar)(c); } void flush(void) { - ao_usb_flush(); + stdios[ao_cur_stdio].flush(); } +__xdata uint8_t ao_stdin_ready; + char -getchar(void) +getchar(void) __reentrant +{ + char c; + int8_t stdio = ao_cur_stdio; + + for (;;) { + c = stdios[stdio].pollchar(); + if (c != AO_READ_AGAIN) + break; + if (++stdio == ao_num_stdios) + stdio = 0; + if (stdio == ao_cur_stdio) + ao_sleep(&ao_stdin_ready); + } + ao_cur_stdio = stdio; + return c; +} + +void +ao_add_stdio(char (*pollchar)(void), + void (*putchar)(char), + void (*flush)(void)) { - return ao_usb_getchar(); + if (ao_num_stdios == AO_NUM_STDIOS) + ao_panic(AO_PANIC_STDIO); + stdios[ao_num_stdios].pollchar = pollchar; + stdios[ao_num_stdios].putchar = putchar; + stdios[ao_num_stdios].flush = flush; + ao_num_stdios++; } diff --git a/src/ao_task.c b/src/ao_task.c index 12b73943..4664163d 100644 --- a/src/ao_task.c +++ b/src/ao_task.c @@ -93,7 +93,9 @@ ao_yield(void) _naked push _bp _endasm; - if (ao_cur_task_index != AO_NO_TASK_INDEX) + if (ao_cur_task_index == AO_NO_TASK_INDEX) + ao_cur_task_index = ao_num_tasks-1; + else { uint8_t stack_len; __data uint8_t *stack_ptr; @@ -127,6 +129,13 @@ ao_yield(void) _naked break; } + /* Check if the alarm is set for a time which has passed */ + if (ao_cur_task->alarm && + (int16_t) (ao_time() - ao_cur_task->alarm) >= 0) { + ao_cur_task_index = ao_next_task_index; + break; + } + /* Enter lower power mode when there isn't anything to do */ if (ao_next_task_index == ao_cur_task_index) PCON = PCON_IDLE; @@ -179,13 +188,20 @@ ao_yield(void) _naked _endasm; } -void +uint8_t ao_sleep(__xdata void *wchan) { __critical { ao_cur_task->wchan = wchan; } ao_yield(); + if (ao_cur_task->wchan) { + ao_cur_task->wchan = NULL; + ao_cur_task->alarm = 0; + return 1; + } + ao_cur_task->alarm = 0; + return 0; } void @@ -198,6 +214,31 @@ ao_wakeup(__xdata void *wchan) ao_tasks[i]->wchan = NULL; } +void +ao_alarm(uint16_t delay) +{ + if (!(ao_cur_task->alarm = ao_time() + delay)) + ao_cur_task->alarm = 1; +} + +void +ao_wake_task(__xdata struct ao_task *task) +{ + task->wchan = NULL; +} + +void +ao_exit(void) __critical +{ + uint8_t i; + ao_num_tasks--; + for (i = ao_cur_task_index; i < ao_num_tasks; i++) + ao_tasks[i] = ao_tasks[i+1]; + ao_cur_task_index = AO_NO_TASK_INDEX; + ao_yield(); + /* we'll never get back here */ +} + void ao_task_info(void) { diff --git a/src/ao_teledongle.c b/src/ao_teledongle.c index d7b4b75a..e4828d80 100644 --- a/src/ao_teledongle.c +++ b/src/ao_teledongle.c @@ -33,6 +33,8 @@ main(void) ao_monitor_init(AO_LED_GREEN, TRUE); ao_rssi_init(AO_LED_RED); ao_radio_init(); + ao_packet_slave_init(); + ao_packet_master_init(); ao_config_init(); ao_start_scheduler(); } diff --git a/src/ao_telemetrum.c b/src/ao_telemetrum.c index 1dbacf89..5250078e 100644 --- a/src/ao_telemetrum.c +++ b/src/ao_telemetrum.c @@ -40,6 +40,7 @@ main(void) ao_gps_report_init(); ao_telemetry_init(); ao_radio_init(); + ao_packet_slave_init(); ao_igniter_init(); ao_config_init(); ao_start_scheduler(); diff --git a/src/ao_teleterra.c b/src/ao_teleterra.c index deb63597..d696b914 100644 --- a/src/ao_teleterra.c +++ b/src/ao_teleterra.c @@ -31,7 +31,6 @@ main(void) ao_cmd_init(); ao_usb_init(); ao_serial_init(); - ao_gps_init(); ao_monitor_init(AO_LED_GREEN, TRUE); ao_radio_init(); ao_config_init(); diff --git a/src/ao_timer.c b/src/ao_timer.c index 78c6e063..e81f937d 100644 --- a/src/ao_timer.c +++ b/src/ao_timer.c @@ -24,13 +24,13 @@ uint16_t ao_time(void) __critical return ao_tick_count; } +static __xdata uint8_t ao_forever; + void ao_delay(uint16_t ticks) { - uint16_t until = ao_time() + ticks; - - while ((int16_t) (until - ao_time()) > 0) - ao_sleep(DATA_TO_XDATA(&ao_tick_count)); + ao_alarm(ticks); + ao_sleep(&ao_forever); } #define T1_CLOCK_DIVISOR 8 /* 24e6/8 = 3e6 */ @@ -46,7 +46,6 @@ void ao_timer_isr(void) interrupt 9 ao_adc_count = 0; ao_adc_poll(); } - ao_wakeup(DATA_TO_XDATA(&ao_tick_count)); } void diff --git a/src/ao_usb.c b/src/ao_usb.c index 22665725..daca71a7 100644 --- a/src/ao_usb.c +++ b/src/ao_usb.c @@ -53,7 +53,7 @@ ao_usb_isr(void) interrupt 6 ao_wakeup(&ao_usb_in_bytes); if (USBOIF & (1 << AO_USB_OUT_EP)) - ao_wakeup(&ao_usb_out_bytes); + ao_wakeup(&ao_stdin_ready); if (USBCIF & USBCIF_RSTIF) ao_usb_set_interrupts(); @@ -72,7 +72,7 @@ uint8_t * __xdata ao_usb_ep0_in_data; __xdata uint8_t ao_usb_ep0_in_len; __xdata uint8_t ao_usb_ep0_in_buf[2]; __xdata uint8_t ao_usb_ep0_out_len; -__xdata uint8_t *__data ao_usb_ep0_out_data; +__xdata uint8_t *__xdata ao_usb_ep0_out_data; __xdata uint8_t ao_usb_configuration; /* Send an IN data packet */ @@ -360,7 +360,7 @@ ao_usb_flush(void) __critical } void -ao_usb_putchar(char c) __critical +ao_usb_putchar(char c) __critical __reentrant { if (!ao_usb_running) return; @@ -374,16 +374,13 @@ ao_usb_putchar(char c) __critical } char -ao_usb_getchar(void) __critical +ao_usb_pollchar(void) __critical { - __xdata char c; + char c; while (ao_usb_out_bytes == 0) { - for (;;) { - USBINDEX = AO_USB_OUT_EP; - if ((USBCSOL & USBCSOL_OUTPKT_RDY) != 0) - break; - ao_sleep(&ao_usb_out_bytes); - } + USBINDEX = AO_USB_OUT_EP; + if ((USBCSOL & USBCSOL_OUTPKT_RDY) == 0) + return AO_READ_AGAIN; ao_usb_out_bytes = (USBCNTH << 8) | USBCNTL; } --ao_usb_out_bytes; @@ -395,6 +392,16 @@ ao_usb_getchar(void) __critical return c; } +char +ao_usb_getchar(void) +{ + char c; + + while ((c = ao_usb_pollchar()) == AO_READ_AGAIN) + ao_sleep(&ao_stdin_ready); + return c; +} + void ao_usb_enable(void) { @@ -438,4 +445,5 @@ ao_usb_init(void) ao_usb_enable(); ao_add_task(&ao_usb_task, ao_usb_ep0, "usb"); + ao_add_stdio(ao_usb_pollchar, ao_usb_putchar, ao_usb_flush); } diff --git a/src/cc1111.h b/src/cc1111.h index 87b14485..ee4c9f09 100644 --- a/src/cc1111.h +++ b/src/cc1111.h @@ -882,6 +882,16 @@ sfr at 0xE9 RFIF; #define RFIF_IM_CCA (1 << 1) #define RFIF_IM_SFD (1 << 0) +sfr at 0x91 RFIM; +#define RFIM_IM_TXUNF (1 << 7) +#define RFIM_IM_RXOVF (1 << 6) +#define RFIM_IM_TIMEOUT (1 << 5) +#define RFIM_IM_DONE (1 << 4) +#define RFIM_IM_CS (1 << 3) +#define RFIM_IM_PQT (1 << 2) +#define RFIM_IM_CCA (1 << 1) +#define RFIM_IM_SFD (1 << 0) + sfr at 0xE1 RFST; #define RFST_SFSTXON 0x00 diff --git a/src/skytraq-cksum b/src/skytraq-cksum new file mode 100644 index 00000000..ab0464a7 --- /dev/null +++ b/src/skytraq-cksum @@ -0,0 +1,44 @@ +#!/usr/bin/env nickle + +int checksum(int[] msg) +{ + int sum = 0; + for (int i = 0; i < dim(msg); i++) { + sum ^= msg[i]; + sum &= 0xff; + } + return sum; +} + +void main() +{ + string[...] input; + int[...] msg; + + setdim(input, 0); + while (!File::end(stdin)) { + input[dim(input)] = gets(); + } + + setdim(msg, 0); + for (int i = 0; i < dim(input); i++) { + string[*] words = String::wordsplit(input[i], " ,\t"); + for (int j = 0; j < dim(words); j++) { + if (words[j] == "/" + "*") + break; + if (String::length(words[j]) > 0 && + Ctype::isdigit(words[j][0])) { + msg[dim(msg)] = string_to_integer(words[j]); + } + } + } + printf("\t0xa0, 0xa1, 0x%02x, 0x%02x,\t\t/* length: %d bytes */\n", + dim(msg) >> 8, dim(msg) & 0xff, dim(msg)); + for (int i = 0; i < dim(input); i++) + printf("%s\n", input[i]); + int csum = checksum(msg); + printf ("\t0x%02x, 0x0d, 0x0a,\n", + csum); +} + +main();