ao-tools: Add ao-edit-telem

This lets you edit a telemetry file. The only current editing
available is to change the pad location, allowing a flight to be
replayed anywhere in the world.

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

4 files changed, 238 insertions, 1 deletions

diff --git a/ao-tools/Makefile.am b/ao-tools/Makefile.am
index 139eea3f..e4df2e63 100644
--- a/ao-tools/Makefile.am
+++ b/ao-tools/Makefile.am
@@ -1,3 +1,3 @@
 SUBDIRS=lib ao-rawload ao-dbg ao-bitbang ao-eeprom ao-list \
 	ao-load ao-telem ao-stmload ao-send-telem ao-sky-flash \
-	ao-dumpflash
+	ao-dumpflash ao-edit-telem
diff --git a/ao-tools/ao-edit-telem/Makefile.am b/ao-tools/ao-edit-telem/Makefile.am
new file mode 100644
index 00000000..c5965c47
--- /dev/null
+++ b/ao-tools/ao-edit-telem/Makefile.am
@@ -0,0 +1,12 @@
+bin_PROGRAMS=ao-edit-telem
+
+AM_CFLAGS=-I$(top_srcdir)/ao-tools/lib $(LIBUSB_CFLAGS)
+AO_POSTFLIGHT_LIBS=$(top_builddir)/ao-tools/lib/libao-tools.a
+
+ao_edit_telem_DEPENDENCIES = $(AO_POSTFLIGHT_LIBS)
+
+ao_edit_telem_LDADD=$(AO_POSTFLIGHT_LIBS) $(LIBUSB_LIBS)
+
+ao_edit_telem_SOURCES = ao-edit-telem.c
+
+man_MANS = ao-edit-telem.1
diff --git a/ao-tools/ao-edit-telem/ao-edit-telem.1 b/ao-tools/ao-edit-telem/ao-edit-telem.1
new file mode 100644
index 00000000..8f125878
--- /dev/null
+++ b/ao-tools/ao-edit-telem/ao-edit-telem.1
@@ -0,0 +1,33 @@
+.\"
+.\" Copyright © 2013 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-EDIT-TELEM 1 "ao-edit-telem" ""
+.SH NAME
+ao-edit-telem \- Edit telemetry file, creating new telemetry stream
+.SH SYNOPSIS
+.B "ao-edit-telem"
+[\--lat=<pad-lat>]
+[\--lon=<pad-lon>]
+{flight.telem}
+.SH DESCRIPTION
+.I ao-edit-telem
+reads the specified telemetry log and produces a new telemetry log,
+changed as directed by the options provided.
+output.
+.SH AUTHOR
+Keith Packard
diff --git a/ao-tools/ao-edit-telem/ao-edit-telem.c b/ao-tools/ao-edit-telem/ao-edit-telem.c
new file mode 100644
index 00000000..3f6830e7
--- /dev/null
+++ b/ao-tools/ao-edit-telem/ao-edit-telem.c
@@ -0,0 +1,192 @@
+/*
+ * Copyright © 2013 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"
+
+static const struct option options[] = {
+	{ .name = "lat", .has_arg = 1, .val = 'L' },
+	{ .name = "lon", .has_arg = 1, .val = 'l' },
+	{ 0, 0, 0, 0},
+};
+
+static void usage(char *program)
+{
+	fprintf(stderr, "usage: %s [--lat <pad-lat>] [--lon <pad-lon>]\n"
+		"\t{flight-log} ...\n", program);
+	exit(1);
+}
+
+#define bool(b)	((b) ? "true" : "false")
+
+struct telem_ent {
+	struct telem_ent	*next;
+	union ao_telemetry_all	telem;
+};
+
+static struct telem_ent *pad, **last = &pad;
+
+static void
+save_telem(union ao_telemetry_all *telem)
+{
+	struct telem_ent *t = malloc (sizeof *t);
+	t->telem = *telem;
+	t->next = NULL;
+	*last = t;
+	last = &t->next;
+}
+
+static void
+dump_telem(union ao_telemetry_all *telem)
+{
+	char s[CC_TELEMETRY_BUFSIZE];
+
+	cc_telemetry_unparse(telem, s);
+	printf("%s\n", s);
+}
+
+double	pad_lat = 0, pad_lon = 0;
+double	target_pad_lat = 0, target_pad_lon = 0;
+double	lat_off = 0, lon_off = 0;
+int pending = 1;
+
+static void
+dump_saved(void);
+
+void
+doit(union ao_telemetry_all *telem)
+{
+	double lat, lon;
+
+	switch (telem->generic.type) {
+	case AO_TELEMETRY_SENSOR_TELEMETRUM:
+	case AO_TELEMETRY_SENSOR_TELEMINI:
+	case AO_TELEMETRY_SENSOR_TELENANO:
+		if (telem->sensor.state > ao_flight_pad && pad) {
+			pending = 0;
+			if (target_pad_lat)
+				lat_off = target_pad_lat - pad_lat;
+			if (target_pad_lon)
+				lon_off = target_pad_lon - pad_lon;
+			dump_saved();
+		}
+		break;
+	case AO_TELEMETRY_LOCATION: {
+		lat = telem->location.latitude / 1.0e7;
+		lon = telem->location.longitude / 1.0e7;
+		if (pending) {
+			if (telem->location.flags & (1 << 4)) {
+				if (pad_lat) {
+					pad_lat = pad_lat - pad_lat / 32 + lat / 32.0;
+					pad_lon = pad_lon - pad_lon / 32 + lon / 32.0;
+				} else {
+					pad_lat = lat;
+					pad_lon = lon;
+				}
+			}
+		} else {
+			lat += lat_off;
+			lon += lon_off;
+			if (lat > 90)
+				lat = 90;
+			if (lat < -90)
+				lat = -90;
+			while (lon > 180)
+				lon -= 360;
+			while (lon < -180)
+				lon += 360;
+			telem->location.latitude = lat * 1.0e7;
+			telem->location.longitude = lon * 1.0e7;
+		}
+		break;
+	}
+	}
+}
+
+static void
+dump_saved(void)
+{
+	struct telem_ent	*t, *n;
+
+	for (t = pad; t; t = n) {
+		n = t->next;
+		doit(&t->telem);
+		dump_telem(&t->telem);
+		free(t);
+	}
+	pad = NULL;
+	last = &pad;
+}
+
+int
+main (int argc, char **argv)
+{
+	char	line[80];
+	int c, i, ret;
+	char *s;
+	FILE *file;
+	int serial;
+	while ((c = getopt_long(argc, argv, "l:L:", options, NULL)) != -1) {
+		switch (c) {
+		case 'L':
+			target_pad_lat = strtod(optarg, NULL);
+			break;
+		case 'l':
+			target_pad_lon = strtod(optarg, NULL);
+			break;
+		default:
+			usage(argv[0]);
+			break;
+		}
+	}
+	for (i = optind; i < argc; i++) {
+		file = fopen(argv[i], "r");
+		if (!file) {
+			perror(argv[i]);
+			ret++;
+			continue;
+		}
+		s = strstr(argv[i], "-serial-");
+		if (s)
+			serial = atoi(s + 8);
+		else
+			serial = 0;
+		while (fgets(line, sizeof (line), file)) {
+			union ao_telemetry_all telem;
+
+			if (cc_telemetry_parse(line, &telem)) {
+				if ((telem.generic.status & (1 << 7)) == 0) {
+					dump_telem(&telem);
+					continue;
+				}
+				doit (&telem);
+				if (pending)
+					save_telem(&telem);
+				else
+					dump_telem(&telem);
+			}
+		}
+		fclose (file);
+
+	}
+	return ret;
+}