path: root/ao-tools/ao-send-telem/ao-send-telem.c

/*
 * 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; 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.
 */

#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' },
	{ .name = "verbose", .has_arg = 0, .val = 'v' },
	{ .name = "fake", .has_arg = 0, .val = 'f' },
	{ 0, 0, 0, 0},
};

static void usage(char *program)
{
	fprintf(stderr, "usage: %s [--tty <tty-name>] [--device <device-name>] [--frequency <kHz>] [--realtime] [--verbose] [--fake] 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;
	case AO_TELEMETRY_METRUM_SENSOR:
		cur_state = telem->metrum_sensor.state;
		break;
	case AO_TELEMETRY_MINI:
		cur_state = telem->mini.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	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;
	FILE 		*file;
	uint16_t	last_tick;
	int		started;
	int		realtime = 0;
	int		verbose = 0;
	int		fake = 0;
	int		rate = 0;

	while ((c = getopt_long(argc, argv, "vRfT:D:F:r:", 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;
			break;
		case 'v':
			verbose++;
			break;
		case 'f':
			fake++;
			break;
		case 'r':
			rate = atoi(optarg);
			switch (rate) {
			case 38400:
				rate = 0;
				break;
			case 9600:
				rate = 1;
				break;
			case 2400:
				rate = 2;
				break;
			default:
				fprintf(stderr, "Rate %d isn't 38400, 9600 or 2400\n", rate);
				usage(argv[0]);
				break;
			}
			break;
		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, "c F %d\n", freq);
	cc_usb_printf(cc, "c T %d\n", rate);

	if (fake) {
		union ao_telemetry_all	telem;
		int			i;

		memset(&telem, '\0', sizeof (telem));
		telem.generic.serial = 1;
		telem.generic.type = 0;
		for (i = 0; i < sizeof (telem.generic.payload); i++)
			telem.generic.payload[i] = i & 7;
		for (;;) {
			telem.generic.tick += 50;
			send_telem(cc, &telem);
			do_delay(50, 0);
		}
	} else {
		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 (verbose)
						printf ("type %4d\n", telem.generic.type);

					if (started || realtime) {
						do_delay(telem.generic.tick, last_tick);
						last_tick = telem.generic.tick;
						send_telem(cc, &telem);
					} else {
						enum ao_flight_state state = packet_state(&telem);
						printf ("\tstate %4d\n", state);
						add_telem(&telem);
						if (ao_flight_pad < state && state < ao_flight_landed) {
							printf ("started\n");
							started = 1;
							last_tick = send_queued(cc, realtime);
						}
					}
				}
			}
			fclose (file);

		}
	}
	return ret;
}