From 7b14c3e609749f4fc00dbd660541375048535218 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 30 Oct 2009 16:39:15 -0700 Subject: [PATCH] Initial packet bits. Just testing transmission --- src/Makefile | 1 + src/ao.h | 65 +++++---- src/ao_dma.c | 6 +- src/ao_gps_skytraq.c | 6 + src/ao_monitor.c | 4 +- src/ao_packet.c | 308 +++++++++++++++++++++++++++++++++++++++++++ src/ao_radio.c | 70 +++++++++- src/ao_teledongle.c | 1 + src/ao_telemetrum.c | 1 + src/skytraq-cksum | 2 +- 10 files changed, 427 insertions(+), 37 deletions(-) create mode 100644 src/ao_packet.c diff --git a/src/Makefile b/src/Makefile index 9891cdad..4575f443 100644 --- a/src/Makefile +++ b/src/Makefile @@ -44,6 +44,7 @@ ALTOS_DRIVER_SRC = \ TELE_COMMON_SRC = \ ao_gps_print.c \ + ao_packet.c \ ao_state.c # diff --git a/src/ao.h b/src/ao.h index 13271109..e86b4bf9 100644 --- a/src/ao.h +++ b/src/ao.h @@ -138,7 +138,7 @@ ao_clock_init(void); * ao_adc.c */ -#define AO_ADC_RING 64 +#define AO_ADC_RING 32 #define ao_adc_ring_next(n) (((n) + 1) & (AO_ADC_RING - 1)) #define ao_adc_ring_prev(n) (((n) - 1) & (AO_ADC_RING - 1)) @@ -349,9 +349,15 @@ ao_cmd_init(void); * ao_dma.c */ -/* Allocate a DMA channel. the 'done' parameter will be set to 1 - * when the dma is finished and will be used to wakeup any waiters +/* Allocate a DMA channel. the 'done' parameter will be set + * when the dma is finished or aborted and will be used to + * wakeup any waiters */ + +#define AO_DMA_DONE 1 +#define AO_DMA_ABORTED 2 +#define AO_DMA_TIMEOUT 4 + uint8_t ao_dma_alloc(__xdata uint8_t * done); @@ -374,7 +380,7 @@ ao_dma_trigger(uint8_t id); /* Abort a running DMA transfer */ void -ao_dma_abort(uint8_t id); +ao_dma_abort(uint8_t id, uint8_t reason); /* DMA interrupt routine */ void @@ -796,6 +802,19 @@ ao_telemetry_init(void); * ao_radio.c */ +extern __xdata uint8_t ao_radio_dma; +extern __xdata uint8_t ao_radio_dma_done; +extern __xdata uint8_t ao_radio_mutex; + +void +ao_radio_set_telemetry(void); + +void +ao_radio_set_packet(void); + +void +ao_radio_set_rdf(void); + void ao_radio_send(__xdata struct ao_telemetry *telemetry) __reentrant; @@ -805,15 +824,21 @@ struct ao_radio_recv { uint8_t status; }; -void +uint8_t ao_radio_recv(__xdata struct ao_radio_recv *recv) __reentrant; void ao_radio_rdf(int ms); +void +ao_radio_abort(uint8_t reason); + void ao_radio_rdf_abort(void); +void +ao_radio_idle(void); + void ao_radio_init(void); @@ -943,35 +968,21 @@ struct ao_fifo { * Packet-based command interface */ -#define AO_PACKET_MAX 32 -#define AO_PACKET_WIN 256 - -#define AO_PACKET_FIN (1 << 0) -#define AO_PACKET_SYN (1 << 1) -#define AO_PACKET_RST (1 << 2) -#define AO_PACKET_ACK (1 << 3) +#define AO_PACKET_MAX 8 struct ao_packet { uint8_t addr; - uint8_t flags; - uint16_t seq; - uint16_t ack; - uint16_t window; uint8_t len; + uint8_t seq; + uint8_t ack; uint8_t d[AO_PACKET_MAX]; }; -uint8_t -ao_packet_connect(uint8_t dest); - -uint8_t -ao_packet_accept(void); - -int -ao_packet_send(uint8_t *data, int len); - -int -ao_packet_recv(uint8_t *data, int len); +struct ao_packet_recv { + struct ao_packet packet; + int8_t rssi; + uint8_t status; +}; void ao_packet_init(void); diff --git a/src/ao_dma.c b/src/ao_dma.c index a4d45f14..704108e6 100644 --- a/src/ao_dma.c +++ b/src/ao_dma.c @@ -102,11 +102,13 @@ ao_dma_trigger(uint8_t id) } void -ao_dma_abort(uint8_t id) +ao_dma_abort(uint8_t id, uint8_t reason) { uint8_t mask = (1 << id); DMAARM = 0x80 | mask; DMAIRQ &= ~mask; + *(ao_dma_done[id]) |= reason; + ao_wakeup(ao_dma_done[id]); } void @@ -122,7 +124,7 @@ ao_dma_isr(void) interrupt 8 DMAIF = 0; /* Clear the completed ID */ DMAIRQ = ~mask; - *(ao_dma_done[id]) = 1; + *(ao_dma_done[id]) |= AO_DMA_DONE; ao_wakeup(ao_dma_done[id]); break; } diff --git a/src/ao_gps_skytraq.c b/src/ao_gps_skytraq.c index b397d975..cd5f78b9 100644 --- a/src/ao_gps_skytraq.c +++ b/src/ao_gps_skytraq.c @@ -46,6 +46,12 @@ static const char ao_gps_config[] = { 1, /* zda interval */ 0, /* attributes (0 = update to sram, 1 = update flash too) */ 0x09, 0x0d, 0x0a, + + 0xa0, 0xa1, 0x00, 0x03, /* length: 3 bytes */ + 0x3c, /* configure navigation mode */ + 0x00, /* 0 = car, 1 = pedestrian */ + 0x00, /* 0 = update to sram, 1 = update sram + flash */ + 0x3c, 0x0d, 0x0a, }; static void diff --git a/src/ao_monitor.c b/src/ao_monitor.c index e57ea145..9431f726 100644 --- a/src/ao_monitor.c +++ b/src/ao_monitor.c @@ -30,7 +30,8 @@ ao_monitor(void) for (;;) { __critical while (!ao_monitoring) ao_sleep(&ao_monitoring); - ao_radio_recv(&recv); + if (!ao_radio_recv(&recv)) + continue; state = recv.telemetry.flight_state; memcpy(callsign, recv.telemetry.callsign, AO_MAX_CALLSIGN); if (state > ao_flight_invalid) @@ -74,6 +75,7 @@ ao_set_monitor(uint8_t monitoring) { ao_monitoring = monitoring; ao_wakeup(&ao_monitoring); + ao_radio_abort(AO_DMA_ABORTED); } static void diff --git a/src/ao_packet.c b/src/ao_packet.c new file mode 100644 index 00000000..e3133b88 --- /dev/null +++ b/src/ao_packet.c @@ -0,0 +1,308 @@ +/* + * Copyright © 2009 Keith Packard + * + * 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. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" + +static __xdata struct ao_packet_recv rx_packet; +static __xdata struct ao_packet tx_packet; +static __xdata char tx_data[AO_PACKET_MAX]; +static __xdata char rx_data[AO_PACKET_MAX]; +static __pdata uint8_t rx_len, rx_used, tx_used; +static __pdata uint8_t rx_seq; + +static __xdata uint16_t ao_packet_timer_delay; +static __xdata uint8_t ao_packet_timer_cancelled; + +static __xdata struct ao_task ao_packet_task; +static __xdata struct ao_task ao_packet_timer_task; +static __xdata uint8_t ao_packet_enable; +static __xdata uint8_t ao_packet_master_sleeping; + +void +ao_packet_timer(void) __reentrant +{ + uint16_t delay; + + while (ao_packet_enable) { + + /* wait until the timer task is needed + */ + while (!ao_packet_timer_delay && ao_packet_enable) + ao_sleep(&ao_packet_timer_delay); + + delay = ao_packet_timer_delay; + ao_packet_timer_delay = 0; + + /* pause waiting for either a timeout or + * a timer cancel + */ + ao_delay(delay); + + /* if not canceled, abort the receive + */ + if (!ao_packet_timer_cancelled) { + printf ("packet timeout\n"); flush(); + ao_radio_abort(AO_DMA_TIMEOUT); + } + } + ao_exit(); +} + +void +ao_packet_timer_set(uint16_t delay) +{ + ao_packet_timer_delay = delay; + ao_packet_timer_cancelled = 0; + ao_wakeup(&ao_packet_timer_delay); +} + +void +ao_packet_timer_cancel(void) +{ + ao_packet_timer_cancelled = 1; + ao_packet_timer_delay = 0; + ao_wake_task(&ao_packet_timer_task); +} + +void +ao_packet_send(void) +{ + ao_config_get(); + ao_mutex_get(&ao_radio_mutex); + ao_radio_idle(); + RF_CHANNR = ao_config.radio_channel; + ao_dma_set_transfer(ao_radio_dma, + &tx_packet, + &RFDXADDR, + sizeof (struct ao_packet), + DMA_CFG0_WORDSIZE_8 | + DMA_CFG0_TMODE_SINGLE | + DMA_CFG0_TRIGGER_RADIO, + DMA_CFG1_SRCINC_1 | + DMA_CFG1_DESTINC_0 | + DMA_CFG1_PRIORITY_HIGH); + ao_dma_start(ao_radio_dma); + RFST = RFST_STX; + __critical while (!ao_radio_dma_done) + ao_sleep(&ao_radio_dma_done); + ao_mutex_put(&ao_radio_mutex); +} + +uint8_t +ao_packet_recv(void) +{ + uint8_t dma_done; + + ao_config_get(); + ao_mutex_get(&ao_radio_mutex); + ao_radio_idle(); + RF_CHANNR = ao_config.radio_channel; + ao_dma_set_transfer(ao_radio_dma, + &RFDXADDR, + &rx_packet, + sizeof (struct ao_packet_recv), + DMA_CFG0_WORDSIZE_8 | + DMA_CFG0_TMODE_SINGLE | + DMA_CFG0_TRIGGER_RADIO, + DMA_CFG1_SRCINC_0 | + DMA_CFG1_DESTINC_1 | + DMA_CFG1_PRIORITY_HIGH); + ao_dma_start(ao_radio_dma); + RFST = RFST_SRX; + __critical while (!ao_radio_dma_done) + ao_sleep(&ao_radio_dma_done); + dma_done = ao_radio_dma_done; + ao_mutex_put(&ao_radio_mutex); + + if (dma_done & AO_DMA_DONE) { + printf ("rssi %d status %x\n", rx_packet.rssi, rx_packet.status); flush(); + if (!(rx_packet.status & PKT_APPEND_STATUS_1_CRC_OK)) { + printf ("bad crc\n"); flush(); +// return AO_DMA_ABORTED; + } + if (rx_packet.packet.len) { + if (rx_packet.packet.seq == rx_seq + 1 && rx_used == rx_len) + { + memcpy(rx_data, rx_packet.packet.d, rx_packet.packet.len); + rx_used = 0; + rx_len = rx_packet.packet.len; + rx_seq = rx_packet.packet.seq; + tx_packet.ack = rx_seq; + ao_wakeup(&rx_data); + } + } + if (rx_packet.packet.ack == tx_packet.seq) { + tx_packet.len = 0; + ao_wakeup(&tx_packet); + } + } + return dma_done; +} + +void +ao_packet_slave(void) +{ + tx_packet.addr = ao_serial_number; + ao_radio_set_packet(); + while (ao_packet_enable) { + ao_packet_recv(); + ao_led_toggle(AO_LED_GREEN); + ao_delay(AO_MS_TO_TICKS(100)); + ao_packet_send(); + ao_led_toggle(AO_LED_RED); + } + ao_exit(); +} + +/* Thread for the master side of the packet link */ + +void +ao_packet_master(void) +{ + uint8_t status; + tx_packet.addr = ao_serial_number; + ao_radio_set_packet(); + while (ao_packet_enable) { + ao_delay(AO_MS_TO_TICKS(100)); + ao_packet_send(); + ao_led_toggle(AO_LED_RED); + ao_packet_timer_set(AO_MS_TO_TICKS(1000)); + status = ao_packet_recv(); + ao_packet_timer_cancel(); + if (status & AO_DMA_DONE) { + ao_led_toggle(AO_LED_GREEN); + ao_packet_master_sleeping = 1; + ao_sleep(AO_MS_TO_TICKS(1000)); + ao_packet_master_sleeping = 0; + } + } + ao_exit(); +} + +void +ao_packet_flush(void) +{ + if (!tx_used) + return; + + /* Wait for previous packet to be received + */ + while (tx_packet.len) + ao_sleep(&tx_packet); + + /* Prepare next packet + */ + if (tx_used) { + memcpy(&tx_packet.d, tx_data, tx_used); + tx_packet.len = tx_used; + tx_packet.seq++; + tx_used = 0; + + if (ao_packet_master_sleeping) + ao_wake_task(&ao_packet_task); + } +} + +void +ao_packet_putchar(char c) +{ + while (tx_used == AO_PACKET_MAX && ao_packet_enable) + ao_packet_flush(); + + if (ao_packet_enable) + tx_data[tx_used++] = c; +} + +char +ao_packet_getchar(void) __critical +{ + while (rx_used == rx_len && ao_packet_enable) + ao_sleep(&rx_data); + + if (!ao_packet_enable) + return 0; + + return rx_data[rx_used++]; +} + +static void +ao_packet_echo(void) __reentrant +{ + uint8_t c; + while (ao_packet_enable) { + c = ao_packet_getchar(); + if (ao_packet_enable) + putchar(c); + } + ao_exit(); +} + +static __xdata struct ao_task ao_packet_echo_task; + +static void +ao_packet_forward(void) __reentrant +{ + char c; + ao_packet_enable = 1; + ao_cmd_white(); + + ao_radio_set_packet(); + if (ao_cmd_lex_c == 'm') { + while ((c = getchar()) != '~') + ao_packet_send(); + } else { + for (;;) { + ao_packet_recv(); + ao_led_toggle(AO_LED_GREEN); + if (rx_packet.packet.d[0] == (uint8_t) '@') + break; + } + } + ao_packet_enable = 0; + return; +#if 0 + if (ao_cmd_lex_c == 'm') { + ao_add_task(&ao_packet_timer_task, ao_packet_timer, "timeout"); + ao_add_task(&ao_packet_task, ao_packet_master, "master"); + } + else + ao_add_task(&ao_packet_task, ao_packet_slave, "slave"); + ao_add_task(&ao_packet_echo_task, ao_packet_echo, "echo"); + while ((c = getchar()) != '~') { + ao_packet_putchar(c); + if (c == '\n') + ao_packet_flush(); + } + ao_packet_enable = 0; + ao_radio_abort(AO_DMA_ABORTED); + while (ao_packet_echo_task.wchan || ao_packet_task.wchan) { + ao_wake_task(&ao_packet_echo_task); + ao_wake_task(&ao_packet_task); + } +#endif +} + +__code struct ao_cmds ao_packet_cmds[] = { + { 'p', ao_packet_forward, "p {m|s} Remote packet link. m=master, s=slave" }, + { 0, ao_packet_forward, NULL }, +}; + +void +ao_packet_init(void) +{ + ao_cmd_register(&ao_packet_cmds[0]); +} diff --git a/src/ao_radio.c b/src/ao_radio.c index a7fa682e..2dfa9279 100644 --- a/src/ao_radio.c +++ b/src/ao_radio.c @@ -255,11 +255,62 @@ static __code uint8_t telemetry_setup[] = { RF_PKTCTRL0_LENGTH_CONFIG_FIXED), }; +static __code uint8_t packet_setup[] = { + RF_MDMCFG4_OFF, ((CHANBW_E << RF_MDMCFG4_CHANBW_E_SHIFT) | + (CHANBW_M << RF_MDMCFG4_CHANBW_M_SHIFT) | + (DRATE_E << RF_MDMCFG4_DRATE_E_SHIFT)), + RF_MDMCFG3_OFF, (DRATE_M << RF_MDMCFG3_DRATE_M_SHIFT), + RF_MDMCFG2_OFF, (RF_MDMCFG2_DEM_DCFILT_OFF | + RF_MDMCFG2_MOD_FORMAT_GFSK | + RF_MDMCFG2_SYNC_MODE_15_16_THRES), + RF_MDMCFG1_OFF, (RF_MDMCFG1_FEC_EN | + RF_MDMCFG1_NUM_PREAMBLE_4 | + (2 << RF_MDMCFG1_CHANSPC_E_SHIFT)), + + RF_DEVIATN_OFF, ((DEVIATION_E << RF_DEVIATN_DEVIATION_E_SHIFT) | + (DEVIATION_M << RF_DEVIATN_DEVIATION_M_SHIFT)), + + /* max packet length */ + RF_PKTLEN_OFF, sizeof (struct ao_packet), + RF_PKTCTRL1_OFF, ((1 << PKTCTRL1_PQT_SHIFT)| + PKTCTRL1_APPEND_STATUS| + PKTCTRL1_ADR_CHK_NONE), + RF_PKTCTRL0_OFF, (RF_PKTCTRL0_WHITE_DATA| + RF_PKTCTRL0_PKT_FORMAT_NORMAL| + RF_PKTCTRL0_CRC_EN| + RF_PKTCTRL0_LENGTH_CONFIG_FIXED), +}; + + +void +ao_radio_set_telemetry(void) +{ + uint8_t i; + for (i = 0; i < sizeof (telemetry_setup); i += 2) + RF[telemetry_setup[i]] = telemetry_setup[i+1]; +} + +void +ao_radio_set_packet(void) +{ + uint8_t i; + for (i = 0; i < sizeof (packet_setup); i += 2) + RF[packet_setup[i]] = packet_setup[i+1]; +} + +void +ao_radio_set_rdf(void) +{ + uint8_t i; + for (i = 0; i < sizeof (rdf_setup); i += 2) + RF[rdf_setup[i]] = rdf_setup[i+1]; +} + __xdata uint8_t ao_radio_dma; __xdata uint8_t ao_radio_dma_done; __xdata uint8_t ao_radio_mutex; -static void +void ao_radio_idle(void) { if (RF_MARCSTATE != RF_MARCSTATE_IDLE) @@ -295,7 +346,7 @@ ao_radio_send(__xdata struct ao_telemetry *telemetry) __reentrant ao_mutex_put(&ao_radio_mutex); } -void +uint8_t ao_radio_recv(__xdata struct ao_radio_recv *radio) __reentrant { ao_config_get(); @@ -317,6 +368,7 @@ ao_radio_recv(__xdata struct ao_radio_recv *radio) __reentrant __critical while (!ao_radio_dma_done) ao_sleep(&ao_radio_dma_done); ao_mutex_put(&ao_radio_mutex); + return (ao_radio_dma_done & AO_DMA_DONE); } __xdata ao_radio_rdf_running; @@ -367,13 +419,18 @@ ao_radio_rdf(int ms) ao_mutex_put(&ao_radio_mutex); } +void +ao_radio_abort(uint8_t reason) +{ + ao_dma_abort(ao_radio_dma, reason); + ao_radio_idle(); +} + void ao_radio_rdf_abort(void) { - if (ao_radio_rdf_running) { - ao_dma_abort(ao_radio_dma); - ao_radio_idle(); - } + if (ao_radio_rdf_running) + ao_radio_abort(AO_DMA_ABORTED); } void @@ -382,6 +439,7 @@ ao_radio_init(void) uint8_t i; for (i = 0; i < sizeof (radio_setup); i += 2) RF[radio_setup[i]] = radio_setup[i+1]; + ao_radio_set_telemetry(); ao_radio_dma_done = 1; ao_radio_dma = ao_dma_alloc(&ao_radio_dma_done); } diff --git a/src/ao_teledongle.c b/src/ao_teledongle.c index d7b4b75a..98642180 100644 --- a/src/ao_teledongle.c +++ b/src/ao_teledongle.c @@ -33,6 +33,7 @@ main(void) ao_monitor_init(AO_LED_GREEN, TRUE); ao_rssi_init(AO_LED_RED); ao_radio_init(); + ao_packet_init(); ao_config_init(); ao_start_scheduler(); } diff --git a/src/ao_telemetrum.c b/src/ao_telemetrum.c index 1dbacf89..07737f30 100644 --- a/src/ao_telemetrum.c +++ b/src/ao_telemetrum.c @@ -40,6 +40,7 @@ main(void) ao_gps_report_init(); ao_telemetry_init(); ao_radio_init(); + ao_packet_init(); ao_igniter_init(); ao_config_init(); ao_start_scheduler(); diff --git a/src/skytraq-cksum b/src/skytraq-cksum index e4960bff..ab0464a7 100644 --- a/src/skytraq-cksum +++ b/src/skytraq-cksum @@ -32,7 +32,7 @@ void main() } } } - printf("\t0xa0, 0xa1, 0x02x, 0x%02x,\t\t/* length: %d bytes */\n", + printf("\t0xa0, 0xa1, 0x%02x, 0x%02x,\t\t/* length: %d bytes */\n", dim(msg) >> 8, dim(msg) & 0xff, dim(msg)); for (int i = 0; i < dim(input); i++) printf("%s\n", input[i]); -- 2.30.2