X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fkernel%2Fao_monitor.c;h=7cbee28815b611775beb4ec1e1785f7040995a46;hb=40236913922e0395780cd7d90354546ecaf279f9;hp=2d75c41c2c786bf98d7c03c05e4a09f9a58b70a0;hpb=657d455a2fad36193e6b3a7037d9ba7f09ae1168;p=fw%2Faltos

diff --git a/src/kernel/ao_monitor.c b/src/kernel/ao_monitor.c
index 2d75c41c..7cbee288 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,6 +36,7 @@
 #error Must define AO_MONITOR_LED
 #endif
 
+__xdata uint8_t ao_monitoring_mutex;
 __data uint8_t ao_monitoring;
 static __data uint8_t ao_monitor_disabled;
 static __data uint8_t ao_internal_monitoring;
@@ -94,9 +96,18 @@ __xdata struct ao_task ao_monitor_blink_task;
 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));
+#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
@@ -231,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;
@@ -241,6 +253,7 @@ ao_monitor_put(void)
 			}
 			hex(sum);
 			putchar ('\n');
+			ao_mutex_put(&ao_monitoring_mutex);
 #if HAS_RSSI
 			if (recv_raw.packet[ao_monitoring + 1] & AO_RADIO_STATUS_CRC_OK) {
 				rssi = AO_RSSI_FROM_RADIO(recv_raw.packet[ao_monitoring]);