aclocal.m4
src/ao_flight_test
src/ao_gps_test
+src/ao_gps_test_skytraq
ao-teledongle.h
ao-telemetrum.h
ao-teleterra.h
stamp-h1
tags
teledongle
-telemetrum
+telemetrum-sirf
+telemetrum-sky
teleterra
tidongle
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)
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.
return pd;
}
+static const char kml_header[] =
+ "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
+ "<kml xmlns=\"http://earth.google.com/kml/2.0\">\n"
+ " <Placemark>\n"
+ " <name>gps</name>\n"
+ " <Style id=\"khStyle690\">\n"
+ " <LineStyle id=\"khLineStyle694\">\n"
+ " <color>ff00ffff</color>\n"
+ " <width>4</width>\n"
+ " </LineStyle>\n"
+ " </Style>\n"
+ " <MultiGeometry id=\"khMultiGeometry697\">\n"
+ " <LineString id=\"khLineString698\">\n"
+ " <tessellate>1</tessellate>\n"
+ " <altitudeMode>absolute</altitudeMode>\n"
+ " <coordinates>\n";
+
+static const char kml_footer[] =
+ "</coordinates>\n"
+ " </LineString>\n"
+ " </MultiGeometry>\n"
+ "</Placemark>\n"
+ "</kml>\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;
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 <!-- time %12.7f sats %d -->",
+ 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");
{ .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},
};
"\t[--raw=<raw-file> -r <raw-file]\n"
"\t[--plot=<plot-file> -p <plot-file>]\n"
"\t[--gps=<gps-file> -g <gps-file>]\n"
+ "\t[--kml=<kml-file> -k <kml-file>]\n"
"\t{flight-log} ...\n", program);
exit(1);
}
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;
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;
case 'g':
gps_name = optarg;
break;
+ case 'k':
+ kml_name = optarg;
+ break;
default:
usage(argv[0]);
break;
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) {
}
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;
TELE_COMMON_SRC = \
ao_gps_print.c \
+ ao_packet.c \
+ ao_packet_slave.c \
ao_state.c
#
#
TELE_RECEIVER_SRC =\
ao_monitor.c \
+ ao_packet_master.c \
ao_rssi.c
#
TELE_DRIVER_SRC = \
ao_convert.c \
- ao_gps.c \
ao_serial.c
#
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
#
$(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
$(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
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)
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)
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
/* 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 */
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;
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);
#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
* 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))
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);
* 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);
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);
* 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;
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);
* 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
*/
* 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_ */
}
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,
uint8_t mask = (1 << id);
DMAARM = 0x80 | mask;
DMAIRQ &= ~mask;
+ *(ao_dma_done[id]) |= AO_DMA_ABORTED;
+ ao_wakeup(ao_dma_done[id]);
}
void
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;
}
}
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;
}
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;
} 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 */
#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],
#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,
+++ /dev/null
-/*
- * 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.
- */
-
-#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]);
-}
--- /dev/null
+/*
+ * 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.
+ */
+
+#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]);
+}
--- /dev/null
+/*
+ * 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.
+ */
+
+#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<CR><LF>
+ *
+ * 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]);
+}
}
#include "ao_gps_print.c"
-#include "ao_gps.c"
+#include "ao_gps_sirf.c"
void
ao_dump_state(void *wchan)
--- /dev/null
+/*
+ * 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>
+#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 <sys/time.h>
+
+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 <getopt.h>
+
+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 <tty-name>]\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();
+}
}
static void
-dump_log(void)
+dump_log(void) __reentrant
{
uint8_t more;
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)
{
ao_monitoring = monitoring;
ao_wakeup(&ao_monitoring);
+ ao_radio_abort();
}
static void
--- /dev/null
+/*
+ * 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.
+ */
+
+#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++];
+}
--- /dev/null
+/*
+ * 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.
+ */
+
+#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]);
+}
--- /dev/null
+/*
+ * 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.
+ */
+
+#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);
+}
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)
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,
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();
__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;
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]);
}
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;
/* .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
* 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++;
}
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;
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;
_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
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)
{
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();
}
ao_gps_report_init();
ao_telemetry_init();
ao_radio_init();
+ ao_packet_slave_init();
ao_igniter_init();
ao_config_init();
ao_start_scheduler();
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();
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 */
ao_adc_count = 0;
ao_adc_poll();
}
- ao_wakeup(DATA_TO_XDATA(&ao_tick_count));
}
void
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();
__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 */
}
void
-ao_usb_putchar(char c) __critical
+ao_usb_putchar(char c) __critical __reentrant
{
if (!ao_usb_running)
return;
}
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;
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)
{
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);
}
#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
--- /dev/null
+#!/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();
projects / fw / altos / commitdiff