altos: Finish up primitive telelaunch protocol

This adds two commands to teledongle, one to query and one to actually
fire the igniter. These will (eventually) want to be replaced with
something nicer.

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

3 files changed, 111 insertions, 36 deletions

diff --git a/src/ao_launch.c b/src/ao_launch.c
index 6c154719..35afbda8 100644
--- a/src/ao_launch.c
+++ b/src/ao_launch.c
@@ -19,6 +19,12 @@
 
 __xdata uint16_t ao_launch_ignite;
 
+#if 0
+#define PRINTD(...) printf(__VA_ARGS__)
+#else
+#define PRINTD(...) 
+#endif
+
 static void
 ao_launch_run(void)
 {
@@ -71,19 +77,20 @@ ao_launch(void)
 	ao_led_off(AO_LED_RED);
 	ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200));
 	for (;;) {
+		flush();
 		if (ao_radio_cmac_recv(&command, sizeof (command), 0) != AO_RADIO_CMAC_OK)
 			continue;
 		
-		printf ("tick %d serial %d cmd %d channel %d\n",
+		PRINTD ("tick %d serial %d cmd %d channel %d\n",
 			command.tick, command.serial, command.cmd, command.channel);
 
-		if (command.serial != ao_serial_number) {
-			printf ("serial number mismatch\n");
-			continue;
-		}
-
 		switch (command.cmd) {
 		case AO_LAUNCH_QUERY:
+			if (command.serial != ao_serial_number) {
+				PRINTD ("serial number mismatch\n");
+				break;
+			}
+
 			if (command.channel == 0) {
 				query.valid = 1;
 				query.arm_status = ao_igniter_status(ao_igniter_drogue);
@@ -94,35 +101,38 @@ ao_launch(void)
 			query.tick = ao_time();
 			query.serial = ao_serial_number;
 			query.channel = command.channel;
-			printf ("query tick %d serial %d channel %d valid %d arm %d igniter %d\n",
+			PRINTD ("query tick %d serial %d channel %d valid %d arm %d igniter %d\n",
 				query.tick, query.serial, query.channel, query.valid, query.arm_status,
 				query.igniter_status);
 			ao_radio_cmac_send(&query, sizeof (query));
 			break;
 		case AO_LAUNCH_ARM:
+			if (command.serial != ao_serial_number) {
+				PRINTD ("serial number mismatch\n");
+				break;
+			}
+
 			if (command.channel != 0)
 				break;
 			time_difference = command.tick - ao_time();
-			printf ("arm tick %d local tick %d\n", command.tick, ao_time());
+			PRINTD ("arm tick %d local tick %d\n", command.tick, ao_time());
 			if (time_difference < 0)
 				time_difference = -time_difference;
 			if (time_difference > 10) {
-				printf ("time difference too large %d\n", time_difference);
+				PRINTD ("time difference too large %d\n", time_difference);
 				break;
 			}
-			printf ("armed\n");
+			PRINTD ("armed\n");
 			ao_launch_armed = 1;
 			ao_launch_arm_time = ao_time();
 			break;
 		case AO_LAUNCH_FIRE:
-			if (command.channel != 0)
-				break;
 			if (!ao_launch_armed) {
-				printf ("not armed\n");
+				PRINTD ("not armed\n");
 				break;
 			}
-			if ((uint16_t) (ao_launch_arm_time - ao_time()) > AO_SEC_TO_TICKS(20)) {
-				printf ("late launch arm_time %d time %d\n",
+			if ((uint16_t) (ao_time() - ao_launch_arm_time) > AO_SEC_TO_TICKS(20)) {
+				PRINTD ("late launch arm_time %d time %d\n",
 					ao_launch_arm_time, ao_time());
 				break;
 			}
@@ -130,10 +140,10 @@ ao_launch(void)
 			if (time_difference < 0)
 				time_difference = -time_difference;
 			if (time_difference > 10) {
-				printf ("time different too large %d\n", time_difference);
+				PRINTD ("time different too large %d\n", time_difference);
 				break;
 			}
-			printf ("ignite\n");
+			PRINTD ("ignite\n");
 			ao_launch_ignite = 1;
 			ao_wakeup(&ao_launch_ignite);
 			break;
diff --git a/src/ao_radio_cmac.c b/src/ao_radio_cmac.c
index 9694b5b3..c2757b16 100644
--- a/src/ao_radio_cmac.c
+++ b/src/ao_radio_cmac.c
@@ -227,14 +227,37 @@ radio_cmac_recv_cmd(void) __reentrant
 	ao_mutex_put(&ao_radio_cmac_mutex);
 }
 
+static __xdata struct ao_launch_command	command;
+static __xdata struct ao_launch_query	query;
+
+
+static int8_t
+launch_query(uint16_t serial, uint8_t channel)
+{
+	uint8_t	i;
+	int8_t	r = AO_RADIO_CMAC_OK;
+
+	for (i = 0; i < 10; i++) {
+		printf ("."); flush();
+		command.tick = ao_time();
+		command.serial = serial;
+		command.cmd = AO_LAUNCH_QUERY;
+		command.channel = channel;
+		ao_radio_cmac_send(&command, sizeof (command));
+		r = ao_radio_cmac_recv(&query, sizeof (query), AO_MS_TO_TICKS(500));
+		if (r == AO_RADIO_CMAC_OK)
+			break;
+	}
+	printf("\n"); flush();
+	return r;
+}
+
 static void
 launch_report_cmd(void) __reentrant
 {
-	static __xdata struct ao_launch_command	command;
-	static __xdata struct ao_launch_query	query;
 	uint8_t		channel;
 	uint16_t	serial;
-	uint8_t		i;
+	int8_t		r;
 
 	ao_cmd_decimal();
 	serial = ao_cmd_lex_i;
@@ -242,17 +265,10 @@ launch_report_cmd(void) __reentrant
 	channel = ao_cmd_lex_i;
 	if (ao_cmd_status != ao_cmd_success)
 		return;
-	flush();
-	for (i = 0; i < 10; i++) {
-		printf ("."); flush();
-		command.tick = 0;
-		command.serial = serial;
-		command.cmd = AO_LAUNCH_QUERY;
-		command.channel = channel;
-		ao_radio_cmac_send(&command, sizeof (command));
-		switch (ao_radio_cmac_recv(&query, sizeof (query), AO_MS_TO_TICKS(500))) {
-		case AO_RADIO_CMAC_OK:
-			printf("\n");
+	r = launch_query(serial, channel);
+	switch (r) {
+	case AO_RADIO_CMAC_OK:
+		if (query.valid) {
 			switch (query.arm_status) {
 			case ao_igniter_ready:
 			case ao_igniter_active:
@@ -268,21 +284,70 @@ launch_report_cmd(void) __reentrant
 					printf("igniter bad\n");
 					break;
 				}
+				break;
 			default:
 				printf("Disarmed\n");
 			}
-			return;
-		default:
-			continue;
+		} else {
+			printf("Invalid channel %d\n", channel);
 		}
+		break;
+	default:
+		printf("Error %d\n", r);
+		break;
+	}
+}
+
+static void
+launch_fire_cmd(void) __reentrant
+{
+	static __xdata struct ao_launch_command	command;
+	uint8_t		channel;
+	uint16_t	serial;
+	uint8_t		secs;
+	uint8_t		i;
+	int8_t		r;
+	uint16_t	tick_offset;
+
+	ao_cmd_decimal();
+	serial = ao_cmd_lex_i;
+	ao_cmd_decimal();
+	channel = ao_cmd_lex_i;
+	ao_cmd_decimal();
+	secs = ao_cmd_lex_i;
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	tick_offset = ao_time();
+	r = launch_query(serial, channel);
+	tick_offset -= query.tick;
+
+	for (i = 0; i < 4; i++) {
+		printf("arm %d\n", i); flush();
+		command.tick = ao_time() - tick_offset;
+		command.serial = serial;
+		command.cmd = AO_LAUNCH_ARM;
+		command.channel = channel;
+		ao_radio_cmac_send(&command, sizeof (command));
+	}
+	secs = secs * 10 - 5;
+	if (secs > 100)
+		secs = 100;
+	for (i = 0; i < secs; i++) {
+		printf("fire %d\n", i); flush();
+		command.tick = ao_time() - tick_offset;
+		command.serial = serial;
+		command.cmd = AO_LAUNCH_FIRE;
+		command.channel = 0;
+		ao_radio_cmac_send(&command, sizeof (command));
+		ao_delay(AO_MS_TO_TICKS(100));
 	}
-	printf ("Timeout\n");
 }
 
 static __code struct ao_cmds ao_radio_cmac_cmds[] = {
 	{ radio_cmac_send_cmd,	"s <length>\0Send AES-CMAC packet. Bytes to send follow on next line" },
 	{ radio_cmac_recv_cmd,	"S <length> <timeout>\0Receive AES-CMAC packet. Timeout in ms" },
 	{ launch_report_cmd,    "l <serial> <channel>\0Get remote launch status" },
+	{ launch_fire_cmd,	"f <serial> <channel> <secs>\0Fire remote igniter" },
 	{ 0, NULL },
 };
 
diff --git a/src/core/ao_config.c b/src/core/ao_config.c
index 7f999feb..28854ab5 100644
--- a/src/core/ao_config.c
+++ b/src/core/ao_config.c
@@ -108,7 +108,7 @@ _ao_config_get(void)
 		if (ao_config.minor < 8)
 			ao_config.radio_enable = TRUE;
 		if (ao_config.minor < 9)
-			memset(&ao_config.aes_key, '\0', AO_AES_LEN);
+			memset(&ao_config.aes_key, 0, AO_AES_LEN);
 		ao_config.minor = AO_CONFIG_MINOR;
 		ao_config_dirty = 1;
 	}