From 500353ec83af0da7fce3d67f2707f4725b1f50ba Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Wed, 2 Jul 2014 22:42:43 -0700
Subject: altos: ao_rssi subsystem needs to use AO_LED_TYPE instead of uint8_t

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/kernel/ao.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src/kernel/ao.h')

diff --git a/src/kernel/ao.h b/src/kernel/ao.h
index 5ff9b518..b8189e43 100644
--- a/src/kernel/ao.h
+++ b/src/kernel/ao.h
@@ -761,7 +761,7 @@ void
 ao_rssi_set(int rssi_value);
 
 void
-ao_rssi_init(uint8_t rssi_led);
+ao_rssi_init(AO_LED_TYPE rssi_led);
 
 /*
  * ao_product.c
-- 
cgit v1.2.3


From 657d455a2fad36193e6b3a7037d9ba7f09ae1168 Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Wed, 2 Jul 2014 22:46:07 -0700
Subject: altos: Use TeleMetrum v1.9 board for MegaDongle experiments

This configures the MegaDongle v0.1 directory to use the pin
assignments in TeleMetrum v1.9 boards.

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/kernel/ao.h                     |  4 ++-
 src/kernel/ao_monitor.c             |  4 ++-
 src/kernel/ao_rssi.c                |  2 +-
 src/megadongle-v0.1/Makefile        |  1 +
 src/megadongle-v0.1/ao_megadongle.c |  4 +--
 src/megadongle-v0.1/ao_pins.h       | 49 +++++++++++++++----------------------
 6 files changed, 30 insertions(+), 34 deletions(-)

(limited to 'src/kernel/ao.h')

diff --git a/src/kernel/ao.h b/src/kernel/ao.h
index b8189e43..1df2ea8b 100644
--- a/src/kernel/ao.h
+++ b/src/kernel/ao.h
@@ -757,11 +757,13 @@ extern __xdata uint8_t ao_force_freq;
  * ao_rssi.c
  */
 
+#ifdef AO_LED_TYPE
 void
-ao_rssi_set(int rssi_value);
+ao_rssi_set(int16_t rssi_value);
 
 void
 ao_rssi_init(AO_LED_TYPE rssi_led);
+#endif
 
 /*
  * ao_product.c
diff --git a/src/kernel/ao_monitor.c b/src/kernel/ao_monitor.c
index 18f170b4..2d75c41c 100644
--- a/src/kernel/ao_monitor.c
+++ b/src/kernel/ao_monitor.c
@@ -115,6 +115,8 @@ ao_monitor_put(void)
 {
 #if LEGACY_MONITOR
 	__xdata char callsign[AO_MAX_CALLSIGN+1];
+#endif
+#if LEGACY_MONITOR || HAS_RSSI
 	int16_t rssi;
 #endif
 	uint8_t ao_monitor_tail;
@@ -240,7 +242,7 @@ ao_monitor_put(void)
 			hex(sum);
 			putchar ('\n');
 #if HAS_RSSI
-			if (recv_raw.packet[ao_monitoring + 1] & PKT_APPEND_STATUS_1_CRC_OK) {
+			if (recv_raw.packet[ao_monitoring + 1] & AO_RADIO_STATUS_CRC_OK) {
 				rssi = AO_RSSI_FROM_RADIO(recv_raw.packet[ao_monitoring]);
 				ao_rssi_set(rssi);
 			}
diff --git a/src/kernel/ao_rssi.c b/src/kernel/ao_rssi.c
index f8eeb217..793b190b 100644
--- a/src/kernel/ao_rssi.c
+++ b/src/kernel/ao_rssi.c
@@ -33,7 +33,7 @@ ao_rssi(void)
 }
 
 void
-ao_rssi_set(int rssi_value)
+ao_rssi_set(int16_t rssi_value)
 {
 	if (rssi_value > 0)
 		rssi_value = 0;
diff --git a/src/megadongle-v0.1/Makefile b/src/megadongle-v0.1/Makefile
index ade8e496..bbe2ea58 100644
--- a/src/megadongle-v0.1/Makefile
+++ b/src/megadongle-v0.1/Makefile
@@ -53,6 +53,7 @@ ALTOS_SRC = \
 	ao_rssi.c \
 	ao_send_packet.c \
 	ao_packet_master.c \
+	ao_eeprom_stm.c \
 	ao_packet.c
 
 PRODUCT=MegaDongle-v0.1
diff --git a/src/megadongle-v0.1/ao_megadongle.c b/src/megadongle-v0.1/ao_megadongle.c
index 2792af22..68d6de60 100644
--- a/src/megadongle-v0.1/ao_megadongle.c
+++ b/src/megadongle-v0.1/ao_megadongle.c
@@ -24,7 +24,7 @@ int
 main(void)
 {
 	ao_clock_init();
-	
+
 #if HAS_STACK_GUARD
 	ao_mpu_init();
 #endif
@@ -48,7 +48,7 @@ main(void)
 	ao_send_packet_init();
 
 	ao_config_init();
-	
+
 	ao_start_scheduler();
 	return 0;
 }
diff --git a/src/megadongle-v0.1/ao_pins.h b/src/megadongle-v0.1/ao_pins.h
index d460a490..9edd91f4 100644
--- a/src/megadongle-v0.1/ao_pins.h
+++ b/src/megadongle-v0.1/ao_pins.h
@@ -15,6 +15,8 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  */
 
+/* Using TeleMetrum v1.9 board */
+
 #ifndef _AO_PINS_H_
 #define _AO_PINS_H_
 
@@ -46,7 +48,7 @@
 #define HAS_SERIAL_1		0
 #define USE_SERIAL_1_STDIN	0
 #define SERIAL_1_PB6_PB7	0
-#define SERIAL_1_PA9_PA10	1
+#define SERIAL_1_PA9_PA10	0
 
 #define HAS_SERIAL_2		0
 #define USE_SERIAL_2_STDIN	0
@@ -56,19 +58,18 @@
 #define HAS_SERIAL_3		0
 #define USE_SERIAL_3_STDIN	0
 #define SERIAL_3_PB10_PB11	0
-#define SERIAL_3_PC10_PC11	1
+#define SERIAL_3_PC10_PC11	0
 #define SERIAL_3_PD8_PD9	0
 
-#define ao_gps_getchar		ao_serial3_getchar
-#define ao_gps_putchar		ao_serial3_putchar
-#define ao_gps_set_speed	ao_serial3_set_speed
-
-#define HAS_EEPROM		0
-#define USE_INTERNAL_FLASH	0
+#define HAS_EEPROM		1
+#define USE_INTERNAL_FLASH	1
+#define USE_STORAGE_CONFIG	0
+#define USE_EEPROM_CONFIG	1
 #define HAS_USB			1
 #define HAS_BEEP		0
 #define HAS_RADIO		1
 #define HAS_TELEMETRY		0
+#define HAS_RSSI		1
 
 #define HAS_SPI_1		0
 #define SPI_1_PA5_PA6_PA7	0	/* Barometer */
@@ -76,7 +77,7 @@
 #define SPI_1_PE13_PE14_PE15	0	/* Accelerometer */
 
 #define HAS_SPI_2		1
-#define SPI_2_PB13_PB14_PB15	1	/* Flash, Companion */
+#define SPI_2_PB13_PB14_PB15	1	/* Radio */
 #define SPI_2_PD1_PD3_PD4	0
 #define SPI_2_OSPEEDR		STM_OSPEEDR_10MHz
 
@@ -85,27 +86,17 @@
 #define SPI_2_MISO_PIN		14
 #define SPI_2_MOSI_PIN		15
 
-#define HAS_I2C_1		0
-#define I2C_1_PB8_PB9		1
-
-#define HAS_I2C_2		0
-#define I2C_2_PB10_PB11		1
-
 #define PACKET_HAS_SLAVE	0
 #define PACKET_HAS_MASTER	1
 
 #define LOW_LEVEL_DEBUG		0
 
-#define LED_PORT_0_ENABLE	STM_RCC_AHBENR_GPIOAEN
-#define LED_PORT_1_ENABLE	STM_RCC_AHBENR_GPIOBEN
-#define LED_PORT_0		(&stm_gpioa)
-#define LED_PORT_0_MASK		(0xff)
+#define LED_PORT_0_ENABLE	STM_RCC_AHBENR_GPIOCEN
+#define LED_PORT_0		(&stm_gpioc)
+#define LED_PORT_0_MASK		(0xffff)
 #define LED_PORT_0_SHIFT	0
-#define LED_PORT_1_MASK		(0xff00)
-#define LED_PORT_1_SHIFT	0
-#define LED_PORT_1		(&stm_gpiob)
-#define LED_PIN_RED		1
-#define LED_PIN_GREEN		12
+#define LED_PIN_RED		14
+#define LED_PIN_GREEN		15
 #define AO_LED_RED		(1 << LED_PIN_RED)
 #define AO_LED_GREEN		(1 << LED_PIN_GREEN)
 
@@ -134,15 +125,15 @@
 
 #define AO_FEC_DEBUG		0
 #define AO_CC1120_SPI_CS_PORT	(&stm_gpioa)
-#define AO_CC1120_SPI_CS_PIN	0
+#define AO_CC1120_SPI_CS_PIN	2
 #define AO_CC1120_SPI_BUS	AO_SPI_2_PB13_PB14_PB15
 #define AO_CC1120_SPI		stm_spi2
 
-#define AO_CC1120_INT_PORT	(&stm_gpioc)
-#define AO_CC1120_INT_PIN	13
+#define AO_CC1120_INT_PORT	(&stm_gpioa)
+#define AO_CC1120_INT_PIN	3
 
-#define AO_CC1120_MCU_WAKEUP_PORT	(&stm_gpioc)
-#define AO_CC1120_MCU_WAKEUP_PIN	(0)
+#define AO_CC1120_MCU_WAKEUP_PORT	(&stm_gpioa)
+#define AO_CC1120_MCU_WAKEUP_PIN	(4)
 
 #define AO_CC1120_INT_GPIO	2
 #define AO_CC1120_INT_GPIO_IOCFG	CC1120_IOCFG2
-- 
cgit v1.2.3


From 0605b5a1cdff5cfd71fc9ef3161a62d74a1124b4 Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Wed, 2 Jul 2014 22:42:10 -0700
Subject: altos: Add defines for 2400 and 9600 baud telemetry rates

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/kernel/ao.h | 13 +++++++++++++
 1 file changed, 13 insertions(+)

(limited to 'src/kernel/ao.h')

diff --git a/src/kernel/ao.h b/src/kernel/ao.h
index 1df2ea8b..6c6abd36 100644
--- a/src/kernel/ao.h
+++ b/src/kernel/ao.h
@@ -521,6 +521,10 @@ struct ao_telemetry_raw_recv {
 #define AO_TELEMETRY_INTERVAL_RECOVER	AO_MS_TO_TICKS(1000)
 #endif
 
+#define AO_RADIO_RATE_38400	0
+#define AO_RADIO_RATE_9600	1
+#define AO_RADIO_RATE_2400	2
+
 void
 ao_telemetry_set_interval(uint16_t interval);
 
@@ -558,6 +562,15 @@ extern __xdata int8_t	ao_radio_rssi;
 #define HAS_RADIO_XMIT HAS_RADIO
 #endif
 
+#define AO_RADIO_RATE_38400	0
+#define AO_RADIO_RATE_9600	1
+#define AO_RADIO_RATE_2400	2
+#define AO_RADIO_RATE_MAX	AO_RADIO_RATE_2400
+
+#if defined(HAS_RADIO) && !defined(HAS_RADIO_RATE)
+#define HAS_RADIO_RATE	HAS_RADIO
+#endif
+
 void
 ao_radio_general_isr(void) ao_arch_interrupt(16);
 
-- 
cgit v1.2.3


From ea5887027e7a39da2b7d84a142d74950b7a24703 Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Sat, 5 Jul 2014 00:09:25 -0700
Subject: altos: Call ao_telemetry_reset_interval when telemetry rate changes

This lets the radio code adjust the telemetry packet sending pattern
when the data rate changes.

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/cc1111/ao_pins.h          | 15 +++++++++++++++
 src/kernel/ao.h               |  5 ++---
 src/kernel/ao_config.c        |  7 +++++++
 src/kernel/ao_telemetry.c     | 34 ++++++++++++++++++++++++++++++----
 src/telebt-v1.0/ao_pins.h     |  1 +
 src/telefire-v0.1/ao_pins.h   |  3 ++-
 src/telefire-v0.2/ao_pins.h   |  3 ++-
 src/telemini-v2.0/ao_pins.h   |  1 +
 src/teleshield-v0.1/ao_pins.h |  1 +
 src/teleterra-v0.2/ao_pins.h  |  2 +-
 10 files changed, 62 insertions(+), 10 deletions(-)

(limited to 'src/kernel/ao.h')

diff --git a/src/cc1111/ao_pins.h b/src/cc1111/ao_pins.h
index 2b19f1f6..b40acbbd 100644
--- a/src/cc1111/ao_pins.h
+++ b/src/cc1111/ao_pins.h
@@ -56,6 +56,7 @@
 	#define HAS_ACCEL		1
 	#define HAS_IGNITE		1
 	#define HAS_MONITOR		0
+	#define HAS_TELEMETRY		1
 #endif
 
 #if defined(TELEMETRUM_V_1_1)
@@ -96,6 +97,7 @@
 	#define HAS_ACCEL		1
 	#define HAS_IGNITE		1
 	#define HAS_MONITOR		0
+	#define HAS_TELEMETRY		1
 #endif
 
 #if defined(TELEMETRUM_V_1_2)
@@ -136,6 +138,7 @@
 	#define HAS_ACCEL		1
 	#define HAS_IGNITE		1
 	#define HAS_MONITOR		0
+	#define HAS_TELEMETRY		1
 #endif
 
 #if defined(TELEDONGLE_V_0_2)
@@ -164,6 +167,7 @@
 	#define LEGACY_MONITOR		1
 	#define HAS_RSSI		1
 	#define HAS_AES			0
+	#define HAS_TELEMETRY		0
 #endif
 
 #if defined(TELEMINI_V_1_0)
@@ -193,6 +197,8 @@
 	#define HAS_ACCEL		0
 	#define HAS_IGNITE		1
 	#define HAS_MONITOR		0
+	#define HAS_TELEMETRY		1
+	#define HAS_RADIO_RATE		0	/* not enough space for this */
 #endif
 
 #if defined(TELENANO_V_0_1)
@@ -220,6 +226,8 @@
 	#define HAS_ACCEL		0
 	#define HAS_IGNITE		0
 	#define HAS_MONITOR		0
+	#define HAS_TELEMETRY		1
+	#define HAS_RADIO_RATE		0	/* not enough space for this */
 #endif
 
 #if defined(TELEMETRUM_V_0_1)
@@ -252,6 +260,8 @@
 	#define HAS_ACCEL		1
 	#define HAS_IGNITE		1
 	#define HAS_MONITOR		0
+	#define HAS_TELEMETRY		1
+	#define HAS_RADIO_RATE		0	/* not enough space for this */
 	#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX	((uint32_t) 127 * (uint32_t) 1024)
 #endif
 
@@ -283,6 +293,7 @@
 	#define LEGACY_MONITOR		1
 	#define HAS_RSSI		1
 	#define HAS_AES			0
+	#define HAS_TELEMETRY		0
 #endif
 
 #if defined(TIDONGLE)
@@ -312,6 +323,7 @@
 	#define LEGACY_MONITOR		1
 	#define HAS_RSSI		1
 	#define HAS_AES			0
+	#define HAS_TELEMETRY		0
 #endif
 
 #if defined(TELEBT_V_0_0)
@@ -350,6 +362,7 @@
 	#define LEGACY_MONITOR		1
 	#define HAS_RSSI		0
 	#define HAS_AES			0
+	#define HAS_TELEMETRY		0
 #endif
 
 #if defined(TELEBT_V_0_1)
@@ -396,6 +409,7 @@
 	#define LEGACY_MONITOR		1
 	#define HAS_RSSI		0
 	#define HAS_AES			0
+	#define HAS_TELEMETRY		0
 #endif
 
 #if defined(TELELAUNCH_V_0_1)
@@ -428,6 +442,7 @@
 	#define HAS_IGNITE		1
 	#define HAS_MONITOR		0
 	#define HAS_AES			1
+	#define HAS_TELEMETRY		0
 #endif
 
 #if DBG_ON_P1
diff --git a/src/kernel/ao.h b/src/kernel/ao.h
index 6c6abd36..c11aa028 100644
--- a/src/kernel/ao.h
+++ b/src/kernel/ao.h
@@ -521,9 +521,8 @@ struct ao_telemetry_raw_recv {
 #define AO_TELEMETRY_INTERVAL_RECOVER	AO_MS_TO_TICKS(1000)
 #endif
 
-#define AO_RADIO_RATE_38400	0
-#define AO_RADIO_RATE_9600	1
-#define AO_RADIO_RATE_2400	2
+void
+ao_telemetry_reset_interval(void);
 
 void
 ao_telemetry_set_interval(uint16_t interval);
diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c
index 52c0c4f6..32a0967c 100644
--- a/src/kernel/ao_config.c
+++ b/src/kernel/ao_config.c
@@ -492,6 +492,10 @@ ao_config_radio_cal_set(void) __reentrant
 #endif
 
 #if HAS_RADIO_RATE
+#ifndef HAS_TELEMETRY
+#error Please define HAS_TELEMETRY
+#endif
+
 void
 ao_config_radio_rate_show(void) __reentrant
 {
@@ -510,6 +514,9 @@ ao_config_radio_rate_set(void) __reentrant
 	}
 	_ao_config_edit_start();
 	ao_config.radio_rate = ao_cmd_lex_i;
+#if HAS_TELEMETRY
+	ao_telemetry_reset_interval();
+#endif
 	_ao_config_edit_finish();
 }
 #endif
diff --git a/src/kernel/ao_telemetry.c b/src/kernel/ao_telemetry.c
index 6fb30069..f4fcf400 100644
--- a/src/kernel/ao_telemetry.c
+++ b/src/kernel/ao_telemetry.c
@@ -25,6 +25,10 @@
 
 static __pdata uint16_t ao_telemetry_interval;
 
+#if HAS_RADIO_RATE
+static __pdata uint16_t ao_telemetry_desired_interval;
+#endif
+
 #if HAS_RDF
 static __pdata uint8_t ao_rdf = 0;
 static __pdata uint16_t ao_rdf_time;
@@ -64,7 +68,7 @@ static void
 ao_send_sensor(void)
 {
 	__xdata	struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)];
-			
+
 	telemetry.generic.tick = packet->tick;
 	telemetry.generic.type = AO_TELEMETRY_SENSOR;
 
@@ -106,12 +110,13 @@ ao_send_sensor(void)
 
 
 #ifdef AO_SEND_MEGA
+
 /* Send mega sensor packet */
 static void
 ao_send_mega_sensor(void)
 {
 	__xdata	struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)];
-			
+
 	telemetry.generic.tick = packet->tick;
 	telemetry.generic.type = AO_TELEMETRY_MEGA_SENSOR;
 
@@ -240,7 +245,7 @@ static void
 ao_send_mini(void)
 {
 	__xdata	struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)];
-			
+
 	telemetry.generic.tick = packet->tick;
 	telemetry.generic.type = AO_TELEMETRY_MINI;
 
@@ -490,12 +495,33 @@ ao_telemetry(void)
 	}
 }
 
+#if HAS_RADIO_RATE
+void
+ao_telemetry_reset_interval(void)
+{
+	ao_telemetry_set_interval(ao_telemetry_desired_interval);
+}
+#endif
+
 void
 ao_telemetry_set_interval(uint16_t interval)
 {
 	int8_t	cur = 0;
+
+#if HAS_RADIO_RATE
+	/* Limit max telemetry rate based on available radio bandwidth.
+	 */
+	static const uint16_t min_interval[] = {
+		/* [AO_RADIO_RATE_38400] = */ AO_MS_TO_TICKS(100),
+		/* [AO_RADIO_RATE_9600] = */ AO_MS_TO_TICKS(500),
+		/* [AO_RADIO_RATE_2400] = */ AO_MS_TO_TICKS(1000)
+	};
+
+	ao_telemetry_desired_interval = interval;
+	if (interval < min_interval[ao_config.radio_rate])
+		interval = min_interval[ao_config.radio_rate];
+#endif
 	ao_telemetry_interval = interval;
-	
 #if AO_SEND_MEGA
 	if (interval > 1)
 		ao_telemetry_mega_data_max = 1;
diff --git a/src/telebt-v1.0/ao_pins.h b/src/telebt-v1.0/ao_pins.h
index 9e47f3b8..b5562573 100644
--- a/src/telebt-v1.0/ao_pins.h
+++ b/src/telebt-v1.0/ao_pins.h
@@ -48,6 +48,7 @@
 #define BT_LINK_PIN		P1_7
 #define HAS_MONITOR		1
 #define LEGACY_MONITOR		0
+#define HAS_TELEMETRY		0
 
 #define HAS_ADC			1
 #define AO_PAD_ADC_BATT		0
diff --git a/src/telefire-v0.1/ao_pins.h b/src/telefire-v0.1/ao_pins.h
index f7a3ff2c..47ae663f 100644
--- a/src/telefire-v0.1/ao_pins.h
+++ b/src/telefire-v0.1/ao_pins.h
@@ -18,7 +18,8 @@
 #ifndef _AO_PINS_H_
 #define _AO_PINS_H_
 
-#define HAS_RADIO	1
+#define HAS_RADIO		1
+#define HAS_TELEMETRY		0
 
 #define HAS_FLIGHT		0
 #define HAS_USB			1
diff --git a/src/telefire-v0.2/ao_pins.h b/src/telefire-v0.2/ao_pins.h
index 96e6b066..9e6631ce 100644
--- a/src/telefire-v0.2/ao_pins.h
+++ b/src/telefire-v0.2/ao_pins.h
@@ -18,7 +18,8 @@
 #ifndef _AO_PINS_H_
 #define _AO_PINS_H_
 
-#define HAS_RADIO	1
+#define HAS_RADIO		1
+#define HAS_TELEMETRY		0
 
 #define HAS_FLIGHT		0
 #define HAS_USB			1
diff --git a/src/telemini-v2.0/ao_pins.h b/src/telemini-v2.0/ao_pins.h
index f202ccd1..c9f9de62 100644
--- a/src/telemini-v2.0/ao_pins.h
+++ b/src/telemini-v2.0/ao_pins.h
@@ -39,6 +39,7 @@
 #define USE_INTERNAL_FLASH	0
 #define HAS_DBG			0
 #define PACKET_HAS_SLAVE	1
+#define HAS_RADIO_RATE		0	/* not enough space for this */
 
 #define AO_LED_RED		2
 #define LEDS_AVAILABLE		AO_LED_RED
diff --git a/src/teleshield-v0.1/ao_pins.h b/src/teleshield-v0.1/ao_pins.h
index 30239afc..68bb44ee 100644
--- a/src/teleshield-v0.1/ao_pins.h
+++ b/src/teleshield-v0.1/ao_pins.h
@@ -62,6 +62,7 @@
 	#define HAS_RSSI		0
 	#define HAS_AES			0
 	#define HAS_RADIO		1
+	#define HAS_TELEMETRY		0
 #endif
 
 #if DBG_ON_P1
diff --git a/src/teleterra-v0.2/ao_pins.h b/src/teleterra-v0.2/ao_pins.h
index 1c12c437..60d627ad 100644
--- a/src/teleterra-v0.2/ao_pins.h
+++ b/src/teleterra-v0.2/ao_pins.h
@@ -71,7 +71,7 @@
 	#define HAS_P2_ISR		1
 
 	#define BATTERY_PIN		5
-	
+	#define HAS_TELEMETRY		0
 #endif
 
 #if DBG_ON_P1
-- 
cgit v1.2.3


From 292cb8380b478542555b5f370e8252eafa2f74ac Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Sat, 5 Jul 2014 00:04:06 -0700
Subject: altos: Rework packet receive for cc1120

Instead of blocking on PQT, just set up the receiver to start going
and when the first bit interrupt comes in, grab the SPI bus if
possible and configure it for reception. This improves sensitivity in
the radio by a significant amount while making the code conceptually a
bit nicer.

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/cc1111/ao_pins.h    |   3 +
 src/drivers/ao_cc1120.c | 160 +++++++++++++++++++++++++++++++-----------------
 src/kernel/ao.h         |   3 +
 src/kernel/ao_mutex.c   |  23 +++++++
 src/lpc/ao_arch_funcs.h |  17 ++---
 src/stm/ao_arch_funcs.h |  25 ++++++--
 src/stm/ao_spi_stm.c    |  51 +++++++++++++--
 7 files changed, 208 insertions(+), 74 deletions(-)

(limited to 'src/kernel/ao.h')

diff --git a/src/cc1111/ao_pins.h b/src/cc1111/ao_pins.h
index 83a3c774..1bc3d716 100644
--- a/src/cc1111/ao_pins.h
+++ b/src/cc1111/ao_pins.h
@@ -58,6 +58,7 @@
 	#define HAS_MONITOR		0
 	#define HAS_TELEMETRY		1
 	#define HAS_RADIO_RATE		0	/* not enough space for this */
+	#define HAS_MUTEX_TRY		0
 #endif
 
 #if defined(TELEMETRUM_V_1_1)
@@ -100,6 +101,7 @@
 	#define HAS_MONITOR		0
 	#define HAS_TELEMETRY		1
 	#define HAS_RADIO_RATE		0	/* not enough space for this */
+	#define HAS_MUTEX_TRY		0
 #endif
 
 #if defined(TELEMETRUM_V_1_2)
@@ -142,6 +144,7 @@
 	#define HAS_MONITOR		0
 	#define HAS_TELEMETRY		1
 	#define HAS_RADIO_RATE		0	/* not enough space for this */
+	#define HAS_MUTEX_TRY		0
 #endif
 
 #if defined(TELEDONGLE_V_0_2)
diff --git a/src/drivers/ao_cc1120.c b/src/drivers/ao_cc1120.c
index 5d51fbcd..1b907940 100644
--- a/src/drivers/ao_cc1120.c
+++ b/src/drivers/ao_cc1120.c
@@ -41,8 +41,10 @@ extern const uint32_t	ao_radio_cal;
 
 #define FOSC	32000000
 
+#define ao_radio_try_select(task_id)	ao_spi_try_get_mask(AO_CC1120_SPI_CS_PORT,(1 << AO_CC1120_SPI_CS_PIN),AO_CC1120_SPI_BUS,AO_SPI_SPEED_4MHz, task_id)
 #define ao_radio_select()	ao_spi_get_mask(AO_CC1120_SPI_CS_PORT,(1 << AO_CC1120_SPI_CS_PIN),AO_CC1120_SPI_BUS,AO_SPI_SPEED_4MHz)
 #define ao_radio_deselect()	ao_spi_put_mask(AO_CC1120_SPI_CS_PORT,(1 << AO_CC1120_SPI_CS_PIN),AO_CC1120_SPI_BUS)
+#define ao_radio_spi_send_sync(d,l)	ao_spi_send_sync((d), (l), AO_CC1120_SPI_BUS)
 #define ao_radio_spi_send(d,l)	ao_spi_send((d), (l), AO_CC1120_SPI_BUS)
 #define ao_radio_spi_send_fixed(d,l) ao_spi_send_fixed((d), (l), AO_CC1120_SPI_BUS)
 #define ao_radio_spi_recv(d,l)	ao_spi_recv((d), (l), AO_CC1120_SPI_BUS)
@@ -107,7 +109,7 @@ ao_radio_reg_write(uint16_t addr, uint8_t value)
 }
 
 static void
-ao_radio_burst_read_start (uint16_t addr)
+_ao_radio_burst_read_start (uint16_t addr)
 {
 	uint8_t data[2];
 	uint8_t d;
@@ -124,8 +126,8 @@ ao_radio_burst_read_start (uint16_t addr)
 			   addr);
 		d = 1;
 	}
-	ao_radio_select();
-	ao_radio_spi_send(data, d);
+
+	ao_radio_spi_send_sync(data, d);
 }
 
 static void
@@ -209,7 +211,7 @@ ao_radio_tx_fifo_space(void)
 	return CC1120_FIFO_SIZE - ao_radio_reg_read(CC1120_NUM_TXBYTES);
 }
 
-#if 0
+#if CC1120_DEBUG
 static uint8_t
 ao_radio_status(void)
 {
@@ -275,11 +277,13 @@ static void
 ao_radio_idle(void)
 {
 	for (;;) {
-		uint8_t	state = ao_radio_strobe(CC1120_SIDLE);
-		if ((state >> CC1120_STATUS_STATE) == CC1120_STATUS_STATE_IDLE)
+		uint8_t	state = (ao_radio_strobe(CC1120_SIDLE) >> CC1120_STATUS_STATE) & CC1120_STATUS_STATE_MASK;
+		if (state == CC1120_STATUS_STATE_IDLE)
 			break;
-		if ((state >> CC1120_STATUS_STATE) == CC1120_STATUS_STATE_TX_FIFO_ERROR)
+		if (state == CC1120_STATUS_STATE_TX_FIFO_ERROR)
 			ao_radio_strobe(CC1120_SFTX);
+		if (state == CC1120_STATUS_STATE_RX_FIFO_ERROR)
+			ao_radio_strobe(CC1120_SFRX);
 	}
 	/* Flush any pending TX bytes */
 	ao_radio_strobe(CC1120_SFTX);
@@ -948,6 +952,11 @@ static uint16_t	rx_data_consumed;
 static uint16_t rx_data_cur;
 static uint8_t	rx_ignore;
 static uint8_t	rx_waiting;
+static uint8_t	rx_starting;
+static uint8_t	rx_task_id;
+static uint32_t	rx_fast_start;
+static uint32_t rx_slow_start;
+static uint32_t	rx_missed;
 
 #if AO_PROFILE
 static uint32_t	rx_start_tick, rx_packet_tick, rx_done_tick, rx_last_done_tick;
@@ -962,13 +971,34 @@ ao_radio_rx_isr(void)
 {
 	uint8_t	d;
 
+	if (rx_task_id) {
+		if (ao_radio_try_select(rx_task_id)) {
+			++rx_fast_start;
+			rx_task_id = 0;
+			_ao_radio_burst_read_start(CC1120_SOFT_RX_DATA_OUT);
+		} else {
+			if (rx_ignore)
+				--rx_ignore;
+			else {
+				ao_radio_abort = 1;
+				rx_missed++;
+			}
+			return;
+		}
+	}
+	if (rx_starting) {
+		rx_starting = 0;
+		ao_wakeup(&ao_radio_wake);
+	}
 	d = AO_CC1120_SPI.dr;
 	AO_CC1120_SPI.dr = 0;
 	if (rx_ignore == 0) {
-		if (rx_data_cur >= rx_data_count)
-			ao_exti_disable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
-		else
+		if (rx_data_cur < rx_data_count)
 			rx_data[rx_data_cur++] = d;
+		if (rx_data_cur >= rx_data_count) {
+			ao_spi_clr_cs(AO_CC1120_SPI_CS_PORT,(1 << AO_CC1120_SPI_CS_PIN));
+			ao_exti_disable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
+		}
 		if (rx_waiting && rx_data_cur - rx_data_consumed >= AO_FEC_DECODE_BLOCK) {
 #if AO_PROFILE
 			if (!rx_packet_tick)
@@ -987,24 +1017,20 @@ ao_radio_rx_isr(void)
 static uint16_t
 ao_radio_rx_wait(void)
 {
-	do {
-		if (ao_radio_mcu_wake)
-			ao_radio_check_marc_status();
-		ao_alarm(AO_MS_TO_TICKS(100));
-		ao_arch_block_interrupts();
-		rx_waiting = 1;
-		while (rx_data_cur - rx_data_consumed < AO_FEC_DECODE_BLOCK &&
-		       !ao_radio_abort &&
-		       !ao_radio_mcu_wake)
-		{
-			if (ao_sleep(&ao_radio_wake))
-				ao_radio_abort = 1;
-		}
-		rx_waiting = 0;
-		ao_arch_release_interrupts();
-		ao_clear_alarm();
-	} while (ao_radio_mcu_wake);
-	if (ao_radio_abort)
+	ao_alarm(AO_MS_TO_TICKS(100));
+	ao_arch_block_interrupts();
+	rx_waiting = 1;
+	while (rx_data_cur - rx_data_consumed < AO_FEC_DECODE_BLOCK &&
+	       !ao_radio_abort &&
+	       !ao_radio_mcu_wake)
+	{
+		if (ao_sleep(&ao_radio_wake))
+			ao_radio_abort = 1;
+	}
+	rx_waiting = 0;
+	ao_arch_release_interrupts();
+	ao_clear_alarm();
+	if (ao_radio_abort || ao_radio_mcu_wake)
 		return 0;
 	rx_data_consumed += AO_FEC_DECODE_BLOCK;
 #if AO_PROFILE
@@ -1044,48 +1070,67 @@ ao_radio_recv(__xdata void *d, uint8_t size, uint8_t timeout)
 	 */
 	ao_radio_abort = 0;
 
-	/* configure interrupt pin */
 	ao_radio_get(len);
-	ao_radio_set_mode(AO_RADIO_MODE_PACKET_RX);
 
 	ao_radio_wake = 0;
 	ao_radio_mcu_wake = 0;
 
-	AO_CC1120_SPI.cr2 = 0;
-
-	/* clear any RXNE */
-	(void) AO_CC1120_SPI.dr;
+	ao_radio_set_mode(AO_RADIO_MODE_PACKET_RX);
 
-	/* Have the radio signal when the preamble quality goes high */
-	ao_radio_reg_write(AO_CC1120_INT_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_PQT_REACHED);
+	/* configure interrupt pin */
+	ao_radio_reg_write(AO_CC1120_INT_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_CLKEN_SOFT);
 	ao_exti_set_mode(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN,
-			 AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_HIGH);
-	ao_exti_set_callback(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_isr);
+			 AO_EXTI_MODE_FALLING|AO_EXTI_PRIORITY_HIGH);
+
+	ao_exti_set_callback(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_rx_isr);
 	ao_exti_enable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
-	ao_exti_enable(AO_CC1120_MCU_WAKEUP_PORT, AO_CC1120_MCU_WAKEUP_PIN);
+
+	rx_starting = 1;
+	rx_task_id = ao_cur_task->task_id;
 
 	ao_radio_strobe(CC1120_SRX);
 
-	/* Wait for the preamble to appear */
-	ao_radio_wait_isr(timeout);
+	if (timeout)
+		ao_alarm(timeout);
+	ao_arch_block_interrupts();
+	while (rx_starting && !ao_radio_abort) {
+		if (ao_sleep(&ao_radio_wake))
+			ao_radio_abort = 1;
+	}
+	uint8_t	rx_task_id_save = rx_task_id;
+	rx_task_id = 0;
+	rx_starting = 0;
+	ao_arch_release_interrupts();
+	if (timeout)
+		ao_clear_alarm();
+
 	if (ao_radio_abort) {
 		ret = 0;
+		rx_task_id = 0;
 		goto abort;
 	}
 
-	ao_radio_reg_write(AO_CC1120_INT_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_CLKEN_SOFT);
-	ao_exti_set_mode(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN,
-			 AO_EXTI_MODE_FALLING|AO_EXTI_PRIORITY_HIGH);
-
-	ao_exti_set_callback(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_rx_isr);
-	ao_exti_enable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN);
-
-	ao_radio_burst_read_start(CC1120_SOFT_RX_DATA_OUT);
+	if (rx_task_id_save) {
+		++rx_slow_start;
+		ao_radio_select();
+		_ao_radio_burst_read_start(CC1120_SOFT_RX_DATA_OUT);
+		if (rx_ignore) {
+			uint8_t ignore = AO_CC1120_SPI.dr;
+			(void) ignore;
+			AO_CC1120_SPI.dr = 0;
+			--rx_ignore;
+		}
+	}
 
 	ret = ao_fec_decode(rx_data, rx_data_count, d, size + 2, ao_radio_rx_wait);
 
 	ao_radio_burst_read_stop();
 
+	if (ao_radio_mcu_wake)
+		ao_radio_check_marc_status();
+	if (ao_radio_abort)
+		ret = 0;
+
 abort:
 	/* Convert from 'real' rssi to cc1111-style values */
 
@@ -1100,7 +1145,7 @@ abort:
 		radio_rssi = AO_RADIO_FROM_RSSI (rssi);
 	}
 
-	ao_radio_strobe(CC1120_SIDLE);
+	ao_radio_idle();
 
 	ao_radio_put();
 
@@ -1139,7 +1184,7 @@ struct ao_cc1120_reg {
 	char		*name;
 };
 
-const static struct ao_cc1120_reg ao_cc1120_reg[] = {
+static const struct ao_cc1120_reg ao_cc1120_reg[] = {
 	{ .addr = CC1120_IOCFG3,	.name = "IOCFG3" },
 	{ .addr = CC1120_IOCFG2,	.name = "IOCFG2" },
 	{ .addr = CC1120_IOCFG1,	.name = "IOCFG1" },
@@ -1320,7 +1365,7 @@ const static struct ao_cc1120_reg ao_cc1120_reg[] = {
 
 static void ao_radio_show(void) {
 	uint8_t	status = ao_radio_status();
-	int	i;
+	unsigned int	i;
 
 	ao_radio_get(0xff);
 	status = ao_radio_status();
@@ -1331,6 +1376,10 @@ static void ao_radio_show(void) {
 
 	for (i = 0; i < AO_NUM_CC1120_REG; i++)
 		printf ("\t%02x %-20.20s\n", ao_radio_reg_read(ao_cc1120_reg[i].addr), ao_cc1120_reg[i].name);
+
+	printf("RX fast start: %u\n", rx_fast_start);
+	printf("RX slow start: %u\n", rx_slow_start);
+	printf("RX missed:     %u\n", rx_missed);
 	ao_radio_put();
 }
 
@@ -1354,12 +1403,12 @@ static void ao_radio_packet(void) {
 }
 
 void
-ao_radio_test_recv()
+ao_radio_test_recv(void)
 {
 	uint8_t	bytes[34];
 	uint8_t	b;
 
-	if (ao_radio_recv(bytes, 34)) {
+	if (ao_radio_recv(bytes, 34, 0)) {
 		if (bytes[33] & 0x80)
 			printf ("CRC OK");
 		else
@@ -1375,13 +1424,12 @@ ao_radio_test_recv()
 #include <ao_aprs.h>
 
 static void
-ao_radio_aprs()
+ao_radio_aprs(void)
 {
 	ao_packet_slave_stop();
 	ao_aprs_send();
 }
 #endif
-
 #endif
 
 static const struct ao_cmds ao_radio_cmds[] = {
diff --git a/src/kernel/ao.h b/src/kernel/ao.h
index c11aa028..a225bc4a 100644
--- a/src/kernel/ao.h
+++ b/src/kernel/ao.h
@@ -132,6 +132,9 @@ ao_clock_init(void);
  */
 
 #ifndef ao_mutex_get
+uint8_t
+ao_mutex_try(__xdata uint8_t *ao_mutex, uint8_t task_id) __reentrant;
+
 void
 ao_mutex_get(__xdata uint8_t *ao_mutex) __reentrant;
 
diff --git a/src/kernel/ao_mutex.c b/src/kernel/ao_mutex.c
index 952ff462..a36fe939 100644
--- a/src/kernel/ao_mutex.c
+++ b/src/kernel/ao_mutex.c
@@ -17,6 +17,29 @@
 
 #include "ao.h"
 
+#ifndef HAS_MUTEX_TRY
+#define HAS_MUTEX_TRY 1
+#endif
+
+#if HAS_MUTEX_TRY
+
+uint8_t
+ao_mutex_try(__xdata uint8_t *mutex, uint8_t task_id) __reentrant
+{
+	uint8_t	ret;
+	if (*mutex == task_id)
+		ao_panic(AO_PANIC_MUTEX);
+	ao_arch_critical(
+		if (*mutex)
+			ret = 0;
+		else {
+			*mutex = task_id;
+			ret = 1;
+		});
+	return ret;
+}
+#endif
+
 void
 ao_mutex_get(__xdata uint8_t *mutex) __reentrant
 {
diff --git a/src/lpc/ao_arch_funcs.h b/src/lpc/ao_arch_funcs.h
index 0891903e..21a7a8e5 100644
--- a/src/lpc/ao_arch_funcs.h
+++ b/src/lpc/ao_arch_funcs.h
@@ -161,16 +161,17 @@ static inline void ao_arch_restore_stack(void) {
 
 #endif /* HAS_TASK */
 
-#define ao_arch_wait_interrupt() do {			\
-		asm(".global ao_idle_loc\n\twfi\nao_idle_loc:");	\
-		ao_arch_release_interrupts();				\
-		ao_arch_block_interrupts();				\
+#define ao_arch_wait_interrupt() do {				\
+		asm("\twfi\n");					\
+		ao_arch_release_interrupts();			\
+		asm(".global ao_idle_loc\n\nao_idle_loc:");	\
+		ao_arch_block_interrupts();			\
 	} while (0)
 
-#define ao_arch_critical(b) do {				\
-		ao_arch_block_interrupts();			\
-		do { b } while (0);				\
-		ao_arch_release_interrupts();			\
+#define ao_arch_critical(b) do {			\
+		uint32_t __mask = ao_arch_irqsave();	\
+		do { b } while (0);			\
+		ao_arch_irqrestore(__mask);		\
 	} while (0)
 
 /*
diff --git a/src/stm/ao_arch_funcs.h b/src/stm/ao_arch_funcs.h
index b461cd3f..7ad3b4b8 100644
--- a/src/stm/ao_arch_funcs.h
+++ b/src/stm/ao_arch_funcs.h
@@ -64,6 +64,9 @@
 #define AO_SPI_INDEX(id)	((id) & AO_SPI_INDEX_MASK)
 #define AO_SPI_CONFIG(id)	((id) & AO_SPI_CONFIG_MASK)
 
+uint8_t
+ao_spi_try_get(uint8_t spi_index, uint32_t speed, uint8_t task_id);
+
 void
 ao_spi_get(uint8_t spi_index, uint32_t speed);
 
@@ -76,6 +79,9 @@ ao_spi_send(void *block, uint16_t len, uint8_t spi_index);
 void
 ao_spi_send_fixed(uint8_t value, uint16_t len, uint8_t spi_index);
 
+void
+ao_spi_send_sync(void *block, uint16_t len, uint8_t spi_index);
+
 void
 ao_spi_recv(void *block, uint16_t len, uint8_t spi_index);
 
@@ -95,6 +101,15 @@ ao_spi_init(void);
 		ao_spi_set_cs(reg,mask);			\
 	} while (0)
 
+static inline uint8_t
+ao_spi_try_get_mask(struct stm_gpio *reg, uint16_t mask, uint8_t bus, uint32_t speed, uint8_t task_id)
+{
+	if (!ao_spi_try_get(bus, speed, task_id))
+		return 0;
+	ao_spi_set_cs(reg, mask);
+	return 1;
+}
+
 #define ao_spi_put_mask(reg,mask,bus) do {	\
 		ao_spi_clr_cs(reg,mask);	\
 		ao_spi_put(bus);		\
@@ -252,6 +267,8 @@ extern struct ao_stm_usart	ao_stm_usart3;
 
 #define ARM_PUSH32(stack, val)	(*(--(stack)) = (val))
 
+typedef uint32_t	ao_arch_irq_t;
+
 static inline uint32_t
 ao_arch_irqsave(void) {
 	uint32_t	primask;
@@ -369,10 +386,10 @@ static inline void ao_arch_start_scheduler(void) {
 		ao_arch_block_interrupts();			\
 	} while (0)
 
-#define ao_arch_critical(b) do {				\
-		ao_arch_block_interrupts();			\
-		do { b } while (0);				\
-		ao_arch_release_interrupts();			\
+#define ao_arch_critical(b) do {			\
+		uint32_t __mask = ao_arch_irqsave();	\
+		do { b } while (0);			\
+		ao_arch_irqrestore(__mask);		\
 	} while (0)
 
 #endif /* _AO_ARCH_FUNCS_H_ */
diff --git a/src/stm/ao_spi_stm.c b/src/stm/ao_spi_stm.c
index 56329c24..885af544 100644
--- a/src/stm/ao_spi_stm.c
+++ b/src/stm/ao_spi_stm.c
@@ -153,6 +153,28 @@ ao_spi_send_fixed(uint8_t value, uint16_t len, uint8_t spi_index)
 	ao_dma_done_transfer(miso_dma_index);
 }
 
+void
+ao_spi_send_sync(void *block, uint16_t len, uint8_t spi_index)
+{
+	uint8_t		*b = block;
+	struct stm_spi	*stm_spi = ao_spi_stm_info[AO_SPI_INDEX(spi_index)].stm_spi;
+
+	stm_spi->cr2 = ((0 << STM_SPI_CR2_TXEIE) |
+			(0 << STM_SPI_CR2_RXNEIE) |
+			(0 << STM_SPI_CR2_ERRIE) |
+			(0 << STM_SPI_CR2_SSOE) |
+			(0 << STM_SPI_CR2_TXDMAEN) |
+			(0 << STM_SPI_CR2_RXDMAEN));
+
+	/* Clear RXNE */
+	(void) stm_spi->dr;
+
+	while (len--) {
+		while (!(stm_spi->sr & (1 << STM_SPI_SR_TXE)));
+		stm_spi->dr = *b++;
+	}
+}
+
 void
 ao_spi_recv(void *block, uint16_t len, uint8_t spi_index)
 {
@@ -356,13 +378,11 @@ ao_spi_enable_index(uint8_t spi_index)
 	}
 }
 
-void
-ao_spi_get(uint8_t spi_index, uint32_t speed)
+static void
+ao_spi_config(uint8_t spi_index, uint32_t speed)
 {
 	uint8_t		id = AO_SPI_INDEX(spi_index);
 	struct stm_spi	*stm_spi = ao_spi_stm_info[id].stm_spi;
-
-	ao_mutex_get(&ao_spi_mutex[id]);
 	stm_spi->cr1 = ((0 << STM_SPI_CR1_BIDIMODE) |			/* Three wire mode */
 			(0 << STM_SPI_CR1_BIDIOE) |
 			(0 << STM_SPI_CR1_CRCEN) |			/* CRC disabled */
@@ -378,7 +398,7 @@ ao_spi_get(uint8_t spi_index, uint32_t speed)
 			(0 << STM_SPI_CR1_CPOL) |			/* Format 0 */
 			(0 << STM_SPI_CR1_CPHA));
 	if (spi_index != ao_spi_index[id]) {
-		
+
 		/* Disable old config
 		 */
 		ao_spi_disable_index(ao_spi_index[id]);
@@ -386,13 +406,32 @@ ao_spi_get(uint8_t spi_index, uint32_t speed)
 		/* Enable new config
 		 */
 		ao_spi_enable_index(spi_index);
-		
+
 		/* Remember current config
 		 */
 		ao_spi_index[id] = spi_index;
 	}
 }
 
+uint8_t
+ao_spi_try_get(uint8_t spi_index, uint32_t speed, uint8_t task_id)
+{
+	uint8_t		id = AO_SPI_INDEX(spi_index);
+
+	if (!ao_mutex_try(&ao_spi_mutex[id], task_id))
+		return 0;
+	ao_spi_config(spi_index, speed);
+	return 1;
+}
+
+void
+ao_spi_get(uint8_t spi_index, uint32_t speed)
+{
+	uint8_t		id = AO_SPI_INDEX(spi_index);
+	ao_mutex_get(&ao_spi_mutex[id]);
+	ao_spi_config(spi_index, speed);
+}
+
 void
 ao_spi_put(uint8_t spi_index)
 {
-- 
cgit v1.2.3


From 013e9ccfbe76dc46e8c69ea314950bed83d9a39f Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Thu, 10 Jul 2014 17:18:38 -0700
Subject: altos: Use 32-bits for flight state data (alt/speed/accel)

Stores 32-bits for all of the flight parameters. Uses 64-bit
intermediates for kalman computation.

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 src/Makefile                    |   2 +-
 src/cc1111/ao_pins.h            |   1 +
 src/kalman/kalman.5c            |   2 +-
 src/kalman/kalman_micro.5c      |   2 +-
 src/kernel/ao.h                 |  14 ++--
 src/kernel/ao_cmd.c             |   3 +
 src/kernel/ao_convert.c         |  10 +--
 src/kernel/ao_convert_pa.c      |  16 ++---
 src/kernel/ao_convert_pa_test.c |   1 +
 src/kernel/ao_data.h            |   6 --
 src/kernel/ao_flight.c          |  12 ++--
 src/kernel/ao_kalman.c          | 147 ++++++++++++++++++++--------------------
 src/kernel/ao_microkalman.c     |  12 ++--
 src/kernel/ao_sample.c          |   2 +-
 src/kernel/ao_sample.h          |  56 +++++++++++----
 src/product/Makefile.telemetrum |   1 +
 src/telefire-v0.1/Makefile      |   1 -
 src/telefire-v0.2/Makefile      |   1 -
 src/telemega-v1.0/Makefile      |   1 +
 src/telemini-v2.0/ao_pins.h     |   1 +
 src/teleterra-v0.2/ao_pins.h    |   2 +
 src/test/ao_flight_test.c       |   5 +-
 src/test/ao_micropeak_test.c    |   1 +
 23 files changed, 170 insertions(+), 129 deletions(-)

(limited to 'src/kernel/ao.h')

diff --git a/src/Makefile b/src/Makefile
index a7a26b26..3494ba62 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -109,7 +109,7 @@ altitude-pa.h: make-altitude-pa
 altitude-pa-small.h: make-altitude-pa
 	nickle $< --sample 3 > $@
 
-ao_kalman.h: make-kalman kalman.5c kalman_filter.5c load_csv.5c matrix.5c
+ao_kalman.h: make-kalman kalman.5c kalman_micro.5c kalman_filter.5c load_csv.5c matrix.5c
 	bash $< kalman > $@
 
 ao_whiten.h: make-whiten
diff --git a/src/cc1111/ao_pins.h b/src/cc1111/ao_pins.h
index 4db49215..e12f813f 100644
--- a/src/cc1111/ao_pins.h
+++ b/src/cc1111/ao_pins.h
@@ -20,6 +20,7 @@
 
 #define HAS_RADIO		1
 #define DISABLE_LOG_SPACE	1
+#define AO_VALUE_32		0
 #define HAS_WIDE_GPS		0
 
 #if defined(TELEMETRUM_V_1_0)
diff --git a/src/kalman/kalman.5c b/src/kalman/kalman.5c
index 46fa8e1f..55fde04c 100755
--- a/src/kalman/kalman.5c
+++ b/src/kalman/kalman.5c
@@ -457,7 +457,7 @@ void main() {
 					name = sprintf("%s_K%d_%d", prefix, i, time_inc);
 				else
 					name = sprintf("%s_K%d%d_%d", prefix, i, j, time_inc);
-				printf ("#define %s to_fix32(%12.10f)\n", name, k[i,j]);
+				printf ("#define %s to_fix_k(%12.10f)\n", name, k[i,j]);
 			}
 		printf ("\n");
 		exit(0);
diff --git a/src/kalman/kalman_micro.5c b/src/kalman/kalman_micro.5c
index 266a1182..1b080384 100644
--- a/src/kalman/kalman_micro.5c
+++ b/src/kalman/kalman_micro.5c
@@ -307,7 +307,7 @@ void main() {
 					name = sprintf("%s_K%d_%d", prefix, i, time_inc);
 				else
 					name = sprintf("%s_K%d%d_%d", prefix, i, j, time_inc);
-				printf ("#define %s to_fix32(%12.10f)\n", name, k[i,j]);
+				printf ("#define %s to_fix_k(%12.10f)\n", name, k[i,j]);
 			}
 		printf ("\n");
 		exit(0);
diff --git a/src/kernel/ao.h b/src/kernel/ao.h
index a225bc4a..ad5bbf8e 100644
--- a/src/kernel/ao.h
+++ b/src/kernel/ao.h
@@ -278,15 +278,17 @@ ao_report_init(void);
  * Given raw data, convert to SI units
  */
 
+#if HAS_BARO
 /* pressure from the sensor to altitude in meters */
-int16_t
-ao_pres_to_altitude(int16_t pres) __reentrant;
+alt_t
+ao_pres_to_altitude(pres_t pres) __reentrant;
 
-int16_t
-ao_altitude_to_pres(int16_t alt) __reentrant;
+pres_t
+ao_altitude_to_pres(alt_t alt) __reentrant;
 
 int16_t
 ao_temp_to_dC(int16_t temp) __reentrant;
+#endif
 
 /*
  * ao_convert_pa.c
@@ -296,11 +298,13 @@ ao_temp_to_dC(int16_t temp) __reentrant;
 
 #include <ao_data.h>
 
+#if HAS_BARO
 alt_t
-ao_pa_to_altitude(int32_t pa);
+ao_pa_to_altitude(pres_t pa);
 
 int32_t
 ao_altitude_to_pa(alt_t alt);
+#endif
 
 #if HAS_DBG
 #include <ao_dbg.h>
diff --git a/src/kernel/ao_cmd.c b/src/kernel/ao_cmd.c
index 0052bdce..d2f583ef 100644
--- a/src/kernel/ao_cmd.c
+++ b/src/kernel/ao_cmd.c
@@ -289,6 +289,9 @@ version(void)
 #endif
 #if defined(AO_BOOT_APPLICATION_BASE) && defined(AO_BOOT_APPLICATION_BOUND)
 	       "program-space    %u\n"
+#endif
+#if AO_VALUE_32
+	       "altitude-32      1\n"
 #endif
 	       , ao_manufacturer
 	       , ao_product
diff --git a/src/kernel/ao_convert.c b/src/kernel/ao_convert.c
index aa9b5f48..db1f2301 100644
--- a/src/kernel/ao_convert.c
+++ b/src/kernel/ao_convert.c
@@ -19,14 +19,16 @@
 #include "ao.h"
 #endif
 
-static const int16_t altitude_table[] = {
+#include <ao_sample.h>
+
+static const ao_v_t altitude_table[] = {
 #include "altitude.h"
 };
 
 #define ALT_FRAC_SCALE	(1 << ALT_FRAC_BITS)
 #define ALT_FRAC_MASK	(ALT_FRAC_SCALE - 1)
 
-int16_t
+ao_v_t
 ao_pres_to_altitude(int16_t pres) __reentrant
 {
 	uint8_t	o;
@@ -43,9 +45,9 @@ ao_pres_to_altitude(int16_t pres) __reentrant
 
 #if AO_NEED_ALTITUDE_TO_PRES
 int16_t
-ao_altitude_to_pres(int16_t alt) __reentrant
+ao_altitude_to_pres(ao_v_t alt) __reentrant
 {
-	int16_t span, sub_span;
+	ao_v_t span, sub_span;
 	uint8_t	l, h, m;
 	int32_t pres;
 
diff --git a/src/kernel/ao_convert_pa.c b/src/kernel/ao_convert_pa.c
index 20162c1f..410815b6 100644
--- a/src/kernel/ao_convert_pa.c
+++ b/src/kernel/ao_convert_pa.c
@@ -39,11 +39,11 @@ static const alt_t altitude_table[] AO_CONST_ATTRIB = {
 #define ALT_MASK	(ALT_SCALE - 1)
 
 alt_t
-ao_pa_to_altitude(int32_t pa)
+ao_pa_to_altitude(pres_t pa)
 {
 	int16_t	o;
 	int16_t	part;
-	int32_t low, high;
+	alt_t low, high;
 
 	if (pa < 0)
 		pa = 0;
@@ -52,16 +52,16 @@ ao_pa_to_altitude(int32_t pa)
 	o = pa >> ALT_SHIFT;
 	part = pa & ALT_MASK;
 
-	low = (int32_t) FETCH_ALT(o) * (ALT_SCALE - part);
-	high = (int32_t) FETCH_ALT(o+1) * part + (ALT_SCALE >> 1);
+	low = (alt_t) FETCH_ALT(o) * (ALT_SCALE - part);
+	high = (alt_t) FETCH_ALT(o+1) * part + (ALT_SCALE >> 1);
 	return (low + high) >> ALT_SHIFT;
 }
 
 #ifdef AO_CONVERT_TEST
-int32_t
-ao_altitude_to_pa(int32_t alt)
+pres_t
+ao_altitude_to_pa(alt_t alt)
 {
-	int32_t 	span, sub_span;
+	alt_t 	span, sub_span;
 	uint16_t	l, h, m;
 	int32_t 	pa;
 
@@ -76,7 +76,7 @@ ao_altitude_to_pa(int32_t alt)
 	}
 	span = altitude_table[l] - altitude_table[h];
 	sub_span = altitude_table[l] - alt;
-	pa = ((((int32_t) l * (span - sub_span) + (int32_t) h * sub_span) << ALT_SHIFT) + (span >> 1)) / span;
+	pa = ((((alt_t) l * (span - sub_span) + (alt_t) h * sub_span) << ALT_SHIFT) + (span >> 1)) / span;
 	if (pa > 120000)
 		pa = 120000;
 	if (pa < 0)
diff --git a/src/kernel/ao_convert_pa_test.c b/src/kernel/ao_convert_pa_test.c
index 7d5b1922..95422862 100644
--- a/src/kernel/ao_convert_pa_test.c
+++ b/src/kernel/ao_convert_pa_test.c
@@ -18,6 +18,7 @@
 #include <stdint.h>
 #define AO_CONVERT_TEST
 typedef int32_t alt_t;
+typedef int32_t pres_t;
 #include "ao_host.h"
 #include "ao_convert_pa.c"
 
diff --git a/src/kernel/ao_data.h b/src/kernel/ao_data.h
index c4b062fd..8f75ad87 100644
--- a/src/kernel/ao_data.h
+++ b/src/kernel/ao_data.h
@@ -117,9 +117,7 @@ extern volatile __data uint8_t		ao_data_count;
 
 typedef int32_t	pres_t;
 
-#ifndef AO_ALT_TYPE
 #define AO_ALT_TYPE	int32_t
-#endif
 
 typedef AO_ALT_TYPE	alt_t;
 
@@ -146,10 +144,6 @@ typedef int16_t alt_t;
 
 #endif
 
-#if !HAS_BARO
-typedef int16_t alt_t;
-#endif
-
 /*
  * Need a few macros to pull data from the sensors:
  *
diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c
index 2b433ee9..251dbc02 100644
--- a/src/kernel/ao_flight.c
+++ b/src/kernel/ao_flight.c
@@ -60,10 +60,10 @@ __xdata uint8_t			ao_sensor_errors;
  * resting
  */
 static __data uint16_t		ao_interval_end;
-static __data int16_t		ao_interval_min_height;
-static __data int16_t		ao_interval_max_height;
+static __data ao_v_t		ao_interval_min_height;
+static __data ao_v_t		ao_interval_max_height;
 #if HAS_ACCEL
-static __data int16_t		ao_coast_avg_accel;
+static __data ao_v_t		ao_coast_avg_accel;
 #endif
 
 __pdata uint8_t			ao_flight_force_idle;
@@ -398,14 +398,14 @@ ao_flight(void)
 }
 
 #if HAS_FLIGHT_DEBUG
-static inline int int_part(int16_t i)	{ return i >> 4; }
-static inline int frac_part(int16_t i)	{ return ((i & 0xf) * 100 + 8) / 16; }
+static inline int int_part(ao_v_t i)	{ return i >> 4; }
+static inline int frac_part(ao_v_t i)	{ return ((i & 0xf) * 100 + 8) / 16; }
 
 static void
 ao_flight_dump(void)
 {
 #if HAS_ACCEL
-	int16_t	accel;
+	ao_v_t	accel;
 
 	accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16;
 #endif
diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c
index 9aea1f14..7b0f8145 100644
--- a/src/kernel/ao_kalman.c
+++ b/src/kernel/ao_kalman.c
@@ -23,32 +23,31 @@
 #include "ao_sample.h"
 #include "ao_kalman.h"
 
+static __pdata ao_k_t		ao_k_height;
+static __pdata ao_k_t		ao_k_speed;
+static __pdata ao_k_t		ao_k_accel;
 
-static __pdata int32_t		ao_k_height;
-static __pdata int32_t		ao_k_speed;
-static __pdata int32_t		ao_k_accel;
+#define AO_K_STEP_100		to_fix_v(0.01)
+#define AO_K_STEP_2_2_100	to_fix_v(0.00005)
 
-#define AO_K_STEP_100		to_fix16(0.01)
-#define AO_K_STEP_2_2_100	to_fix16(0.00005)
+#define AO_K_STEP_10		to_fix_v(0.1)
+#define AO_K_STEP_2_2_10	to_fix_v(0.005)
 
-#define AO_K_STEP_10		to_fix16(0.1)
-#define AO_K_STEP_2_2_10	to_fix16(0.005)
+#define AO_K_STEP_1		to_fix_v(1)
+#define AO_K_STEP_2_2_1		to_fix_v(0.5)
 
-#define AO_K_STEP_1		to_fix16(1)
-#define AO_K_STEP_2_2_1		to_fix16(0.5)
+__pdata ao_v_t			ao_height;
+__pdata ao_v_t			ao_speed;
+__pdata ao_v_t			ao_accel;
+__xdata ao_v_t			ao_max_height;
+static __pdata ao_k_t		ao_avg_height_scaled;
+__xdata ao_v_t			ao_avg_height;
 
-__pdata int16_t			ao_height;
-__pdata int16_t			ao_speed;
-__pdata int16_t			ao_accel;
-__xdata int16_t			ao_max_height;
-static __pdata int32_t		ao_avg_height_scaled;
-__xdata int16_t			ao_avg_height;
-
-__pdata int16_t			ao_error_h;
-__pdata int16_t			ao_error_h_sq_avg;
+__pdata ao_v_t			ao_error_h;
+__pdata ao_v_t			ao_error_h_sq_avg;
 
 #if HAS_ACCEL
-__pdata int16_t			ao_error_a;
+__pdata ao_v_t			ao_error_a;
 #endif
 
 static void
@@ -56,40 +55,40 @@ ao_kalman_predict(void)
 {
 #ifdef AO_FLIGHT_TEST
 	if (ao_sample_tick - ao_sample_prev_tick > 50) {
-		ao_k_height += ((int32_t) ao_speed * AO_K_STEP_1 +
-				(int32_t) ao_accel * AO_K_STEP_2_2_1) >> 4;
-		ao_k_speed += (int32_t) ao_accel * AO_K_STEP_1;
+		ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_1 +
+				(ao_k_t) ao_accel * AO_K_STEP_2_2_1) >> 4;
+		ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_1;
 
 		return;
 	}
 	if (ao_sample_tick - ao_sample_prev_tick > 5) {
-		ao_k_height += ((int32_t) ao_speed * AO_K_STEP_10 +
-				(int32_t) ao_accel * AO_K_STEP_2_2_10) >> 4;
-		ao_k_speed += (int32_t) ao_accel * AO_K_STEP_10;
+		ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_10 +
+				(ao_k_t) ao_accel * AO_K_STEP_2_2_10) >> 4;
+		ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_10;
 
 		return;
 	}
 	if (ao_flight_debug) {
 		printf ("predict speed %g + (%g * %g) = %g\n",
 			ao_k_speed / (65536.0 * 16.0), ao_accel / 16.0, AO_K_STEP_100 / 65536.0,
-			(ao_k_speed + (int32_t) ao_accel * AO_K_STEP_100) / (65536.0 * 16.0));
+			(ao_k_speed + (ao_k_t) ao_accel * AO_K_STEP_100) / (65536.0 * 16.0));
 	}
 #endif
-	ao_k_height += ((int32_t) ao_speed * AO_K_STEP_100 +
-			(int32_t) ao_accel * AO_K_STEP_2_2_100) >> 4;
-	ao_k_speed += (int32_t) ao_accel * AO_K_STEP_100;
+	ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_100 +
+			(ao_k_t) ao_accel * AO_K_STEP_2_2_100) >> 4;
+	ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_100;
 }
 
 static void
 ao_kalman_err_height(void)
 {
-	int16_t	e;
-	int16_t height_distrust;
+	ao_v_t	e;
+	ao_v_t height_distrust;
 #if HAS_ACCEL
-	int16_t	speed_distrust;
+	ao_v_t	speed_distrust;
 #endif
 
-	ao_error_h = ao_sample_height - (int16_t) (ao_k_height >> 16);
+	ao_error_h = ao_sample_height - (ao_v_t) (ao_k_height >> 16);
 
 	e = ao_error_h;
 	if (e < 0)
@@ -123,7 +122,7 @@ ao_kalman_err_height(void)
 #endif
 		if (height_distrust > 0x100)
 			height_distrust = 0x100;
-		ao_error_h = (int16_t) (((int32_t) ao_error_h * (0x100 - height_distrust)) >> 8);
+		ao_error_h = (ao_v_t) (((ao_k_t) ao_error_h * (0x100 - height_distrust)) >> 8);
 #ifdef AO_FLIGHT_TEST
 		if (ao_flight_debug) {
 			printf("over height %g over speed %g distrust: %g height: error %d -> %d\n",
@@ -142,21 +141,21 @@ ao_kalman_correct_baro(void)
 	ao_kalman_err_height();
 #ifdef AO_FLIGHT_TEST
 	if (ao_sample_tick - ao_sample_prev_tick > 50) {
-		ao_k_height += (int32_t) AO_BARO_K0_1 * ao_error_h;
-		ao_k_speed  += (int32_t) AO_BARO_K1_1 * ao_error_h;
-		ao_k_accel  += (int32_t) AO_BARO_K2_1 * ao_error_h;
+		ao_k_height += (ao_k_t) AO_BARO_K0_1 * ao_error_h;
+		ao_k_speed  += (ao_k_t) AO_BARO_K1_1 * ao_error_h;
+		ao_k_accel  += (ao_k_t) AO_BARO_K2_1 * ao_error_h;
 		return;
 	}
 	if (ao_sample_tick - ao_sample_prev_tick > 5) {
-		ao_k_height += (int32_t) AO_BARO_K0_10 * ao_error_h;
-		ao_k_speed  += (int32_t) AO_BARO_K1_10 * ao_error_h;
-		ao_k_accel  += (int32_t) AO_BARO_K2_10 * ao_error_h;
+		ao_k_height += (ao_k_t) AO_BARO_K0_10 * ao_error_h;
+		ao_k_speed  += (ao_k_t) AO_BARO_K1_10 * ao_error_h;
+		ao_k_accel  += (ao_k_t) AO_BARO_K2_10 * ao_error_h;
 		return;
 	}
 #endif
-	ao_k_height += (int32_t) AO_BARO_K0_100 * ao_error_h;
-	ao_k_speed  += (int32_t) AO_BARO_K1_100 * ao_error_h;
-	ao_k_accel  += (int32_t) AO_BARO_K2_100 * ao_error_h;
+	ao_k_height += (ao_k_t) AO_BARO_K0_100 * ao_error_h;
+	ao_k_speed  += (ao_k_t) AO_BARO_K1_100 * ao_error_h;
+	ao_k_accel  += (ao_k_t) AO_BARO_K2_100 * ao_error_h;
 }
 
 #if HAS_ACCEL
@@ -164,7 +163,7 @@ ao_kalman_correct_baro(void)
 static void
 ao_kalman_err_accel(void)
 {
-	int32_t	accel;
+	ao_k_t	accel;
 
 	accel = (ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale;
 
@@ -187,18 +186,18 @@ ao_kalman_correct_both(void)
 				(double) ao_error_h, AO_BOTH_K10_1 / 65536.0,
 				(double) ao_error_a, AO_BOTH_K11_1 / 65536.0,
 				(ao_k_speed +
-				 (int32_t) AO_BOTH_K10_1 * ao_error_h +
-				 (int32_t) AO_BOTH_K11_1 * ao_error_a) / (65536.0 * 16.0));
+				 (ao_k_t) AO_BOTH_K10_1 * ao_error_h +
+				 (ao_k_t) AO_BOTH_K11_1 * ao_error_a) / (65536.0 * 16.0));
 		}
 		ao_k_height +=
-			(int32_t) AO_BOTH_K00_1 * ao_error_h +
-			(int32_t) AO_BOTH_K01_1 * ao_error_a;
+			(ao_k_t) AO_BOTH_K00_1 * ao_error_h +
+			(ao_k_t) AO_BOTH_K01_1 * ao_error_a;
 		ao_k_speed +=
-			(int32_t) AO_BOTH_K10_1 * ao_error_h +
-			(int32_t) AO_BOTH_K11_1 * ao_error_a;
+			(ao_k_t) AO_BOTH_K10_1 * ao_error_h +
+			(ao_k_t) AO_BOTH_K11_1 * ao_error_a;
 		ao_k_accel +=
-			(int32_t) AO_BOTH_K20_1 * ao_error_h +
-			(int32_t) AO_BOTH_K21_1 * ao_error_a;
+			(ao_k_t) AO_BOTH_K20_1 * ao_error_h +
+			(ao_k_t) AO_BOTH_K21_1 * ao_error_a;
 		return;
 	}
 	if (ao_sample_tick - ao_sample_prev_tick > 5) {
@@ -208,18 +207,18 @@ ao_kalman_correct_both(void)
 				(double) ao_error_h, AO_BOTH_K10_10 / 65536.0,
 				(double) ao_error_a, AO_BOTH_K11_10 / 65536.0,
 				(ao_k_speed +
-				 (int32_t) AO_BOTH_K10_10 * ao_error_h +
-				 (int32_t) AO_BOTH_K11_10 * ao_error_a) / (65536.0 * 16.0));
+				 (ao_k_t) AO_BOTH_K10_10 * ao_error_h +
+				 (ao_k_t) AO_BOTH_K11_10 * ao_error_a) / (65536.0 * 16.0));
 		}
 		ao_k_height +=
-			(int32_t) AO_BOTH_K00_10 * ao_error_h +
-			(int32_t) AO_BOTH_K01_10 * ao_error_a;
+			(ao_k_t) AO_BOTH_K00_10 * ao_error_h +
+			(ao_k_t) AO_BOTH_K01_10 * ao_error_a;
 		ao_k_speed +=
-			(int32_t) AO_BOTH_K10_10 * ao_error_h +
-			(int32_t) AO_BOTH_K11_10 * ao_error_a;
+			(ao_k_t) AO_BOTH_K10_10 * ao_error_h +
+			(ao_k_t) AO_BOTH_K11_10 * ao_error_a;
 		ao_k_accel +=
-			(int32_t) AO_BOTH_K20_10 * ao_error_h +
-			(int32_t) AO_BOTH_K21_10 * ao_error_a;
+			(ao_k_t) AO_BOTH_K20_10 * ao_error_h +
+			(ao_k_t) AO_BOTH_K21_10 * ao_error_a;
 		return;
 	}
 	if (ao_flight_debug) {
@@ -228,19 +227,19 @@ ao_kalman_correct_both(void)
 			(double) ao_error_h, AO_BOTH_K10_100 / 65536.0,
 			(double) ao_error_a, AO_BOTH_K11_100 / 65536.0,
 			(ao_k_speed +
-			 (int32_t) AO_BOTH_K10_100 * ao_error_h +
-			 (int32_t) AO_BOTH_K11_100 * ao_error_a) / (65536.0 * 16.0));
+			 (ao_k_t) AO_BOTH_K10_100 * ao_error_h +
+			 (ao_k_t) AO_BOTH_K11_100 * ao_error_a) / (65536.0 * 16.0));
 	}
 #endif
 	ao_k_height +=
-		(int32_t) AO_BOTH_K00_100 * ao_error_h +
-		(int32_t) AO_BOTH_K01_100 * ao_error_a;
+		(ao_k_t) AO_BOTH_K00_100 * ao_error_h +
+		(ao_k_t) AO_BOTH_K01_100 * ao_error_a;
 	ao_k_speed +=
-		(int32_t) AO_BOTH_K10_100 * ao_error_h +
-		(int32_t) AO_BOTH_K11_100 * ao_error_a;
+		(ao_k_t) AO_BOTH_K10_100 * ao_error_h +
+		(ao_k_t) AO_BOTH_K11_100 * ao_error_a;
 	ao_k_accel +=
-		(int32_t) AO_BOTH_K20_100 * ao_error_h +
-		(int32_t) AO_BOTH_K21_100 * ao_error_a;
+		(ao_k_t) AO_BOTH_K20_100 * ao_error_h +
+		(ao_k_t) AO_BOTH_K21_100 * ao_error_a;
 }
 
 #else
@@ -251,14 +250,14 @@ ao_kalman_correct_accel(void)
 	ao_kalman_err_accel();
 
 	if (ao_sample_tick - ao_sample_prev_tick > 5) {
-		ao_k_height +=(int32_t) AO_ACCEL_K0_10 * ao_error_a;
-		ao_k_speed  += (int32_t) AO_ACCEL_K1_10 * ao_error_a;
-		ao_k_accel  += (int32_t) AO_ACCEL_K2_10 * ao_error_a;
+		ao_k_height +=(ao_k_t) AO_ACCEL_K0_10 * ao_error_a;
+		ao_k_speed  += (ao_k_t) AO_ACCEL_K1_10 * ao_error_a;
+		ao_k_accel  += (ao_k_t) AO_ACCEL_K2_10 * ao_error_a;
 		return;
 	}
-	ao_k_height += (int32_t) AO_ACCEL_K0_100 * ao_error_a;
-	ao_k_speed  += (int32_t) AO_ACCEL_K1_100 * ao_error_a;
-	ao_k_accel  += (int32_t) AO_ACCEL_K2_100 * ao_error_a;
+	ao_k_height += (ao_k_t) AO_ACCEL_K0_100 * ao_error_a;
+	ao_k_speed  += (ao_k_t) AO_ACCEL_K1_100 * ao_error_a;
+	ao_k_accel  += (ao_k_t) AO_ACCEL_K2_100 * ao_error_a;
 }
 
 #endif /* else FORCE_ACCEL */
diff --git a/src/kernel/ao_microkalman.c b/src/kernel/ao_microkalman.c
index 0684ea2b..75a29cc4 100644
--- a/src/kernel/ao_microkalman.c
+++ b/src/kernel/ao_microkalman.c
@@ -22,19 +22,19 @@
 
 #define FIX_BITS	16
 
-#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5))
-#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5))
+#define to_fix_v(x) ((int16_t) ((x) * 65536.0 + 0.5))
+#define to_fix_k(x) ((int32_t) ((x) * 65536.0 + 0.5))
 #define from_fix8(x)	((x) >> 8)
 #define from_fix(x)	((x) >> 16)
-#define fix8_to_fix16(x)	((x) << 8)
+#define fix8_to_fix_v(x)	((x) << 8)
 #define fix16_to_fix8(x)	((x) >> 8)
 
 #include <ao_kalman.h>
 
 /* Basic time step (96ms) */
-#define AO_MK_STEP	to_fix16(0.096)
+#define AO_MK_STEP	to_fix_v(0.096)
 /* step ** 2 / 2 */
-#define AO_MK_STEP_2_2	to_fix16(0.004608)
+#define AO_MK_STEP_2_2	to_fix_v(0.004608)
 
 uint32_t	ao_k_pa;		/* 24.8 fixed point */
 int32_t		ao_k_pa_speed;		/* 16.16 fixed point */
@@ -49,7 +49,7 @@ ao_microkalman_init(void)
 {
 	ao_pa = pa;
 	ao_k_pa = pa << 8;
-}	
+}
 
 void
 ao_microkalman_predict(void)
diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c
index 34658951..29bf2bf6 100644
--- a/src/kernel/ao_sample.c
+++ b/src/kernel/ao_sample.c
@@ -245,7 +245,7 @@ ao_sample_preflight(void)
 	} else {
 #if HAS_ACCEL
 		ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g;
-		ao_accel_scale = to_fix32(GRAVITY * 2 * 16) / ao_accel_2g;
+		ao_accel_scale = to_fix_32(GRAVITY * 2 * 16) / ao_accel_2g;
 #endif
 		ao_sample_preflight_set();
 		ao_preflight = FALSE;
diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h
index 16d4c507..2ec998bd 100644
--- a/src/kernel/ao_sample.h
+++ b/src/kernel/ao_sample.h
@@ -24,6 +24,24 @@
  * ao_sample.c
  */
 
+#ifndef AO_VALUE_32
+#define AO_VALUE_32	1
+#endif
+
+#if AO_VALUE_32
+/*
+ * For 32-bit computed values, use 64-bit intermediates.
+ */
+typedef int64_t			ao_k_t;
+typedef int32_t			ao_v_t;
+#else
+/*
+ * For 16-bit computed values, use 32-bit intermediates.
+ */
+typedef int32_t			ao_k_t;
+typedef int16_t			ao_v_t;
+#endif
+
 /*
  * Barometer calibration
  *
@@ -87,9 +105,9 @@
  * 2047m/s² (over 200g)
  */
 
-#define AO_M_TO_HEIGHT(m)	((int16_t) (m))
-#define AO_MS_TO_SPEED(ms)	((int16_t) ((ms) * 16))
-#define AO_MSS_TO_ACCEL(mss)	((int16_t) ((mss) * 16))
+#define AO_M_TO_HEIGHT(m)	((ao_v_t) (m))
+#define AO_MS_TO_SPEED(ms)	((ao_v_t) ((ms) * 16))
+#define AO_MSS_TO_ACCEL(mss)	((ao_v_t) ((mss) * 16))
 
 extern __pdata uint16_t	ao_sample_tick;		/* time of last data */
 extern __data uint8_t	ao_sample_adc;		/* Ring position of last processed sample */
@@ -134,21 +152,33 @@ uint8_t ao_sample(void);
  * ao_kalman.c
  */
 
-#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5))
-#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5))
+#define to_fix_16(x) ((int16_t) ((x) * 65536.0 + 0.5))
+#define to_fix_32(x) ((int32_t) ((x) * 65536.0 + 0.5))
+#define to_fix_64(x) ((int64_t) ((x) * 65536.0 + 0.5))
+
+#ifdef AO_VALUE_32
+#if AO_VALUE_32
+#define to_fix_v(x)	to_fix_32(x)
+#define to_fix_k(x)	to_fix_64(x)
+#else
+#define to_fix_v(x)	to_fix_16(x)
+#define to_fix_k(x)	to_fix_32(x)
+#endif
+
 #define from_fix(x)	((x) >> 16)
 
-extern __pdata int16_t			ao_height;	/* meters */
-extern __pdata int16_t			ao_speed;	/* m/s * 16 */
-extern __pdata int16_t			ao_accel;	/* m/s² * 16 */
-extern __xdata int16_t			ao_max_height;	/* max of ao_height */
-extern __xdata int16_t			ao_avg_height;	/* running average of height */
+extern __pdata ao_v_t			ao_height;	/* meters */
+extern __pdata ao_v_t			ao_speed;	/* m/s * 16 */
+extern __pdata ao_v_t			ao_accel;	/* m/s² * 16 */
+extern __xdata ao_v_t			ao_max_height;	/* max of ao_height */
+extern __xdata ao_v_t			ao_avg_height;	/* running average of height */
 
-extern __pdata int16_t			ao_error_h;
-extern __pdata int16_t			ao_error_h_sq_avg;
+extern __pdata ao_v_t			ao_error_h;
+extern __pdata ao_v_t			ao_error_h_sq_avg;
 
 #if HAS_ACCEL
-extern __pdata int16_t			ao_error_a;
+extern __pdata ao_v_t			ao_error_a;
+#endif
 #endif
 
 void ao_kalman(void);
diff --git a/src/product/Makefile.telemetrum b/src/product/Makefile.telemetrum
index dbbf57d8..e9b144c0 100644
--- a/src/product/Makefile.telemetrum
+++ b/src/product/Makefile.telemetrum
@@ -24,6 +24,7 @@ INC = \
 	altitude.h \
 	ao_kalman.h \
 	ao_product.h \
+	ao_telemetry.h \
 	$(TM_INC)
 
 CORE_SRC = \
diff --git a/src/telefire-v0.1/Makefile b/src/telefire-v0.1/Makefile
index 25267268..99d29826 100644
--- a/src/telefire-v0.1/Makefile
+++ b/src/telefire-v0.1/Makefile
@@ -25,7 +25,6 @@ INC = \
 CORE_SRC = \
 	ao_cmd.c \
 	ao_config.c \
-	ao_convert.c \
 	ao_mutex.c \
 	ao_panic.c \
 	ao_stdio.c \
diff --git a/src/telefire-v0.2/Makefile b/src/telefire-v0.2/Makefile
index ad5065c1..944543c5 100644
--- a/src/telefire-v0.2/Makefile
+++ b/src/telefire-v0.2/Makefile
@@ -25,7 +25,6 @@ INC = \
 CORE_SRC = \
 	ao_cmd.c \
 	ao_config.c \
-	ao_convert.c \
 	ao_mutex.c \
 	ao_panic.c \
 	ao_stdio.c \
diff --git a/src/telemega-v1.0/Makefile b/src/telemega-v1.0/Makefile
index 46c768e4..4a1b3908 100644
--- a/src/telemega-v1.0/Makefile
+++ b/src/telemega-v1.0/Makefile
@@ -31,6 +31,7 @@ INC = \
 	ao_mpu.h \
 	stm32l.h \
 	math.h \
+	ao_ms5607_convert.c \
 	Makefile
 
 #
diff --git a/src/telemini-v2.0/ao_pins.h b/src/telemini-v2.0/ao_pins.h
index 948310e5..ed911798 100644
--- a/src/telemini-v2.0/ao_pins.h
+++ b/src/telemini-v2.0/ao_pins.h
@@ -22,6 +22,7 @@
 
 #define HAS_FLIGHT		1
 #define HAS_USB			1
+#define AO_VALUE_32		0
 
 #define HAS_USB_PULLUP	1
 #define AO_USB_PULLUP_PORT	P1
diff --git a/src/teleterra-v0.2/ao_pins.h b/src/teleterra-v0.2/ao_pins.h
index 60d627ad..472af534 100644
--- a/src/teleterra-v0.2/ao_pins.h
+++ b/src/teleterra-v0.2/ao_pins.h
@@ -72,6 +72,8 @@
 
 	#define BATTERY_PIN		5
 	#define HAS_TELEMETRY		0
+
+	#define AO_VALUE_32		0
 #endif
 
 #if DBG_ON_P1
diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c
index 314998c1..bb5c3a7d 100644
--- a/src/test/ao_flight_test.c
+++ b/src/test/ao_flight_test.c
@@ -34,9 +34,11 @@
 #define ao_data_ring_next(n)	(((n) + 1) & (AO_DATA_RING - 1))
 #define ao_data_ring_prev(n)	(((n) - 1) & (AO_DATA_RING - 1))
 
+#if 0
 #define AO_M_TO_HEIGHT(m)	((int16_t) (m))
 #define AO_MS_TO_SPEED(ms)	((int16_t) ((ms) * 16))
 #define AO_MSS_TO_ACCEL(mss)	((int16_t) ((mss) * 16))
+#endif
 
 #define AO_GPS_NEW_DATA		1
 #define AO_GPS_NEW_TRACKING	2
@@ -93,6 +95,7 @@ struct ao_adc {
 #include <ao_data.h>
 #include <ao_log.h>
 #include <ao_telemetry.h>
+#include <ao_sample.h>
 
 #if TELEMEGA
 int ao_gps_count;
@@ -234,7 +237,7 @@ double	main_time;
 
 int	tick_offset;
 
-static int32_t	ao_k_height;
+static ao_k_t	ao_k_height;
 
 int16_t
 ao_time(void)
diff --git a/src/test/ao_micropeak_test.c b/src/test/ao_micropeak_test.c
index 5961bd93..f4af707e 100644
--- a/src/test/ao_micropeak_test.c
+++ b/src/test/ao_micropeak_test.c
@@ -33,6 +33,7 @@ uint8_t ao_flight_debug;
 #define AO_FLIGHT_TEST
 
 typedef int32_t alt_t;
+typedef int32_t pres_t;
 
 #define AO_MS_TO_TICKS(ms)	((ms) / 10)
 
-- 
cgit v1.2.3