Merge branch 'master' into branch-1.6

11 files changed, 812 insertions, 49 deletions

diff --git a/src/drivers/ao_button.c b/src/drivers/ao_button.c
index cdf07352..8e7dead7 100644
--- a/src/drivers/ao_button.c
+++ b/src/drivers/ao_button.c
@@ -18,7 +18,6 @@
 #include <ao.h>
 #include <ao_button.h>
 #include <ao_exti.h>
-#include <ao_debounce.h>
 #if AO_EVENT
 #include <ao_event.h>
 #define ao_button_queue(b,v)	ao_event_put_isr(AO_EVENT_BUTTON, b, v)
@@ -26,9 +25,14 @@
 #define ao_button_queue(b,v)
 #endif
 
-#define AO_BUTTON_DEBOUNCE_HOLD	10
+#define AO_BUTTON_DEBOUNCE_INTERVAL	AO_MS_TO_TICKS(50)
 
-static struct ao_debounce	ao_button_debounce[AO_BUTTON_COUNT];
+struct ao_button_state {
+	AO_TICK_TYPE	time;
+	uint8_t		value;
+};
+
+static struct ao_button_state	ao_button_state[AO_BUTTON_COUNT];
 
 #define port(q)	AO_BUTTON_ ## q ## _PORT
 #define bit(q) AO_BUTTON_ ## q
@@ -38,10 +42,8 @@ static struct ao_debounce	ao_button_debounce[AO_BUTTON_COUNT];
 #define ao_button_value(b)	!ao_gpio_get(port(b), bit(b), pin(b))
 
 static uint8_t
-_ao_button_get(struct ao_debounce *debounce)
+_ao_button_get(uint8_t b)
 {
-	uint8_t	b = debounce - ao_button_debounce;
-
 	switch (b) {
 #if AO_BUTTON_COUNT > 0
 	case 0: return ao_button_value(0);
@@ -63,22 +65,31 @@ _ao_button_get(struct ao_debounce *debounce)
 }
 
 static void
-_ao_button_set(struct ao_debounce *debounce, uint8_t value)
+_ao_button_check(uint8_t b)
 {
-	uint8_t b = debounce - ao_button_debounce;
-
-	ao_button_queue(b, value);
-}
+	uint8_t	value = _ao_button_get(b);
 
+	if (value != ao_button_state[b].value) {
+		AO_TICK_TYPE	now = ao_time();
 
-#define ao_button_update(b)	ao_button_do(b, ao_gpio_get(port(b), bit(b), pin(b)))
+		if ((now - ao_button_state[b].time) >= AO_BUTTON_DEBOUNCE_INTERVAL) {
+			ao_button_state[b].value = value;
+			ao_button_queue(b, value);
+		}
+		ao_button_state[b].time = now;
+	}
+}
 
 static void
-ao_button_debounce_init(struct ao_debounce *debounce) {
-	ao_debounce_config(debounce,
-			   _ao_button_get,
-			   _ao_button_set,
-			   AO_BUTTON_DEBOUNCE_HOLD);
+_ao_button_init(uint8_t b)
+{
+	uint8_t	m = ao_arch_irqsave();
+	uint8_t value = _ao_button_get(b);
+	ao_button_state[b].value = value;
+	ao_button_state[b].time = ao_time();
+	ao_button_queue(b, value);
+	ao_arch_irqrestore(m);
+
 }
 
 static void
@@ -87,17 +98,17 @@ ao_button_isr(void)
 	uint8_t	b;
 
 	for (b = 0; b < AO_BUTTON_COUNT; b++)
-		_ao_debounce_start(&ao_button_debounce[b]);
+		_ao_button_check(b);
 }
 
 #define init(b) do {							\
-		ao_button_debounce_init(&ao_button_debounce[b]);	\
 		ao_enable_port(port(b));				\
 									\
 		ao_exti_setup(port(b), bit(b),				\
 			      AO_BUTTON_MODE|AO_EXTI_MODE_FALLING|AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_MED, \
-			      ao_button_isr);			\
+			      ao_button_isr);				\
 		ao_exti_enable(port(b), bit(b));			\
+		_ao_button_init(b);					\
 	} while (0)
 
 void
@@ -118,5 +129,4 @@ ao_button_init(void)
 #if AO_BUTTON_COUNT > 4
 	init(4);
 #endif
-	ao_debounce_init();
 }
diff --git a/src/drivers/ao_cc1200.c b/src/drivers/ao_cc1200.c
index 6547be39..6bccb188 100644
--- a/src/drivers/ao_cc1200.c
+++ b/src/drivers/ao_cc1200.c
@@ -20,6 +20,9 @@
 #include <ao_exti.h>
 #include <ao_fec.h>
 #include <ao_packet.h>
+#if HAS_PAD
+#include <ao_pad.h>
+#endif
 
 static uint8_t ao_radio_mutex;
 
@@ -813,6 +816,9 @@ ao_radio_test_cmd(void)
 #if PACKET_HAS_SLAVE
 		ao_packet_slave_stop();
 #endif
+#if HAS_PAD
+		ao_pad_disable();
+#endif
 		ao_radio_get(0xff);
 		ao_radio_set_mode(AO_RADIO_MODE_TEST);
 		ao_radio_strobe(CC1200_STX);
@@ -838,6 +844,9 @@ ao_radio_test_cmd(void)
 #if HAS_MONITOR
 		ao_monitor_enable();
 #endif
+#if HAS_PAD
+		ao_pad_enable();
+#endif
 	}
 }
 
diff --git a/src/drivers/ao_event.c b/src/drivers/ao_event.c
index 5c0d2863..8f88d778 100644
--- a/src/drivers/ao_event.c
+++ b/src/drivers/ao_event.c
@@ -41,6 +41,22 @@ ao_event_get(struct ao_event *ev)
 		);
 }
 
+uint8_t
+ao_event_get_for(struct ao_event *ev, uint16_t timeout)
+{
+	uint8_t	empty = 1;
+	ao_arch_critical(
+		while ((empty = ao_event_queue_empty()))
+			if (ao_sleep_for(&ao_event_queue, timeout))
+				break;
+		if (!empty) {
+			*ev = ao_event_queue[ao_event_queue_remove];
+			ao_event_queue_remove = ao_event_queue_next(ao_event_queue_remove);
+		}
+		);
+	return empty;
+}
+
 /* called with interrupts disabled */
 void
 ao_event_put_isr(uint8_t type, uint8_t unit, int32_t value)
diff --git a/src/drivers/ao_event.h b/src/drivers/ao_event.h
index 584a845a..ea89da23 100644
--- a/src/drivers/ao_event.h
+++ b/src/drivers/ao_event.h
@@ -32,6 +32,9 @@ struct ao_event {
 void
 ao_event_get(struct ao_event *ev);
 
+uint8_t
+ao_event_get_for(struct ao_event *ev, uint16_t timeout);
+
 void
 ao_event_put_isr(uint8_t type, uint8_t unit, int32_t value);
 
diff --git a/src/drivers/ao_lco_two.c b/src/drivers/ao_lco_two.c
new file mode 100644
index 00000000..f53fef7d
--- /dev/null
+++ b/src/drivers/ao_lco_two.c
@@ -0,0 +1,314 @@
+/*
+ * Copyright © 2012 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.
+ */
+
+#include <ao.h>
+#include <ao_lco.h>
+#include <ao_event.h>
+#include <ao_lco_func.h>
+#include <ao_radio_cmac.h>
+
+#define DEBUG	1
+
+#if DEBUG
+static uint8_t	ao_lco_debug;
+#define DEBUG_EVENT	1
+#define DEBUG_STATUS	2
+#define PRINTD(l, ...) do { if (!(ao_lco_debug & l)) break; printf ("\r%5u %s: ", ao_tick_count, __func__); printf(__VA_ARGS__); flush(); } while(0)
+#else
+#define PRINTD(l,...)
+#endif
+
+#define AO_LCO_VALID_LAST	1
+#define AO_LCO_VALID_EVER	2
+
+static uint8_t	ao_lco_suspended;
+static uint8_t	ao_lco_selected;
+static uint8_t	ao_lco_valid;
+static uint8_t	ao_lco_channels;
+static uint16_t	ao_lco_tick_offset;
+
+/* UI values */
+static uint8_t	ao_lco_armed;
+static uint8_t	ao_lco_firing;
+
+#define ao_lco_box	(ao_config.pad_box)
+
+static struct ao_pad_query	ao_pad_query;
+
+#define MASK_SIZE(n)	(((n) + 7) >> 3)
+#define MASK_ID(n)	((n) >> 3)
+#define MASK_SHIFT(n)	((n) & 7)
+
+static void
+ao_lco_set_armed(int pad, int armed)
+{
+	uint8_t	bit = (1 << pad);
+
+	if (armed) {
+		ao_lco_selected |= bit;
+		ao_lco_armed |= bit;
+	} else {
+		ao_lco_selected &= ~bit;
+		ao_lco_armed &= ~bit;
+	}
+	PRINTD(DEBUG_EVENT, "pad %d bit 0x%x armed %d ao_lco_selected 0x%x ao_lco_armed 0x%x\n",
+	       pad, bit, armed, ao_lco_selected, ao_lco_armed);
+	ao_wakeup(&ao_lco_armed);
+}
+
+static void
+ao_lco_suspend(void)
+{
+	if (!ao_lco_suspended) {
+		PRINTD(DEBUG_EVENT, "suspend\n");
+		ao_lco_suspended = 1;
+		ao_lco_selected = 0;
+		ao_lco_armed = 0;
+		ao_wakeup(&ao_pad_query);
+	}
+}
+
+static void
+ao_lco_wakeup(void)
+{
+	if (ao_lco_suspended) {
+		ao_lco_suspended = 0;
+		ao_wakeup(&ao_lco_suspended);
+	}
+}
+
+static void
+ao_lco_input(void)
+{
+	static struct ao_event	event;
+	uint8_t	timeout;
+
+	ao_config_get();
+	for (;;) {
+		if (ao_config.pad_idle && !ao_lco_suspended) {
+			timeout = ao_event_get_for(&event, AO_SEC_TO_TICKS(ao_config.pad_idle));
+			if (timeout) {
+				ao_lco_suspend();
+				continue;
+			}
+		} else {
+			ao_event_get(&event);
+		}
+		ao_lco_wakeup();
+		PRINTD(DEBUG_EVENT, "event type %d unit %d value %d\n",
+		       event.type, event.unit, event.value);
+		switch (event.type) {
+		case AO_EVENT_BUTTON:
+			switch (event.unit) {
+			case AO_BUTTON_ARM_0:
+				ao_lco_set_armed(0, event.value);
+				break;
+#if AO_BUTTON_ARM_NUM > 1
+			case AO_BUTTON_ARM_1:
+				ao_lco_set_armed(1, event.value);
+				break;
+#endif
+			case AO_BUTTON_FIRE:
+				if (ao_lco_armed) {
+					ao_lco_firing = event.value;
+					PRINTD(DEBUG_EVENT, "Firing %d\n", ao_lco_firing);
+					ao_wakeup(&ao_lco_armed);
+				}
+				break;
+			}
+			break;
+		}
+	}
+}
+
+static AO_LED_TYPE	continuity_led[AO_LED_CONTINUITY_NUM] = {
+#ifdef AO_LED_CONTINUITY_0
+	AO_LED_CONTINUITY_0,
+#endif
+#ifdef AO_LED_CONTINUITY_1
+	AO_LED_CONTINUITY_1,
+#endif
+#ifdef AO_LED_CONTINUITY_2
+	AO_LED_CONTINUITY_2,
+#endif
+#ifdef AO_LED_CONTINUITY_3
+	AO_LED_CONTINUITY_3,
+#endif
+#ifdef AO_LED_CONTINUITY_4
+	AO_LED_CONTINUITY_4,
+#endif
+#ifdef AO_LED_CONTINUITY_5
+	AO_LED_CONTINUITY_5,
+#endif
+#ifdef AO_LED_CONTINUITY_6
+	AO_LED_CONTINUITY_6,
+#endif
+#ifdef AO_LED_CONTINUITY_7
+	AO_LED_CONTINUITY_7,
+#endif
+};
+
+static uint8_t
+ao_lco_get_channels(void)
+{
+	int8_t			r;
+
+	r = ao_lco_query(ao_lco_box, &ao_pad_query, &ao_lco_tick_offset);
+	if (r == AO_RADIO_CMAC_OK) {
+		ao_lco_channels = ao_pad_query.channels;
+		ao_lco_valid = AO_LCO_VALID_LAST | AO_LCO_VALID_EVER;
+	} else
+		ao_lco_valid &= ~AO_LCO_VALID_LAST;
+	PRINTD(DEBUG_STATUS, "ao_lco_get_channels() rssi %d valid %d ret %d offset %d\n", ao_radio_cmac_rssi, ao_lco_valid, r, ao_lco_tick_offset);
+	ao_wakeup(&ao_pad_query);
+	return ao_lco_valid;
+}
+
+static void
+ao_lco_igniter_status(void)
+{
+	uint8_t		c;
+	uint8_t		t = 0;
+
+	for (;;) {
+		ao_sleep(&ao_pad_query);
+		while (ao_lco_suspended) {
+			ao_led_off(AO_LED_GREEN|AO_LED_AMBER|AO_LED_RED|AO_LED_REMOTE_ARM);
+			for (c = 0; c < AO_LED_CONTINUITY_NUM; c++)
+				ao_led_off(continuity_led[c]);
+			ao_sleep(&ao_lco_suspended);
+		}
+		PRINTD(DEBUG_STATUS, "RSSI %d VALID %d\n", ao_radio_cmac_rssi, ao_lco_valid);
+		if (!(ao_lco_valid & AO_LCO_VALID_LAST)) {
+			ao_led_on(AO_LED_RED);
+			ao_led_off(AO_LED_GREEN|AO_LED_AMBER);
+			continue;
+		}
+		if (ao_radio_cmac_rssi < -90) {
+			ao_led_on(AO_LED_AMBER);
+			ao_led_off(AO_LED_RED|AO_LED_GREEN);
+		} else {
+			ao_led_on(AO_LED_GREEN);
+			ao_led_off(AO_LED_RED|AO_LED_AMBER);
+		}
+		if (ao_pad_query.arm_status)
+			ao_led_on(AO_LED_REMOTE_ARM);
+		else
+			ao_led_off(AO_LED_REMOTE_ARM);
+
+		for (c = 0; c < AO_LED_CONTINUITY_NUM; c++) {
+			uint8_t	status;
+
+			if (ao_pad_query.channels & (1 << c))
+				status = ao_pad_query.igniter_status[c];
+			else
+				status = AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN;
+			PRINTD(DEBUG_STATUS, "\tchannel %d status %d\n", c, status);
+			if (status == AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN)
+				ao_led_on(continuity_led[c]);
+			else
+				ao_led_off(continuity_led[c]);
+		}
+		t = 1-t;
+	}
+}
+
+static void
+ao_lco_arm_warn(void)
+{
+	int	i;
+	for (;;) {
+		while (ao_lco_suspended)
+			ao_sleep(&ao_lco_suspended);
+		while (!ao_lco_armed)
+			ao_sleep(&ao_lco_armed);
+		for (i = 0; i < ao_lco_armed; i++) {
+			ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(100));
+			ao_delay(AO_MS_TO_TICKS(100));
+		}
+		ao_delay(AO_MS_TO_TICKS(300));
+	}
+}
+
+static struct ao_task ao_lco_input_task;
+static struct ao_task ao_lco_monitor_task;
+static struct ao_task ao_lco_arm_warn_task;
+static struct ao_task ao_lco_igniter_status_task;
+
+static void
+ao_lco_monitor(void)
+{
+	uint16_t		delay;
+
+	ao_add_task(&ao_lco_input_task, ao_lco_input, "lco input");
+	ao_add_task(&ao_lco_arm_warn_task, ao_lco_arm_warn, "lco arm warn");
+	ao_add_task(&ao_lco_igniter_status_task, ao_lco_igniter_status, "lco igniter status");
+	ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200));
+	for (;;) {
+		while (ao_lco_suspended)
+			ao_sleep(&ao_lco_suspended);
+
+		PRINTD(DEBUG_STATUS, "monitor armed %d firing %d\n",
+		       ao_lco_armed, ao_lco_firing);
+
+		if (ao_lco_armed && ao_lco_firing) {
+			ao_lco_ignite();
+		} else {
+			ao_lco_get_channels();
+			if (ao_lco_armed) {
+				if (ao_lco_selected) {
+					PRINTD(DEBUG_STATUS, "Arming pads %x\n",
+					       ao_lco_selected);
+					if (ao_lco_valid & AO_LCO_VALID_EVER) {
+						ao_lco_arm(ao_lco_box, ao_lco_selected, ao_lco_tick_offset);
+						ao_delay(AO_MS_TO_TICKS(10));
+					}
+				}
+			}
+		}
+		if (ao_lco_armed && ao_lco_firing)
+			delay = AO_MS_TO_TICKS(100);
+		else {
+			delay = AO_SEC_TO_TICKS(1);
+		}
+		ao_sleep_for(&ao_lco_armed, delay);
+	}
+}
+
+#if DEBUG
+void
+ao_lco_set_debug(void)
+{
+	ao_cmd_decimal();
+	if (ao_cmd_status == ao_cmd_success)
+		ao_lco_debug = ao_cmd_lex_i;
+}
+
+__code struct ao_cmds ao_lco_cmds[] = {
+	{ ao_lco_set_debug,	"D <0 off, 1 on>\0Debug" },
+	{ 0, NULL }
+};
+#endif
+
+void
+ao_lco_init(void)
+{
+	ao_add_task(&ao_lco_monitor_task, ao_lco_monitor, "lco monitor");
+#if DEBUG
+	ao_cmd_register(&ao_lco_cmds[0]);
+#endif
+}
diff --git a/src/drivers/ao_ms5607.c b/src/drivers/ao_ms5607.c
index 6098699e..ef31882e 100644
--- a/src/drivers/ao_ms5607.c
+++ b/src/drivers/ao_ms5607.c
@@ -24,9 +24,13 @@
 __xdata struct ao_ms5607_prom	ao_ms5607_prom;
 static __xdata uint8_t	  	ms5607_configured;
 
+#ifndef AO_MS5607_SPI_SPEED
+#define AO_MS5607_SPI_SPEED	AO_SPI_SPEED_FAST
+#endif
+
 static void
 ao_ms5607_start(void) {
-	ao_spi_get_bit(AO_MS5607_CS_PORT, AO_MS5607_CS_PIN, AO_MS5607_CS, AO_MS5607_SPI_INDEX, AO_SPI_SPEED_FAST);
+	ao_spi_get_bit(AO_MS5607_CS_PORT, AO_MS5607_CS_PIN, AO_MS5607_CS, AO_MS5607_SPI_INDEX, AO_MS5607_SPI_SPEED);
 }
 
 static void
diff --git a/src/drivers/ao_packet_master.c b/src/drivers/ao_packet_master.c
index 5e440db0..2beda4cb 100644
--- a/src/drivers/ao_packet_master.c
+++ b/src/drivers/ao_packet_master.c
@@ -117,6 +117,7 @@ ao_packet_forward(void) __reentrant
 {
 	char c;
 	ao_packet_enable = 1;
+	ao_packet_tx_used = 0;
 	ao_cmd_white();
 
 	flush();
diff --git a/src/drivers/ao_pad.c b/src/drivers/ao_pad.c
index 419ea8d3..99a90e77 100644
--- a/src/drivers/ao_pad.c
+++ b/src/drivers/ao_pad.c
@@ -50,7 +50,11 @@ ao_siren(uint8_t v)
 #ifdef AO_SIREN
 	ao_gpio_set(AO_SIREN_PORT, AO_SIREN_PIN, AO_SIREN, v);
 #else
+#if HAS_BEEP
 	ao_beep(v ? AO_BEEP_MID : 0);
+#else
+	(void) v;
+#endif
 #endif
 }
 
@@ -59,13 +63,15 @@ ao_strobe(uint8_t v)
 {
 #ifdef AO_STROBE
 	ao_gpio_set(AO_STROBE_PORT, AO_STROBE_PIN, AO_STROBE, v);
+#else
+	(void) v;
 #endif
 }
 
 static void
 ao_pad_run(void)
 {
-	uint8_t	pins;
+	AO_PORT_TYPE	pins;
 
 	for (;;) {
 		while (!ao_pad_ignite)
@@ -90,18 +96,28 @@ ao_pad_run(void)
 		if (ao_pad_ignite & (1 << 3))
 			pins |= (1 << AO_PAD_PIN_3);
 #endif
-		AO_PAD_PORT = (AO_PAD_PORT & (~AO_PAD_ALL_PINS)) | pins;
+		PRINTD("ignite pins 0x%x\n", pins);
+		ao_gpio_set_bits(AO_PAD_PORT, pins);
 		while (ao_pad_ignite) {
 			ao_pad_ignite = 0;
 
 			ao_delay(AO_PAD_FIRE_TIME);
 		}
-		AO_PAD_PORT &= ~(AO_PAD_ALL_PINS);
+		ao_gpio_clr_bits(AO_PAD_PORT, pins);
+		PRINTD("turn off pins 0x%x\n", pins);
 	}
 }
 
 #define AO_PAD_ARM_SIREN_INTERVAL	200
 
+#ifndef AO_PYRO_R_PYRO_SENSE
+#define AO_PYRO_R_PYRO_SENSE	100
+#define AO_PYRO_R_SENSE_GND	27
+#define AO_FIRE_R_POWER_FET	100
+#define AO_FIRE_R_FET_SENSE	100
+#define AO_FIRE_R_SENSE_GND	27
+#endif
+
 static void
 ao_pad_monitor(void)
 {
@@ -109,7 +125,7 @@ ao_pad_monitor(void)
 	uint8_t			sample;
 	__pdata uint8_t		prev = 0, cur = 0;
 	__pdata uint8_t		beeping = 0;
-	__xdata struct ao_data	*packet;
+	__xdata volatile struct ao_data	*packet;
 	__pdata uint16_t	arm_beep_time = 0;
 
 	sample = ao_data_head;
@@ -120,12 +136,18 @@ ao_pad_monitor(void)
 				ao_sleep((void *) DATA_TO_XDATA(&ao_data_head));
 			);
 
+
 		packet = &ao_data_ring[sample];
 		sample = ao_data_ring_next(sample);
 
 		pyro = packet->adc.pyro;
 
-#define VOLTS_TO_PYRO(x) ((int16_t) ((x) * 27.0 / 127.0 / 3.3 * 32767.0))
+#define VOLTS_TO_PYRO(x) ((int16_t) ((x) * ((1.0 * AO_PYRO_R_SENSE_GND) / \
+					    (1.0 * (AO_PYRO_R_SENSE_GND + AO_PYRO_R_PYRO_SENSE)) / 3.3 * AO_ADC_MAX)))
+
+
+#define VOLTS_TO_FIRE(x) ((int16_t) ((x) * ((1.0 * AO_FIRE_R_SENSE_GND) / \
+					    (1.0 * (AO_FIRE_R_SENSE_GND + AO_FIRE_R_FET_SENSE)) / 3.3 * AO_ADC_MAX)))
 
 		/* convert ADC value to voltage in tenths, then add .2 for the diode drop */
 		query.battery = (packet->adc.batt + 96) / 192 + 2;
@@ -133,14 +155,16 @@ ao_pad_monitor(void)
 		if (pyro > VOLTS_TO_PYRO(10)) {
 			query.arm_status = AO_PAD_ARM_STATUS_ARMED;
 			cur |= AO_LED_ARMED;
-		} else if (pyro < VOLTS_TO_PYRO(5)) {
-			query.arm_status = AO_PAD_ARM_STATUS_DISARMED;
-			arm_beep_time = 0;
-		} else {
+#if AO_FIRE_R_POWER_FET
+		} else if (pyro > VOLTS_TO_PYRO(5)) {
 			if ((ao_time() % 100) < 50)
 				cur |= AO_LED_ARMED;
 			query.arm_status = AO_PAD_ARM_STATUS_UNKNOWN;
 			arm_beep_time = 0;
+#endif
+		} else {
+			query.arm_status = AO_PAD_ARM_STATUS_DISARMED;
+			arm_beep_time = 0;
 		}
 		if ((ao_time() - ao_pad_packet_time) > AO_SEC_TO_TICKS(2))
 			cur |= AO_LED_RED;
@@ -165,22 +189,39 @@ ao_pad_monitor(void)
 			 *	27k            /
 			 *		gnd ---
 			 *
+			 *		v_pyro \
+			 *	200k		igniter
+			 *		output /
+			 *	200k           \
+			 *		sense   relay
+			 *	22k            /
+			 *		gnd ---
+			 *
 			 *	If the relay is closed, then sense will be 0
 			 *	If no igniter is present, then sense will be v_pyro * 27k/227k = pyro * 127 / 227 ~= pyro/2
 			 *	If igniter is present, then sense will be v_pyro * 27k/127k ~= v_pyro / 20 = pyro
 			 */
 
+#if AO_FIRE_R_POWER_FET
 			if (sense <= pyro / 8) {
 				status = AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_CLOSED;
 				if ((ao_time() % 100) < 50)
 					cur |= AO_LED_CONTINUITY(c);
-			}
-			else if (pyro / 8 * 3 <= sense && sense <= pyro / 8 * 5)
+			} else
+			if (pyro / 8 * 3 <= sense && sense <= pyro / 8 * 5)
 				status = AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN;
 			else if (pyro / 8 * 7 <= sense) {
 				status = AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN;
 				cur |= AO_LED_CONTINUITY(c);
 			}
+#else
+			if (sense >= pyro / 8 * 5) {
+				status = AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN;
+				cur |= AO_LED_CONTINUITY(c);
+			} else {
+				status = AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN;
+			}
+#endif
 			query.igniter_status[c] = status;
 		}
 		if (cur != prev) {
@@ -240,8 +281,14 @@ ao_pad_read_box(void)
 	l = byte & 0xf;
 	return h * 10 + l;
 }
-#else
-#define ao_pad_read_box()	0
+#endif
+
+#if HAS_FIXED_PAD_BOX
+#define ao_pad_read_box()	ao_config.pad_box
+#endif
+
+#ifdef PAD_BOX
+#define ao_pad_read_box()	PAD_BOX
 #endif
 
 static void
diff --git a/src/drivers/ao_servo.c b/src/drivers/ao_servo.c
new file mode 100644
index 00000000..b48a4112
--- /dev/null
+++ b/src/drivers/ao_servo.c
@@ -0,0 +1,118 @@
+/*
+ * Copyright © 2016 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.
+ */
+
+#include "ao.h"
+#include "ao_servo.h"
+#include "ao_pwm.h"
+#include "ao_exti.h"
+
+static uint8_t	limit_wakeup;
+
+static void
+ao_limit_isr(void)
+{
+	ao_wakeup(&limit_wakeup);
+}
+
+static void
+ao_limit_enable(uint8_t dir)
+{
+	if (dir == AO_SERVO_FORE)
+		ao_exti_enable(AO_SERVO_LIMIT_FORE_PORT, AO_SERVO_LIMIT_FORE_BIT);
+	else
+		ao_exti_enable(AO_SERVO_LIMIT_BACK_PORT, AO_SERVO_LIMIT_BACK_BIT);
+}
+
+static void
+ao_limit_disable(uint8_t dir)
+{
+	if (dir == AO_SERVO_FORE)
+		ao_exti_disable(AO_SERVO_LIMIT_FORE_PORT, AO_SERVO_LIMIT_FORE_BIT);
+	else
+		ao_exti_disable(AO_SERVO_LIMIT_BACK_PORT, AO_SERVO_LIMIT_BACK_BIT);
+}
+
+static uint8_t
+ao_limit_get(uint8_t dir)
+{
+	if (dir == AO_SERVO_FORE)
+		return !ao_gpio_get(AO_SERVO_LIMIT_FORE_PORT, AO_SERVO_LIMIT_FORE_BIT, PIN);
+	else
+		return !ao_gpio_get(AO_SERVO_LIMIT_BACK_PORT, AO_SERVO_LIMIT_BACK_BIT, PIN);
+}
+
+void
+ao_servo_run(uint16_t speed, uint8_t dir, uint16_t timeout)
+{
+	printf ("speed %d dir %d\n", speed, dir);
+
+	/* Turn on the motor */
+	ao_gpio_set(AO_SERVO_DIR_PORT, AO_SERVO_DIR_BIT, AO_SERVO_DIR_PIN, dir);
+	ao_pwm_set(AO_SERVO_SPEED_PWM, speed);
+
+	/* Wait until the limit sensor is triggered */
+	ao_arch_block_interrupts();
+	ao_limit_enable(dir);
+	while (!ao_limit_get(dir))
+		if (ao_sleep_for(&limit_wakeup, timeout))
+			break;
+	ao_limit_disable(dir);
+	ao_arch_release_interrupts();
+
+	/* Turn off the motor */
+	ao_pwm_set(AO_SERVO_SPEED_PWM, 0);
+}
+
+#define init_limit(p,b) do {						\
+		ao_enable_port(p);					\
+		ao_exti_setup(p, b,					\
+			      AO_EXTI_MODE_PULL_UP|AO_EXTI_MODE_FALLING|AO_EXTI_PRIORITY_MED, \
+			      ao_limit_isr);				\
+	} while (0)
+
+
+static void
+ao_servo_cmd(void)
+{
+	uint8_t	dir;
+	uint16_t speed;
+
+	ao_cmd_decimal();
+	dir = ao_cmd_lex_u32;
+	ao_cmd_decimal();
+	speed = ao_cmd_lex_u32;
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+
+	printf("Run servo %d\n", dir);
+	ao_servo_run(speed, dir, AO_MS_TO_TICKS(200));
+}
+
+static const struct ao_cmds ao_servo_cmds[] = {
+	{ ao_servo_cmd,	"S <dir> <speed>\0Run servo in indicated direction" },
+	{ 0, NULL },
+};
+
+
+void
+ao_servo_init(void)
+{
+	init_limit(AO_SERVO_LIMIT_FORE_PORT, AO_SERVO_LIMIT_FORE_BIT);
+	init_limit(AO_SERVO_LIMIT_BACK_PORT, AO_SERVO_LIMIT_BACK_BIT);
+	ao_enable_output(AO_SERVO_DIR_PORT, AO_SERVO_DIR_BIT, AO_SERVO_DIR_PIN, 0);
+	ao_cmd_register(&ao_servo_cmds[0]);
+}
diff --git a/src/drivers/ao_servo.h b/src/drivers/ao_servo.h
new file mode 100644
index 00000000..e1df347a
--- /dev/null
+++ b/src/drivers/ao_servo.h
@@ -0,0 +1,41 @@
+/*
+ * Copyright © 2016 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.
+ */
+
+#ifndef _AO_SERVO_H_
+#define _AO_SERVO_H_
+
+void
+ao_servo_run(uint16_t speed, uint8_t dir, uint16_t timeout);
+
+void
+ao_servo_init(void);
+
+#define AO_SERVO_FORE	0
+#define AO_SERVO_BACK	1
+
+/* To configure the servo:
+ *
+ *	#define AO_SERVO_PWM			<pwm index>
+ *	#define AO_SERVO_DIR_PORT		<gpio>
+ *	#define AO_SERVO_DIR_PIN		<pin>
+ *	#define AO_SERVO_LIMIT_FORE_PORT	<gpio>
+ *	#define AO_SERVO_LIMIT_FORE_PIN		<pin>
+ *	#define AO_SERVO_LIMIT_BACK_PORT	<gpio>
+ *	#define AO_SERVO_LIMIT_BACK_PIN		<pin>
+ */
+
+#endif /* _AO_SERVO_H_ */
diff --git a/src/drivers/ao_trng_send.c b/src/drivers/ao_trng_send.c
index bac6035c..171a345f 100644
--- a/src/drivers/ao_trng_send.c
+++ b/src/drivers/ao_trng_send.c
@@ -19,47 +19,247 @@
 #include <ao_adc_fast.h>
 #include <ao_crc.h>
 #include <ao_trng_send.h>
+#include <ao_exti.h>
+#include <ao_power.h>
+
+static struct ao_task	ao_trng_send_task, ao_trng_send_raw_task;
+static uint8_t		trng_running;
+static AO_TICK_TYPE	trng_power_time;
+
+#define TRNG_ENABLE_DELAY	AO_MS_TO_TICKS(100)
+
+static uint8_t		random_mutex;
 
 static void
-ao_trng_send(void)
+ao_trng_get_raw(uint16_t *buf)
+{
+	uint16_t	i;
+	uint16_t	t;
+	uint16_t	v;
+
+	t = ao_adc_get(AO_USB_IN_SIZE>>1);	/* one 16-bit value per two output bytes */
+	for (i = 0; i < AO_USB_IN_SIZE / sizeof (uint16_t); i++) {
+		v = ao_adc_ring[t];
+		*buf++ = v;
+		t = (t + 1) & (AO_ADC_RING_SIZE - 1);
+	}
+	ao_adc_ack(AO_USB_IN_SIZE>>1);
+}
+
+static void
+ao_trng_send_raw(void)
 {
 	static uint16_t	*buffer[2];
 	int		usb_buf_id;
+
+	if (!buffer[0]) {
+		buffer[0] = ao_usb_alloc();
+		buffer[1] = ao_usb_alloc();
+		if (!buffer[0])
+			ao_exit();
+	}
+
+	usb_buf_id = 0;
+
+	for (;;) {
+		ao_mutex_get(&random_mutex);
+		if (!trng_running) {
+			AO_TICK_TYPE	delay;
+
+			delay = trng_power_time + TRNG_ENABLE_DELAY - ao_time();
+			if (delay > TRNG_ENABLE_DELAY)
+				delay = TRNG_ENABLE_DELAY;
+
+			/* Delay long enough for the HV power supply
+			 * to stabilize so that the first bits we read
+			 * aren't of poor quality
+			 */
+			ao_delay(delay);
+			trng_running = TRUE;
+		}
+#ifdef AO_LED_TRNG_RAW
+		ao_led_on(AO_LED_TRNG_RAW);
+#endif
+		ao_trng_get_raw(buffer[usb_buf_id]);
+#ifdef AO_LED_TRNG_RAW
+		ao_led_off(AO_LED_TRNG_RAW);
+#endif
+		ao_mutex_put(&random_mutex);
+		ao_usb_write2(buffer[usb_buf_id], AO_USB_IN_SIZE);
+		usb_buf_id = 1-usb_buf_id;
+	}
+}
+
+/* Make sure there's at least 8 bits of variance in the samples */
+#define MIN_VARIANCE		(128 * 128)
+
+/* Make sure the signal is spread around a bit */
+#define MAX_VARIANCE		(512 * 512)
+
+#define ADD_STATS(value) do {			\
+		sum += (value);			\
+		sum2 += (value) * (value);	\
+	} while(0)
+
+#define VARIANCE(n)	((sum2 - (sum / (n) * sum)) / (n))
+
+static int
+ao_trng_get_cooked(uint16_t *buf)
+{
 	uint16_t	i;
-	uint16_t	*buf;
 	uint16_t	t;
 	uint32_t	*rnd = (uint32_t *) ao_adc_ring;
+	int32_t 	sum, sum2, var;
+
+	sum = sum2 = 0;
+	t = ao_adc_get(AO_USB_IN_SIZE) >> 1;		/* one 16-bit value per output byte */
+	for (i = 0; i < AO_USB_IN_SIZE / sizeof (uint16_t); i++) {
+		uint32_t	v;
+		uint16_t	v1, v2;
+
+		/* Fetch two values in one operation */
+		v = rnd[t];
+		t = (t + 1) & ((AO_ADC_RING_SIZE >> 1) - 1);
+
+		*buf++ = ao_crc_in_32_out_16(v);
+
+		v1 = v;
+		v2 = v >> 16;
+
+		ADD_STATS(v1);
+		ADD_STATS(v2);
+	}
+	ao_adc_ack(AO_USB_IN_SIZE);
+	var = VARIANCE(2 * AO_USB_IN_SIZE / sizeof (uint16_t));
+	return var >= MIN_VARIANCE && var <= MAX_VARIANCE;
+}
+
+#define AO_TRNG_START_WAIT	1024
+#define AO_TRNG_START_CHECK	32
+
+static void
+ao_trng_send(void)
+{
+	static uint16_t	*buffer[2];
+	int	usb_buf_id;
+	int	good_bits;
+	int	failed;
+	int	s;
 
 	if (!buffer[0]) {
 		buffer[0] = ao_usb_alloc();
 		buffer[1] = ao_usb_alloc();
 		if (!buffer[0])
-			return;
+			ao_exit();
 	}
 
 	usb_buf_id = 0;
 
+#ifdef AO_TRNG_ENABLE_PORT
+	ao_gpio_set(AO_TRNG_ENABLE_PORT, AO_TRNG_ENABLE_BIT, AO_TRNG_ENABLE_PIN, 1);
+#endif
+	trng_power_time = ao_time();
+
 	ao_crc_reset();
 
+	ao_delay(TRNG_ENABLE_DELAY);
+
+	for (s = 0; s < AO_TRNG_START_WAIT; s++) {
+		if (ao_trng_get_cooked(buffer[0]))
+			break;
+		ao_delay(AO_MS_TO_TICKS(10));
+	}
+
+	/* Validate the hardware before enabling USB */
+	failed = 0;
+	for (s = 0; s < AO_TRNG_START_CHECK; s++) {
+		if (!ao_trng_get_cooked(buffer[0])) {
+			failed++;
+			ao_delay(AO_MS_TO_TICKS(10));
+		}
+	}
+	if (failed > AO_TRNG_START_CHECK / 4)
+		ao_panic(AO_PANIC_DMA);
+
+	ao_add_task(&ao_trng_send_raw_task, ao_trng_send_raw, "trng_send_raw");
+
+#ifdef AO_USB_START_DISABLED
+	ao_usb_enable();
+#endif
+
 	for (;;) {
-		ao_led_on(AO_LED_TRNG_ACTIVE);
-		t = ao_adc_get(AO_USB_IN_SIZE) >> 1;	/* one 16-bit value per output byte */
-		buf = buffer[usb_buf_id];
-		for (i = 0; i < AO_USB_IN_SIZE / sizeof (uint16_t); i++) {
-			*buf++ = ao_crc_in_32_out_16(rnd[t]);
-			t = (t + 1) & ((AO_ADC_RING_SIZE>>1) - 1);
+		ao_mutex_get(&random_mutex);
+		if (!trng_running) {
+			AO_TICK_TYPE	delay;
+
+			delay = trng_power_time + TRNG_ENABLE_DELAY - ao_time();
+			if (delay > TRNG_ENABLE_DELAY)
+				delay = TRNG_ENABLE_DELAY;
+
+			/* Delay long enough for the HV power supply
+			 * to stabilize so that the first bits we read
+			 * aren't of poor quality
+			 */
+			ao_delay(delay);
+			trng_running = TRUE;
+		}
+#ifdef AO_LED_TRNG_COOKED
+		ao_led_on(AO_LED_TRNG_COOKED);
+#endif
+		good_bits = ao_trng_get_cooked(buffer[usb_buf_id]);
+#ifdef AO_LED_TRNG_COOKED
+		ao_led_off(AO_LED_TRNG_COOKED);
+#endif
+		ao_mutex_put(&random_mutex);
+		if (good_bits) {
+			ao_usb_write(buffer[usb_buf_id], AO_USB_IN_SIZE);
+			usb_buf_id = 1-usb_buf_id;
+			failed = 0;
+		} else {
+			failed++;
+			ao_delay(AO_MS_TO_TICKS(10));
+			if (failed > 10) {
+				ao_usb_disable();
+				ao_panic(AO_PANIC_DMA);
+			}
 		}
-		ao_adc_ack(AO_USB_IN_SIZE);
-		ao_led_off(AO_LED_TRNG_ACTIVE);
-		ao_usb_write(buffer[usb_buf_id], AO_USB_IN_SIZE);
-		usb_buf_id = 1-usb_buf_id;
 	}
 }
 
-static struct ao_task ao_trng_send_task;
+#if AO_POWER_MANAGEMENT
+
+static void ao_trng_suspend(void *arg)
+{
+	(void) arg;
+#ifdef AO_TRNG_ENABLE_PORT
+	ao_gpio_set(AO_TRNG_ENABLE_PORT, AO_TRNG_ENABLE_BIT, AO_TRNG_ENABLE_PIN, 0);
+#endif
+	trng_running = FALSE;
+}
+
+static void ao_trng_resume(void *arg)
+{
+	(void) arg;
+#ifdef AO_TRNG_ENABLE_PORT
+	ao_gpio_set(AO_TRNG_ENABLE_PORT, AO_TRNG_ENABLE_BIT, AO_TRNG_ENABLE_PIN, 1);
+#endif
+	trng_power_time = ao_time();
+}
+
+static struct ao_power ao_trng_power = {
+	.suspend = ao_trng_suspend,
+	.resume = ao_trng_resume
+};
+
+#endif
 
 void
 ao_trng_send_init(void)
 {
+#ifdef AO_TRNG_ENABLE_PORT
+	ao_enable_output(AO_TRNG_ENABLE_PORT, AO_TRNG_ENABLE_BIT, AO_TRNG_ENABLE_PIN, 0);
+	ao_power_register(&ao_trng_power);
+#endif
+	ao_enable_input(AO_RAW_PORT, AO_RAW_BIT, AO_EXTI_MODE_PULL_UP);
 	ao_add_task(&ao_trng_send_task, ao_trng_send, "trng_send");
 }