Add ability to re-play telemetry through TeleDongle

This adds a new command to TeleDongle to send arbitrary data, and then
creates a new tool, 'ao-send-telem' that replays existing telemetry
files through TeleDongle.

Signed-off-by: Keith Packard <keithp@keithp.com>

9 files changed, 417 insertions, 1 deletions

diff --git a/ao-tools/Makefile.am b/ao-tools/Makefile.am
index 6c315dd1..257fdaec 100644
--- a/ao-tools/Makefile.am
+++ b/ao-tools/Makefile.am
@@ -1 +1 @@
-SUBDIRS=lib ao-rawload ao-dbg ao-bitbang ao-eeprom ao-list ao-load ao-telem ao-stmload
+SUBDIRS=lib ao-rawload ao-dbg ao-bitbang ao-eeprom ao-list ao-load ao-telem ao-stmload ao-send-telem
diff --git a/ao-tools/ao-send-telem/Makefile.am b/ao-tools/ao-send-telem/Makefile.am
new file mode 100644
index 00000000..bfddf131
--- /dev/null
+++ b/ao-tools/ao-send-telem/Makefile.am
@@ -0,0 +1,12 @@
+bin_PROGRAMS=ao-send-telem
+
+AM_CFLAGS=-I$(top_srcdir)/ao-tools/lib $(LIBUSB_CFLAGS)
+AO_POSTFLIGHT_LIBS=$(top_builddir)/ao-tools/lib/libao-tools.a
+
+ao_send_telem_DEPENDENCIES = $(AO_POSTFLIGHT_LIBS)
+
+ao_send_telem_LDADD=$(AO_POSTFLIGHT_LIBS) $(LIBUSB_LIBS)
+
+ao_send_telem_SOURCES = ao-send-telem.c
+
+man_MANS = ao-send-telem.1
diff --git a/ao-tools/ao-send-telem/ao-send-telem.1 b/ao-tools/ao-send-telem/ao-send-telem.1
new file mode 100644
index 00000000..fbdb2fe9
--- /dev/null
+++ b/ao-tools/ao-send-telem/ao-send-telem.1
@@ -0,0 +1,64 @@
+.\"
+.\" 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; either version 2 of the License, or
+.\" (at your option) any later version.
+.\"
+.\" 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.
+.\"
+.\"
+.TH AO-SEND-TELEM 1 "ao-send-telem" ""
+.SH NAME
+ao-send-telem \- Re-transmit stored telemetry file
+.SH SYNOPSIS
+.B "ao-send-telem"
+[\-T \fItty-device\fP]
+[\--tty \fItty-device\fP]
+[\-D \fIaltos-device\fP]
+[\--device \fIaltos-device\fP]
+[\-F \fIfrequency (kHz)\fP]
+[\--frequency \fIfrequency (kHz)\fP]
+[\-R]
+[\--realtime]
+<flight.telem>
+.SH OPTIONS
+.TP
+\-T tty-device | --tty tty-device
+This selects which tty device ao-dumplog uses to communicate with
+the target device.
+.TP
+\-D AltOS-device | --device AltOS-device
+Search for a connected device. This requires an argument of one of the
+following forms:
+.IP
+TeleDongle:2
+.br
+TeleDongle
+.br
+2
+.IP
+Leaving out the product name will cause the tool to select a suitable
+product, leaving out the serial number will cause the tool to match
+one of the available devices.
+.TP
+\-F kHz | --frequency kHz
+This selects which frequency to send the specified packets on.
+.TP
+\-R | --realtime
+This makes the program delay between packets in pad mode. Normally,
+pad mode packets are sent as quickly as possible.
+.SH DESCRIPTION
+.I ao-send-telem
+reads the specified flight telemetry log and re-transmits it via the
+specified ground station device
+.SH AUTHOR
+Keith Packard
diff --git a/ao-tools/ao-send-telem/ao-send-telem.c b/ao-tools/ao-send-telem/ao-send-telem.c
new file mode 100644
index 00000000..c4c354e0
--- /dev/null
+++ b/ao-tools/ao-send-telem/ao-send-telem.c
@@ -0,0 +1,238 @@
+/*
+ * Copyright © 2011 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#define _GNU_SOURCE
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <getopt.h>
+#include "cc.h"
+#include "cc-usb.h"
+
+static const struct option options[] = {
+	{ .name = "tty", .has_arg = 1, .val = 'T' },
+	{ .name = "device", .has_arg = 1, .val = 'D' },
+	{ .name = "frequency", .has_arg = 1, .val = 'F' },
+	{ .name = "realtime", .has_arg = 0, .val = 'R' },
+	{ 0, 0, 0, 0},
+};
+
+static void usage(char *program)
+{
+	fprintf(stderr, "usage: %s [--tty <tty-name>] [--device <device-name>] [--frequency <kHz>] [--realtime] file.telem ...\n", program);
+	exit(1);
+}
+
+#define bool(b)	((b) ? "true" : "false")
+
+struct ao_telem_list {
+	struct ao_telem_list	*next;
+	union ao_telemetry_all	telem;
+};
+
+static struct ao_telem_list	*telem_list, **telem_last;
+
+static void
+trim_telem(uint16_t time)
+{
+	while (telem_list && (int16_t) (time - telem_list->telem.generic.tick) > 0) {
+		struct ao_telem_list	*next = telem_list->next;
+		free(telem_list);
+		telem_list = next;
+	}
+	if (!telem_list)
+		telem_last = &telem_list;
+}
+
+static void
+add_telem(union ao_telemetry_all *telem)
+{
+	struct ao_telem_list	*new = malloc (sizeof (struct ao_telem_list));
+	trim_telem((uint16_t) (telem->generic.tick - 20 * 100));
+	new->telem = *telem;
+	new->next = 0;
+	*telem_last = new;
+	telem_last = &new->next;
+}
+
+static enum ao_flight_state	cur_state = ao_flight_invalid;
+static enum ao_flight_state	last_state = ao_flight_invalid;
+
+static enum ao_flight_state
+packet_state(union ao_telemetry_all *telem)
+{
+	switch (telem->generic.type) {
+	case AO_TELEMETRY_SENSOR_TELEMETRUM:
+	case AO_TELEMETRY_SENSOR_TELEMINI:
+	case AO_TELEMETRY_SENSOR_TELENANO:
+		cur_state = telem->sensor.state;
+		break;
+	case AO_TELEMETRY_MEGA_DATA:
+		cur_state = telem->mega_data.state;
+		break;
+	}
+	return cur_state;
+}
+
+static const char *state_names[] = {
+	"startup",
+	"idle",
+	"pad",
+	"boost",
+	"fast",
+	"coast",
+	"drogue",
+	"main",
+	"landed",
+	"invalid"
+};
+
+static void
+send_telem(struct cc_usb *cc, union ao_telemetry_all *telem)
+{
+	int 	rssi = (int8_t) telem->generic.rssi / 2 - 74;
+	int	i;
+	uint8_t	*b;
+
+	packet_state(telem);
+	if (cur_state != last_state) {
+		if (0 <= cur_state && cur_state < sizeof(state_names) / sizeof (state_names[0]))
+			printf ("%s\n", state_names[cur_state]);
+		last_state = cur_state;
+	}
+	cc_usb_printf(cc, "S 20\n");
+	b = (uint8_t *) telem;
+	for (i = 0; i < 0x20; i++)
+		cc_usb_printf(cc, "%02x", b[i]);
+	cc_usb_sync(cc);
+}	
+
+static void
+do_delay(uint16_t now, uint16_t then)
+{
+	int16_t	delay = (int16_t) (now - then);
+
+	if (delay > 0 && delay < 1000)
+		usleep(delay * 10 * 1000);
+}
+
+static uint16_t
+send_queued(struct cc_usb *cc, int pause)
+{
+	struct ao_telem_list	*next;
+	uint16_t		tick = 0;
+	int			started = 0;
+
+	while (telem_list) {
+		if (started && pause)
+			do_delay(telem_list->telem.generic.tick, tick);
+		tick = telem_list->telem.generic.tick;
+		started = 1;
+		send_telem(cc, &telem_list->telem);
+
+		next = telem_list->next;
+		free(telem_list);
+		telem_list = next;
+	}
+	return tick;
+}
+
+int
+main (int argc, char **argv)
+{
+	struct cc_usb	*cc;
+	char		*tty = NULL;
+	char		*device = NULL;
+	char		line[80];
+	int 		c, i, ret = 0;
+	int		freq = 434550;
+	char 		*s;
+	FILE 		*file;
+	int 		serial;
+	uint16_t	last_tick;
+	int		started;
+	int		realtime = 0;
+      
+
+	while ((c = getopt_long(argc, argv, "RT:D:F:", options, NULL)) != -1) {
+		switch (c) {
+		case 'T':
+			tty = optarg;
+			break;
+		case 'D':
+			device = optarg;
+			break;
+		case 'F':
+			freq = atoi(optarg);
+			break;
+		case 'R':
+			realtime = 1;
+		default:
+			usage(argv[0]);
+			break;
+		}
+	}
+	if (!tty)
+		tty = cc_usbdevs_find_by_arg(device, "TeleDongle");
+	if (!tty)
+		tty = getenv("ALTOS_TTY");
+	if (!tty)
+		tty="/dev/ttyACM0";
+	cc = cc_usb_open(tty);
+	if (!cc)
+		exit (1);
+
+	cc_usb_printf(cc, "m 0\n");
+	cc_usb_printf(cc, "F %d\n", freq);
+	for (i = optind; i < argc; i++) {
+		file = fopen(argv[i], "r");
+		if (!file) {
+			perror(argv[i]);
+			ret++;
+			continue;
+		}
+		started = 0;
+		last_tick = 0;
+		while (fgets(line, sizeof (line), file)) {
+			union ao_telemetry_all telem;
+
+			if (cc_telemetry_parse(line, &telem)) {
+				/*
+				 * Skip packets with CRC errors.
+				 */
+				if ((telem.generic.status & (1 << 7)) == 0)
+					continue;
+
+				if (started) {
+					do_delay(telem.generic.tick, last_tick);
+					last_tick = telem.generic.tick;
+					send_telem(cc, &telem);
+				} else {
+					add_telem(&telem);
+					if (packet_state(&telem) > ao_flight_pad) {
+						started = 1;
+						last_tick = send_queued(cc, realtime);
+					}
+				}
+			}
+		}
+		fclose (file);
+
+	}
+	return ret;
+}
diff --git a/configure.ac b/configure.ac
index bcd2dcac..c59261af 100644
--- a/configure.ac
+++ b/configure.ac
@@ -160,6 +160,7 @@ ao-tools/ao-list/Makefile
 ao-tools/ao-load/Makefile
 ao-tools/ao-telem/Makefile
 ao-tools/ao-stmload/Makefile
+ao-tools/ao-send-telem/Makefile
 ao-utils/Makefile
 src/Version
 ])
diff --git a/src/core/ao_send_packet.c b/src/core/ao_send_packet.c
new file mode 100644
index 00000000..1a8e74de
--- /dev/null
+++ b/src/core/ao_send_packet.c
@@ -0,0 +1,74 @@
+/*
+ * Copyright © 2012 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"
+
+#define AO_MAX_SEND	128
+
+static __xdata uint8_t ao_send[AO_MAX_SEND];
+
+static uint8_t
+getnibble(void)
+{
+	char	c;
+
+	c = getchar();
+	if ('0' <= c && c <= '9')
+		return c - '0';
+	if ('a' <= c && c <= 'f')
+		return c - ('a' - 10);
+	if ('A' <= c && c <= 'F')
+		return c - ('A' - 10);
+	ao_cmd_status = ao_cmd_lex_error;
+	return 0;
+}
+
+static void
+ao_send_packet(void)
+{
+	__pdata uint16_t count;
+	uint8_t b;
+	__pdata uint8_t	i;
+
+	ao_cmd_hex();
+	count = ao_cmd_lex_i;
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	if (count > AO_MAX_SEND - 2) {
+		ao_cmd_status = ao_cmd_syntax_error;
+		return;
+	}
+	for (i = 0; i < count; i++) {
+		b = getnibble() << 4;
+		b |= getnibble();
+		if (ao_cmd_status != ao_cmd_success)
+			return;
+		ao_send[i] = b;
+	}
+	ao_radio_send(ao_send, count);
+}
+
+static __code struct ao_cmds ao_send_packet_cmds[] = {
+	{ ao_send_packet, "S <len>\0Send packet. Data on next line" },
+	{ 0, NULL }
+};
+
+void
+ao_send_packet_init(void)
+{
+	ao_cmd_register(&ao_send_packet_cmds[0]);
+}
diff --git a/src/core/ao_send_packet.h b/src/core/ao_send_packet.h
new file mode 100644
index 00000000..526f7b55
--- /dev/null
+++ b/src/core/ao_send_packet.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright © 2012 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_SEND_PACKET_H_
+#define _AO_SEND_PACKET_H_
+
+void
+ao_send_packet_init(void);
+
+#endif /* _AO_SEND_PACKET_H_ */
diff --git a/src/product/Makefile.teledongle b/src/product/Makefile.teledongle
index 5105b567..3101b777 100644
--- a/src/product/Makefile.teledongle
+++ b/src/product/Makefile.teledongle
@@ -43,6 +43,7 @@ CC1111_SRC = \
 	ao_packet.c \
 	ao_packet_master.c \
 	ao_radio.c \
+	ao_send_packet.c \
 	ao_romconfig.c \
 	ao_string.c \
 	ao_timer.c \
diff --git a/src/product/ao_teledongle.c b/src/product/ao_teledongle.c
index 0c829e97..25ebe73e 100644
--- a/src/product/ao_teledongle.c
+++ b/src/product/ao_teledongle.c
@@ -16,6 +16,7 @@
  */
 
 #include "ao.h"
+#include <ao_send_packet.h>
 
 void
 main(void)
@@ -32,6 +33,7 @@ main(void)
 	ao_rssi_init(AO_LED_RED);
 	ao_radio_init();
 	ao_packet_master_init();
+	ao_send_packet_init();
 #if HAS_DBG
 	ao_dbg_init();
 #endif