5 files changed, 60 insertions, 1 deletions
diff --git a/src/avr/ao_pins.h b/src/avr/ao_pins.h
index 3c9010a8..bc423ff7 100644
--- a/src/avr/ao_pins.h
+++ b/src/avr/ao_pins.h
@@ -59,6 +59,8 @@
 
 	#define SPI_SLAVE_PIN_0_3	1
 	#define SPI_SLAVE_PIN_2_5	0
+
+	#define IS_COMPANION		1
 #endif
 
 #ifdef TELEPYRO
@@ -77,6 +79,7 @@
 	#define DISABLE_HELP		1
 	#define HAS_STORAGE_DEBUG	0
 	#define IS_COMPANION		1
+	#define HAS_ORIENT		0
 	#define ao_storage_pos_t	uint16_t
 
 	#define AVR_VCC_5V	       	0
@@ -89,6 +92,33 @@
 
 	#define SPI_SLAVE_PIN_0_3	1
 	#define SPI_SLAVE_PIN_2_5	0
+
+	#define AO_PYRO_NUM		8
+
+	#define AO_PYRO_PORT_0	B
+	#define AO_PYRO_PIN_0	5
+
+	#define AO_PYRO_PORT_1	B
+	#define AO_PYRO_PIN_1	6
+
+	#define AO_PYRO_PORT_2	B
+	#define AO_PYRO_PIN_2	7
+
+	#define AO_PYRO_PORT_3	C
+	#define AO_PYRO_PIN_3	6
+
+	#define AO_PYRO_PORT_4	C
+	#define AO_PYRO_PIN_4	7
+
+	#define AO_PYRO_PORT_5	D
+	#define AO_PYRO_PIN_5	5
+
+	#define AO_PYRO_PORT_6	D
+	#define AO_PYRO_PIN_6	3
+
+	#define AO_PYRO_PORT_7	D
+	#define AO_PYRO_PIN_7	2
+
 #endif
 
 #define AO_M25_SPI_CS_PORT	SPI_CS_PORT
diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c
index a1377d21..4f37e979 100644
--- a/src/core/ao_pyro.c
+++ b/src/core/ao_pyro.c
@@ -20,6 +20,15 @@
 #include <ao_sample.h>
 #include <ao_flight.h>
 
+#if IS_COMPANION
+#include <ao_companion.h>
+#define ao_accel ao_companion_command.accel
+#define ao_speed ao_companion_command.speed
+#define ao_height ao_companion_command.height
+#define ao_flight_state ao_companion_command.flight_state
+#define ao_motor_number ao_companion_command.motor_number
+#endif
+
 #define ao_lowbit(x)	((x) & (-x))
 
 /*
@@ -152,6 +161,8 @@ ao_pyro_fire(uint8_t p)
 	ao_delay(AO_MS_TO_TICKS(50));
 }
 
+uint8_t	ao_pyro_wakeup;
+
 static void
 ao_pyro(void)
 {
@@ -163,7 +174,9 @@ ao_pyro(void)
 		ao_sleep(&ao_flight_state);
 
 	for (;;) {
-		ao_delay(AO_MS_TO_TICKS(100));
+		ao_alarm(AO_MS_TO_TICKS(100));
+		ao_sleep(&ao_pyro_wakeup);
+		ao_clear_alarm();
 		for (p = 0; p < AO_PYRO_NUM; p++) {
 			pyro = &ao_config.pyro[p];
 
diff --git a/src/core/ao_pyro.h b/src/core/ao_pyro.h
index 1989a9e5..5deb69d0 100644
--- a/src/core/ao_pyro.h
+++ b/src/core/ao_pyro.h
@@ -57,6 +57,8 @@ struct ao_pyro {
 	uint8_t			fired;
 };
 
+extern uint8_t	ao_pyro_wakeup;
+
 void
 ao_pyro_set(void);
 
diff --git a/src/drivers/ao_pyro_slave.c b/src/drivers/ao_pyro_slave.c
index 70206a00..f07c2cba 100644
--- a/src/drivers/ao_pyro_slave.c
+++ b/src/drivers/ao_pyro_slave.c
@@ -18,6 +18,8 @@
 #include <ao.h>
 #include <ao_product.h>
 #include <ao_companion.h>
+#include <ao_flight.h>
+#include <ao_pyro.h>
 
 struct ao_companion_command	ao_companion_command;
 
@@ -28,6 +30,11 @@ static const struct ao_companion_setup	ao_telepyro_setup = {
 	.channels		= AO_TELEPYRO_NUM_ADC,
 };
 
+struct ao_config ao_config;
+
+extern volatile __data uint16_t ao_tick_count;
+uint16_t ao_boost_tick;
+
 void ao_spi_slave(void)
 {
 	if (!ao_spi_slave_recv((uint8_t *) &ao_companion_command,
@@ -45,6 +52,10 @@ void ao_spi_slave(void)
 				  AO_TELEPYRO_NUM_ADC * sizeof (uint16_t));
 		break;
 	case AO_COMPANION_NOTIFY:
+		/* Can't use ao_time because we have interrupts suspended */
+		if (ao_companion_command.flight_state < ao_flight_boost && ao_companion_command.flight_state >= ao_flight_boost)
+			ao_boost_tick = ao_tick_count;
+		ao_wakeup(&ao_pyro_wakeup);
 		break;
 	default:
 		return;
diff --git a/src/product/ao_telepyro.c b/src/product/ao_telepyro.c
index a2b8f83c..79454fb7 100644
--- a/src/product/ao_telepyro.c
+++ b/src/product/ao_telepyro.c
@@ -31,6 +31,9 @@ main(void)
 	ao_spi_slave_init();
 	ao_usb_init();
 	ao_adc_init();
+	ao_storage_init();
+	ao_config_init();
+	ao_pyro_init();
 	ao_start_scheduler();
 	return 0;
 }