From 59588ba34159b27c02e1a886b46497ecfa0cf4d3 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 29 Jul 2012 16:22:23 -0700 Subject: [PATCH] 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 --- ao-tools/Makefile.am | 2 +- ao-tools/ao-send-telem/Makefile.am | 12 ++ ao-tools/ao-send-telem/ao-send-telem.1 | 64 +++++++ ao-tools/ao-send-telem/ao-send-telem.c | 238 +++++++++++++++++++++++++ configure.ac | 1 + src/core/ao_send_packet.c | 74 ++++++++ src/core/ao_send_packet.h | 24 +++ src/product/Makefile.teledongle | 1 + src/product/ao_teledongle.c | 2 + 9 files changed, 417 insertions(+), 1 deletion(-) create mode 100644 ao-tools/ao-send-telem/Makefile.am create mode 100644 ao-tools/ao-send-telem/ao-send-telem.1 create mode 100644 ao-tools/ao-send-telem/ao-send-telem.c create mode 100644 src/core/ao_send_packet.c create mode 100644 src/core/ao_send_packet.h 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 +.\" +.\" 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] + +.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 + * + * 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 +#include +#include +#include +#include +#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 ] [--device ] [--frequency ] [--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 + * + * 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 \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 + * + * 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 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 -- 2.30.2