altos: Remove *_TO_DATA macros

[fw/altos] / src / kernel / ao_monitor.c

diff --git a/src/kernel/ao_monitor.c b/src/kernel/ao_monitor.c
index 18f170b49912af18c707e2e8780a03db522f2419..b9a39bfc82e38d719d4c3de875e213c2215d4cc9 100644 (file)
--- 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;
@@ -57,7 +59,7 @@ _ao_monitor_adjust(void)
                else
                        ao_monitoring = ao_internal_monitoring;
        }
-       ao_wakeup(DATA_TO_XDATA(&ao_monitoring));
+       ao_wakeup(&ao_monitoring);
 }
 
 void
@@ -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,7 +86,7 @@ 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);
        }
 }
 
@@ -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));
+               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
@@ -115,6 +126,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;
@@ -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];
@@ -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);
                        }
@@ -283,8 +298,8 @@ 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_wakeup(&ao_external_monitoring);
+       ao_wakeup(&ao_monitor_head);
        _ao_monitor_adjust();
 }