X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fao_monitor.c;h=8f1b9e12cdcc209dae9543c0a41885a68ef3bf4b;hp=d6fd8305ea4634338e16142301b0771a58cfb40e;hb=82e04a0e3a3296288a524ec582785a36fd644331;hpb=be838db49d999426a9dd02c0166fe161722f1e61 diff --git a/src/ao_monitor.c b/src/ao_monitor.c index d6fd8305..8f1b9e12 100644 --- a/src/ao_monitor.c +++ b/src/ao_monitor.c @@ -21,36 +21,79 @@ __xdata uint8_t ao_monitoring; __pdata uint8_t ao_monitor_led; +#define AO_MONITOR_RING 8 + +__xdata union ao_monitor { + struct ao_telemetry_raw_recv raw; + struct ao_telemetry_orig_recv orig; + struct ao_telemetry_tiny_recv tiny; +} ao_monitor_ring[AO_MONITOR_RING]; + +#define ao_monitor_ring_next(n) (((n) + 1) & (AO_MONITOR_RING - 1)) + +__data uint8_t ao_monitor_head; + void -ao_monitor(void) +ao_monitor_get(void) { - __xdata char callsign[AO_MAX_CALLSIGN+1]; - __xdata union { - struct ao_telemetry_recv full; - struct ao_telemetry_tiny_recv tiny; - } u; + uint8_t size; -#define recv (u.full) -#define recv_tiny (u.tiny) + for (;;) { + switch (ao_monitoring) { + case 0: + ao_sleep(&ao_monitoring); + continue; + case AO_MONITORING_ORIG: + size = sizeof (struct ao_telemetry_orig_recv); + break; + case AO_MONITORING_TINY: + size = sizeof (struct ao_telemetry_tiny_recv); + break; + default: + if (ao_monitoring > AO_MAX_TELEMETRY) + ao_monitoring = AO_MAX_TELEMETRY; + size = ao_monitoring; + break; + } + if (!ao_radio_recv(&ao_monitor_ring[ao_monitor_head], size + 2)) + continue; + ao_monitor_head = ao_monitor_ring_next(ao_monitor_head); + ao_wakeup(DATA_TO_XDATA(&ao_monitor_head)); + ao_led_toggle(ao_monitor_led); + } +} +void +ao_monitor_put(void) +{ + __xdata char callsign[AO_MAX_CALLSIGN+1]; + + uint8_t ao_monitor_tail; uint8_t state; + uint8_t sum, byte; int16_t rssi; + __xdata union ao_monitor *m; - for (;;) { - __critical while (!ao_monitoring) - ao_sleep(&ao_monitoring); - if (ao_monitoring == AO_MONITORING_FULL) { - if (!ao_radio_recv(&recv, sizeof (struct ao_telemetry_recv))) - continue; +#define recv_raw ((m->raw)) +#define recv_orig ((m->orig)) +#define recv_tiny ((m->tiny)) - state = recv.telemetry.flight_state; + ao_monitor_tail = ao_monitor_head; + for (;;) { + while (ao_monitor_tail == ao_monitor_head) + ao_sleep(DATA_TO_XDATA(&ao_monitor_head)); + m = &ao_monitor_ring[ao_monitor_tail]; + ao_monitor_tail = ao_monitor_ring_next(ao_monitor_tail); + switch (ao_monitoring) { + case AO_MONITORING_ORIG: + state = recv_orig.telemetry_orig.flight_state; /* Typical RSSI offset for 38.4kBaud at 433 MHz is 74 */ - rssi = (int16_t) (recv.rssi >> 1) - 74; - memcpy(callsign, recv.telemetry.callsign, AO_MAX_CALLSIGN); + rssi = (int16_t) (recv_orig.rssi >> 1) - 74; + memcpy(callsign, recv_orig.telemetry_orig.callsign, AO_MAX_CALLSIGN); if (state > ao_flight_invalid) state = ao_flight_invalid; - if (recv.status & PKT_APPEND_STATUS_1_CRC_OK) { + if (recv_orig.status & PKT_APPEND_STATUS_1_CRC_OK) { /* General header fields */ printf(AO_TELEM_VERSION " %d " @@ -62,11 +105,11 @@ ao_monitor(void) AO_TELEM_TICK " %d ", AO_TELEMETRY_VERSION, callsign, - recv.telemetry.serial, - recv.telemetry.flight, + recv_orig.telemetry_orig.serial, + recv_orig.telemetry_orig.flight, rssi, ao_state_names[state], - recv.telemetry.adc.tick); + recv_orig.telemetry_orig.adc.tick); /* Raw sensor values */ printf(AO_TELEM_RAW_ACCEL " %d " @@ -75,51 +118,49 @@ ao_monitor(void) AO_TELEM_RAW_BATT " %d " AO_TELEM_RAW_DROGUE " %d " AO_TELEM_RAW_MAIN " %d ", - recv.telemetry.adc.accel, - recv.telemetry.adc.pres, - recv.telemetry.adc.temp, - recv.telemetry.adc.v_batt, - recv.telemetry.adc.sense_d, - recv.telemetry.adc.sense_m); + recv_orig.telemetry_orig.adc.accel, + recv_orig.telemetry_orig.adc.pres, + recv_orig.telemetry_orig.adc.temp, + recv_orig.telemetry_orig.adc.v_batt, + recv_orig.telemetry_orig.adc.sense_d, + recv_orig.telemetry_orig.adc.sense_m); /* Sensor calibration values */ printf(AO_TELEM_CAL_ACCEL_GROUND " %d " AO_TELEM_CAL_BARO_GROUND " %d " AO_TELEM_CAL_ACCEL_PLUS " %d " AO_TELEM_CAL_ACCEL_MINUS " %d ", - recv.telemetry.ground_accel, - recv.telemetry.ground_pres, - recv.telemetry.accel_plus_g, - recv.telemetry.accel_minus_g); + recv_orig.telemetry_orig.ground_accel, + recv_orig.telemetry_orig.ground_pres, + recv_orig.telemetry_orig.accel_plus_g, + recv_orig.telemetry_orig.accel_minus_g); -#if 0 - /* Kalman state values */ - printf(AO_TELEM_KALMAN_HEIGHT " %d " - AO_TELEM_KALMAN_SPEED " %d " - AO_TELEM_KALMAN_ACCEL " %d ", - recv.telemetry.height, - recv.telemetry.speed, - recv.telemetry.accel); -#else - /* Ad-hoc flight values */ - printf(AO_TELEM_ADHOC_ACCEL " %d " - AO_TELEM_ADHOC_SPEED " %ld " - AO_TELEM_ADHOC_BARO " %d ", - recv.telemetry.flight_accel, - recv.telemetry.flight_vel, - recv.telemetry.flight_pres); -#endif - ao_gps_print(&recv.telemetry.gps); - ao_gps_tracking_print(&recv.telemetry.gps_tracking); + if (recv_orig.telemetry_orig.u.k.unused == 0x8000) { + /* Kalman state values */ + printf(AO_TELEM_KALMAN_HEIGHT " %d " + AO_TELEM_KALMAN_SPEED " %d " + AO_TELEM_KALMAN_ACCEL " %d ", + recv_orig.telemetry_orig.height, + recv_orig.telemetry_orig.u.k.speed, + recv_orig.telemetry_orig.accel); + } else { + /* Ad-hoc flight values */ + printf(AO_TELEM_ADHOC_ACCEL " %d " + AO_TELEM_ADHOC_SPEED " %ld " + AO_TELEM_ADHOC_BARO " %d ", + recv_orig.telemetry_orig.accel, + recv_orig.telemetry_orig.u.flight_vel, + recv_orig.telemetry_orig.height); + } + ao_gps_print(&recv_orig.telemetry_orig.gps); + ao_gps_tracking_print(&recv_orig.telemetry_orig.gps_tracking); putchar('\n'); ao_rssi_set(rssi); } else { printf("CRC INVALID RSSI %3d\n", rssi); } - } else { - if (!ao_radio_recv(&recv_tiny, sizeof (struct ao_telemetry_tiny_recv))) - continue; - + break; + case AO_MONITORING_TINY: state = recv_tiny.telemetry_tiny.flight_state; /* Typical RSSI offset for 38.4kBaud at 433 MHz is 74 */ @@ -181,13 +222,24 @@ ao_monitor(void) } else { printf("CRC INVALID RSSI %3d\n", rssi); } + break; + default: + printf ("TELEM %02x", ao_monitoring + 2); + sum = 0x5a; + for (state = 0; state < ao_monitoring + 2; state++) { + byte = recv_raw.packet[state]; + sum += byte; + printf("%02x", byte); + } + printf("%02x\n", sum); + break; } ao_usb_flush(); - ao_led_toggle(ao_monitor_led); } } -__xdata struct ao_task ao_monitor_task; +__xdata struct ao_task ao_monitor_get_task; +__xdata struct ao_task ao_monitor_put_task; void ao_set_monitor(uint8_t monitoring) @@ -216,5 +268,6 @@ ao_monitor_init(uint8_t monitor_led, uint8_t monitoring) __reentrant ao_monitor_led = monitor_led; ao_monitoring = monitoring; ao_cmd_register(&ao_monitor_cmds[0]); - ao_add_task(&ao_monitor_task, ao_monitor, "monitor"); + ao_add_task(&ao_monitor_get_task, ao_monitor_get, "monitor_get"); + ao_add_task(&ao_monitor_put_task, ao_monitor_put, "monitor_put"); }