X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fkernel%2Fao_monitor.c;h=cd5116990b7046e83789dfbbe2431e620be74170;hp=18f170b49912af18c707e2e8780a03db522f2419;hb=99525a748e00406424b98a0952f0156437b30b6c;hpb=24167015705ae831692b95735968b04a876f935e

diff --git a/src/kernel/ao_monitor.c b/src/kernel/ao_monitor.c
index 18f170b4..cd511699 100644
--- a/src/kernel/ao_monitor.c
+++ b/src/kernel/ao_monitor.c
@@ -3,7 +3,8 @@
  *
  * 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.
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
  *
  * This program is distributed in the hope that it will be useful, but
  * WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -35,14 +36,15 @@
 #error Must define AO_MONITOR_LED
 #endif
 
-__data uint8_t ao_monitoring;
-static __data uint8_t ao_monitor_disabled;
-static __data uint8_t ao_internal_monitoring;
-static __data uint8_t ao_external_monitoring;
+uint8_t ao_monitoring_mutex;
+uint8_t ao_monitoring;
+static uint8_t ao_monitor_disabled;
+static uint8_t ao_internal_monitoring;
+static uint8_t ao_external_monitoring;
 
-__xdata union ao_monitor ao_monitor_ring[AO_MONITOR_RING];
+union ao_monitor ao_monitor_ring[AO_MONITOR_RING];
 
-__data uint8_t	ao_monitor_head;
+uint8_t	ao_monitor_head;
 
 static void
 _ao_monitor_adjust(void)
@@ -57,10 +59,10 @@ _ao_monitor_adjust(void)
 		else
 			ao_monitoring = ao_internal_monitoring;
 	}
-	ao_wakeup(DATA_TO_XDATA(&ao_monitoring));
+	ao_wakeup(&ao_monitoring);
 }
 
-void
+static void
 ao_monitor_get(void)
 {
 	uint8_t	size;
@@ -68,7 +70,7 @@ ao_monitor_get(void)
 	for (;;) {
 		switch (ao_monitoring) {
 		case 0:
-			ao_sleep(DATA_TO_XDATA(&ao_monitoring));
+			ao_sleep(&ao_monitoring);
 			continue;
 #if LEGACY_MONITOR
 		case AO_MONITORING_ORIG:
@@ -84,19 +86,28 @@ ao_monitor_get(void)
 		if (!ao_radio_recv(&ao_monitor_ring[ao_monitor_head], size + 2, 0))
 			continue;
 		ao_monitor_head = ao_monitor_ring_next(ao_monitor_head);
-		ao_wakeup(DATA_TO_XDATA(&ao_monitor_head));
+		ao_wakeup(&ao_monitor_head);
 	}
 }
 
 #if AO_MONITOR_LED
-__xdata struct ao_task ao_monitor_blink_task;
+struct ao_task ao_monitor_blink_task;
 
-void
+static void
 ao_monitor_blink(void)
 {
+#ifdef AO_MONITOR_BAD
+	uint8_t		*recv;
+#endif
 	for (;;) {
-		ao_sleep(DATA_TO_XDATA(&ao_monitor_head));
-		ao_led_for(AO_MONITOR_LED, AO_MS_TO_TICKS(100));
+		ao_sleep(&ao_monitor_head);
+#ifdef AO_MONITOR_BAD
+		recv = (uint8_t *) &ao_monitor_ring[ao_monitor_ring_prev(ao_monitor_head)];
+		if (ao_monitoring && !(recv[ao_monitoring + 1] & AO_RADIO_STATUS_CRC_OK))
+			ao_led_for(AO_MONITOR_BAD, AO_MS_TO_TICKS(100));
+		else
+#endif
+			ao_led_for(AO_MONITOR_LED, AO_MS_TO_TICKS(100));
 	}
 }
 #endif
@@ -110,17 +121,19 @@ static const char xdigit[16] = {
 
 #define hex(c) do { putchar(xdigit[(c) >> 4]); putchar(xdigit[(c)&0xf]); } while (0)
 
-void
+static void
 ao_monitor_put(void)
 {
 #if LEGACY_MONITOR
-	__xdata char callsign[AO_MAX_CALLSIGN+1];
+	char callsign[AO_MAX_CALLSIGN+1];
+#endif
+#if LEGACY_MONITOR || HAS_RSSI
 	int16_t rssi;
 #endif
 	uint8_t ao_monitor_tail;
 	uint8_t state;
 	uint8_t sum, byte;
-	__xdata union ao_monitor	*m;
+	union ao_monitor	*m;
 
 #define recv_raw	((m->raw))
 #define recv_orig	((m->orig))
@@ -129,9 +142,9 @@ ao_monitor_put(void)
 	ao_monitor_tail = ao_monitor_head;
 	for (;;) {
 		while (!ao_external_monitoring)
-			ao_sleep(DATA_TO_XDATA(&ao_external_monitoring));
+			ao_sleep(&ao_external_monitoring);
 		while (ao_monitor_tail == ao_monitor_head && ao_external_monitoring)
-			ao_sleep(DATA_TO_XDATA(&ao_monitor_head));
+			ao_sleep(&ao_monitor_head);
 		if (!ao_external_monitoring)
 			continue;
 		m = &ao_monitor_ring[ao_monitor_tail];
@@ -144,7 +157,7 @@ ao_monitor_put(void)
 			state = recv_orig.telemetry_orig.flight_state;
 
 			rssi = (int16_t) AO_RSSI_FROM_RADIO(recv_orig.rssi);
-			ao_xmemcpy(callsign, recv_orig.telemetry_orig.callsign, AO_MAX_CALLSIGN);
+			memcpy(callsign, recv_orig.telemetry_orig.callsign, AO_MAX_CALLSIGN);
 			if (state > ao_flight_invalid)
 				state = ao_flight_invalid;
 			if (recv_orig.status & PKT_APPEND_STATUS_1_CRC_OK) {
@@ -229,6 +242,7 @@ ao_monitor_put(void)
 			printf ("rx cleanup: %d\n", ao_rx_done_tick - ao_fec_decode_end);
 		}
 #endif
+			ao_mutex_get(&ao_monitoring_mutex);
 			printf("TELEM ");
 			hex((uint8_t) (ao_monitoring + 2));
 			sum = 0x5a;
@@ -239,8 +253,9 @@ ao_monitor_put(void)
 			}
 			hex(sum);
 			putchar ('\n');
+			ao_mutex_put(&ao_monitoring_mutex);
 #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);
 			}
@@ -251,10 +266,10 @@ ao_monitor_put(void)
 	}
 }
 
-__xdata struct ao_task ao_monitor_put_task;
+struct ao_task ao_monitor_put_task;
 #endif
 
-__xdata struct ao_task ao_monitor_get_task;
+struct ao_task ao_monitor_get_task;
 
 void
 ao_monitor_set(uint8_t monitoring)
@@ -281,21 +296,20 @@ ao_monitor_enable(void)
 static void
 set_monitor(void)
 {
-	ao_cmd_hex();
-	ao_external_monitoring = ao_cmd_lex_i;
-	ao_wakeup(DATA_TO_XDATA(&ao_external_monitoring));
-	ao_wakeup(DATA_TO_XDATA(&ao_monitor_head));
+	ao_external_monitoring = ao_cmd_hex();
+	ao_wakeup(&ao_external_monitoring);
+	ao_wakeup(&ao_monitor_head);
 	_ao_monitor_adjust();
 }
 
-__code struct ao_cmds ao_monitor_cmds[] = {
+const struct ao_cmds ao_monitor_cmds[] = {
 	{ set_monitor,	"m <0 off, 1 old, 20 std>\0Set radio monitoring" },
 	{ 0,	NULL },
 };
 #endif
 
 void
-ao_monitor_init(void) __reentrant
+ao_monitor_init(void) 
 {
 #if HAS_MONITOR_PUT
 	ao_cmd_register(&ao_monitor_cmds[0]);