3 files changed, 28 insertions, 4 deletions

diff --git a/src/core/ao.h b/src/core/ao.h
index 8b978272..f2e40fc6 100644
--- a/src/core/ao.h
+++ b/src/core/ao.h
@@ -1360,6 +1360,7 @@ extern __xdata union ao_monitor ao_monitor_ring[AO_MONITOR_RING];
 
 #define ao_monitor_ring_next(n)	(((n) + 1) & (AO_MONITOR_RING - 1))
 
+extern __data uint8_t ao_monitoring;
 extern __data uint8_t ao_monitor_head;
 
 void
diff --git a/src/core/ao_log_telem.c b/src/core/ao_log_telem.c
index af0e8e6b..9afa13b2 100644
--- a/src/core/ao_log_telem.c
+++ b/src/core/ao_log_telem.c
@@ -19,8 +19,29 @@
 
 __code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMETRY;
 
-static __data uint8_t	ao_log_monitor_pos;
+static __data uint8_t			ao_log_monitor_pos;
+__pdata enum ao_flight_state		ao_flight_state;
+__pdata int16_t				ao_max_height;	/* max of ao_height */
 
+static void
+ao_log_telem_track() {
+	if (ao_monitoring == sizeof (union ao_telemetry_all)) {
+		switch (ao_log_single_write_data.telemetry.generic.type) {
+		case AO_TELEMETRY_SENSOR_TELEMETRUM:
+		case AO_TELEMETRY_SENSOR_TELEMINI:
+		case AO_TELEMETRY_SENSOR_TELENANO:
+			if (ao_log_single_write_data.telemetry.sensor.height > ao_max_height) {
+				ao_max_height = ao_log_single_write_data.telemetry.sensor.height;
+			}
+			if (ao_log_single_write_data.telemetry.sensor.state != ao_flight_state) {
+				ao_flight_state = ao_log_single_write_data.telemetry.sensor.state;
+				if (ao_flight_state == ao_flight_pad)
+					ao_max_height = 0;
+				ao_wakeup(DATA_TO_XDATA(&ao_flight_state));
+			}
+		}
+	}
+}
 void
 ao_log_single(void)
 {
@@ -33,6 +54,7 @@ ao_log_single(void)
 
 	ao_log_running = 1;
 	ao_log_single_restart();
+	ao_flight_state = ao_flight_startup;
 	for (;;) {
 		while (!ao_log_running)
 			ao_sleep(&ao_log_running);
@@ -46,6 +68,7 @@ ao_log_single(void)
 				       AO_LOG_SINGLE_SIZE);
 				ao_log_single_write();
 				ao_log_monitor_pos = ao_monitor_ring_next(ao_log_monitor_pos);
+				ao_log_telem_track();
 			}
 			/* Wait for more telemetry data to arrive */
 			ao_sleep(DATA_TO_XDATA(&ao_monitor_head));
diff --git a/src/core/ao_monitor.c b/src/core/ao_monitor.c
index 382af8a2..56d7604d 100644
--- a/src/core/ao_monitor.c
+++ b/src/core/ao_monitor.c
@@ -22,7 +22,7 @@
 #error Must define HAS_MONITOR to 1
 #endif
 
-__xdata uint8_t ao_monitoring;
+__data uint8_t ao_monitoring;
 __pdata uint8_t ao_monitor_led;
 
 __xdata union ao_monitor ao_monitor_ring[AO_MONITOR_RING];
@@ -37,7 +37,7 @@ ao_monitor_get(void)
 	for (;;) {
 		switch (ao_monitoring) {
 		case 0:
-			ao_sleep(&ao_monitoring);
+			ao_sleep(DATA_TO_XDATA(&ao_monitoring));
 			continue;
 		case AO_MONITORING_ORIG:
 			size = sizeof (struct ao_telemetry_orig_recv);
@@ -262,7 +262,7 @@ ao_set_monitor(uint8_t monitoring)
 	if (ao_monitoring)
 		ao_radio_recv_abort();
 	ao_monitoring = monitoring;
-	ao_wakeup(&ao_monitoring);
+	ao_wakeup(DATA_TO_XDATA(&ao_monitoring));
 }
 
 static void