altos: Fake flight code changes in kernel and stm

Redirects data input from local sensors to USB sourced data, leaving
USB enabled when the computer goes into pad mode.

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

5 files changed, 294 insertions, 4 deletions

diff --git a/src/kernel/ao_fake_flight.c b/src/kernel/ao_fake_flight.c
new file mode 100644
index 00000000..11329bb9
--- /dev/null
+++ b/src/kernel/ao_fake_flight.c
@@ -0,0 +1,219 @@
+/*
+ * Copyright © 2014 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_fake_flight.h>
+#if HAS_MS5607 || HAS_MS5611
+#include <ao_ms5607.h>
+#endif
+
+uint8_t			ao_fake_flight_active;
+
+static uint8_t		ao_fake_has_cur;
+static volatile uint8_t	ao_fake_has_next;
+static uint8_t		ao_fake_has_offset;
+static uint16_t		ao_fake_tick_offset;
+static struct ao_data	ao_fake_cur, ao_fake_next;
+
+void
+ao_fake_flight_poll(void)
+{
+	if (ao_fake_has_next && (ao_tick_count - ao_fake_next.tick) >= 0) {
+		ao_fake_cur = ao_fake_next;
+		ao_fake_has_next = 0;
+		ao_wakeup((void *) &ao_fake_has_next);
+		ao_fake_has_cur = 1;
+	}
+	if (!ao_fake_has_cur)
+		return;
+	ao_data_ring[ao_data_head] = ao_fake_cur;
+	ao_data_ring[ao_data_head].tick = ao_tick_count;
+	ao_data_head = ao_data_ring_next(ao_data_head);
+	ao_wakeup((void *) &ao_data_head);
+}
+
+static uint8_t
+ao_fake_data_read(void)
+{
+	uint8_t	i;
+	uint8_t	*d = (void *) &ao_fake_next;
+
+	if (getchar() == 0)
+		return FALSE;
+	for (i = 0; i < sizeof (struct ao_data); i++)
+		*d++ = getchar();
+	if (!ao_fake_has_offset) {
+		ao_fake_tick_offset = (ao_tick_count + 1000) - ao_fake_next.tick;
+		ao_fake_next.tick = ao_tick_count;
+		ao_fake_has_offset = 1;
+	} else
+		ao_fake_next.tick += ao_fake_tick_offset;
+	ao_fake_has_next = 1;
+	return TRUE;
+}
+
+static void
+ao_fake_calib_get(struct ao_fake_calib *calib)
+{
+#if HAS_ACCEL
+	calib->accel_plus_g = ao_config.accel_plus_g;
+	calib->accel_minus_g = ao_config.accel_minus_g;
+#endif
+#if HAS_GYRO
+	calib->accel_zero_along = ao_config.accel_zero_along;
+	calib->accel_zero_across = ao_config.accel_zero_across;
+	calib->accel_zero_through = ao_config.accel_zero_through;
+#endif
+#if HAS_MS5607 || HAS_MS5611
+	calib->ms5607_prom = ao_ms5607_prom;
+#endif
+}
+
+static void
+ao_fake_calib_set(struct ao_fake_calib *calib)
+{
+#if HAS_ACCEL
+	ao_config.accel_plus_g = calib->accel_plus_g;
+	ao_config.accel_minus_g = calib->accel_minus_g;
+#endif
+#if HAS_GYRO
+	ao_config.accel_zero_along = calib->accel_zero_along;
+	ao_config.accel_zero_across = calib->accel_zero_across;
+	ao_config.accel_zero_through = calib->accel_zero_through;
+#endif
+#if HAS_MS5607 || HAS_MS5611
+	ao_ms5607_prom = calib->ms5607_prom;
+#endif
+}
+
+static uint8_t
+ao_fake_calib_read(void)
+{
+	struct ao_fake_calib	ao_calib;
+	uint8_t			*d = (void *) &ao_calib;
+	uint16_t		i;
+
+	/* Read calibration data */
+	for (i = 0; i < sizeof (struct ao_fake_calib); i++)
+		*d++ = getchar();
+	if (ao_calib.major != AO_FAKE_CALIB_MAJOR
+#if AO_FAKE_CALIB_MINOR != 0
+	    || ao_calib.minor < AO_FAKE_CALIB_MINOR
+#endif
+		) {
+		printf ("Calibration data major version mismatch %d.%d <= %d.%d\n",
+			ao_calib.major, ao_calib.minor, AO_FAKE_CALIB_MAJOR, AO_FAKE_CALIB_MINOR);
+		return FALSE;
+	}
+	ao_fake_calib_set(&ao_calib);
+	return TRUE;
+}
+
+static void
+ao_fake_flight(void)
+{
+	int16_t			calib_size, data_size;
+	struct ao_fake_calib	save_calib;
+	uint16_t		my_pyro_fired = 0;
+	enum ao_flight_state	my_state = ao_flight_invalid;
+	int			i;
+
+	ao_cmd_hex();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	calib_size = ao_cmd_lex_i;
+	ao_cmd_hex();
+	if (ao_cmd_status != ao_cmd_success)
+		return;
+	data_size = ao_cmd_lex_i;
+	if ((unsigned) calib_size != sizeof (struct ao_fake_calib)) {
+		printf ("calib size %d larger than actual size %d\n",
+			calib_size, sizeof (struct ao_fake_calib));
+		ao_cmd_status = ao_cmd_syntax_error;
+		return;
+	}
+	if (data_size != sizeof (struct ao_data)) {
+		printf ("data size %d doesn't match actual size %d\n",
+			data_size, sizeof (struct ao_data));
+		ao_cmd_status = ao_cmd_syntax_error;
+		return;
+	}
+	ao_fake_calib_get(&save_calib);
+	if (!ao_fake_calib_read())
+		return;
+
+	ao_fake_has_next = 0;
+	ao_fake_has_cur = 0;
+	ao_fake_flight_active = 1;
+	ao_sample_init();
+#if PACKET_HAS_SLAVE
+	ao_packet_slave_stop();
+#endif
+#if AO_LED_RED
+	/* Turn on the LED to indicate startup */
+	ao_led_on(AO_LED_RED);
+#endif
+	ao_flight_state = ao_flight_startup;
+	for (;;) {
+		if (my_state != ao_flight_state) {
+			printf("state %d\n", ao_flight_state);
+			my_state = ao_flight_state;
+			flush();
+		}
+		if (my_pyro_fired != ao_pyro_fired) {
+			int	pyro;
+
+			for (pyro = 0; pyro < AO_PYRO_NUM; pyro++) {
+				uint16_t	bit = (1 << pyro);
+				if (!(my_pyro_fired & bit) && (ao_pyro_fired & bit))
+					printf ("fire %d\n", pyro);
+			}
+			my_pyro_fired = ao_pyro_fired;
+		}
+		while (ao_fake_has_next)
+			ao_sleep((void *) &ao_fake_has_next);
+		if (!ao_fake_data_read())
+			break;
+	}
+
+	/* Wait 20 seconds to see if we enter landed state */
+	for (i = 0; i < 200; i++)
+	{
+		if (ao_flight_state == ao_flight_landed)
+			break;
+		ao_delay(AO_MS_TO_TICKS(100));
+	}
+#if AO_LED_RED
+	/* Turn on the LED to indicate startup */
+	ao_led_on(AO_LED_RED);
+#endif
+	ao_fake_flight_active = 0;
+	ao_flight_state = ao_flight_startup;
+	ao_sample_init();
+	ao_fake_calib_set(&save_calib);
+}
+
+static const struct ao_cmds ao_fake_flight_cmds[] = {
+	{ ao_fake_flight,	"F <calib-size> <data-size>\0Start fake flight" },
+	{ 0, NULL }
+};
+
+void
+ao_fake_flight_init(void)
+{
+	ao_cmd_register(&ao_fake_flight_cmds[0]);
+}
diff --git a/src/kernel/ao_fake_flight.h b/src/kernel/ao_fake_flight.h
new file mode 100644
index 00000000..172fc589
--- /dev/null
+++ b/src/kernel/ao_fake_flight.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright © 2014 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_FAKE_FLIGHT_H_
+#define _AO_FAKE_FLIGHT_H_
+#if HAS_MS5607 || HAS_MS5611
+#include <ao_ms5607.h>
+#endif
+
+extern uint8_t	ao_fake_flight_active;
+
+#define AO_FAKE_CALIB_MAJOR	1
+#define AO_FAKE_CALIB_MINOR	0
+
+struct ao_fake_calib {
+	uint16_t		major;
+	uint16_t		minor;
+#if HAS_ACCEL
+	int16_t			accel_plus_g;
+	int16_t			accel_minus_g;
+#endif
+#if HAS_GYRO
+	int16_t			accel_zero_along;
+	int16_t			accel_zero_across;
+	int16_t			accel_zero_through;
+	uint16_t		pad30;
+#endif
+#if HAS_MS5607 || HAS_MS5611
+	struct ao_ms5607_prom	ms5607_prom;
+#endif
+};
+
+void
+ao_fake_flight_poll(void);
+
+void
+ao_fake_flight_init(void);
+
+#endif /* _AO_FAKE_FLIGHT_H_ */
diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c
index 24099347..2b433ee9 100644
--- a/src/kernel/ao_flight.c
+++ b/src/kernel/ao_flight.c
@@ -36,6 +36,10 @@
 #error Please define HAS_USB
 #endif
 
+#if HAS_FAKE_FLIGHT
+#include <ao_fake_flight.h>
+#endif
+
 #ifndef HAS_TELEMETRY
 #define HAS_TELEMETRY	HAS_RADIO
 #endif
@@ -130,7 +134,10 @@ ao_flight(void)
 				/* Disable the USB controller in flight mode
 				 * to save power
 				 */
-				ao_usb_disable();
+#if HAS_FAKE_FLIGHT
+				if (!ao_fake_flight_active)
+#endif
+					ao_usb_disable();
 #endif
 
 #if !HAS_ACCEL && PACKET_HAS_SLAVE
@@ -170,7 +177,6 @@ ao_flight(void)
 
 			break;
 		case ao_flight_pad:
-
 			/* pad to boost:
 			 *
 			 * barometer: > 20m vertical motion
diff --git a/src/kernel/ao_pyro.h b/src/kernel/ao_pyro.h
index 0c5642d6..34c99078 100644
--- a/src/kernel/ao_pyro.h
+++ b/src/kernel/ao_pyro.h
@@ -45,7 +45,11 @@ enum ao_pyro_flag {
 
 	ao_pyro_state_less		= 0x00004000,
 	ao_pyro_state_greater_or_equal  = 0x00008000,
-};
+}
+#ifdef __GNUC__
+	__attribute__ ((packed))
+#endif
+	;
 
 struct ao_pyro {
 	enum ao_pyro_flag	flags;
diff --git a/src/stm/ao_timer.c b/src/stm/ao_timer.c
index d93531fc..8db62e76 100644
--- a/src/stm/ao_timer.c
+++ b/src/stm/ao_timer.c
@@ -17,6 +17,9 @@
 
 #include "ao.h"
 #include <ao_task.h>
+#if HAS_FAKE_FLIGHT
+#include <ao_fake_flight.h>
+#endif
 
 #ifndef HAS_TICK
 #define HAS_TICK 1
@@ -47,7 +50,12 @@ void stm_systick_isr(void)
 #if AO_DATA_ALL
 		if (++ao_data_count == ao_data_interval) {
 			ao_data_count = 0;
-			ao_adc_poll();
+#if HAS_FAKE_FLIGHT
+			if (ao_fake_flight_active)
+				ao_fake_flight_poll();
+			else
+#endif
+				ao_adc_poll();
 #if (AO_DATA_ALL & ~(AO_DATA_ADC))
 			ao_wakeup((void *) &ao_data_count);
 #endif