// Prepare to transmit a response to the TAG.
// TODO: check to see if we should even bother. Did we get enough packets?
static void ranging_listening_window_setup () {
// Stop iterating through timing channels
timer_stop(_anchor_timer);
// We no longer need to receive and need to instead
// start transmitting.
dwt_forcetrxoff();
// Update our state to the TX response state
_state = ASTATE_RESPONDING;
// Set the listening window index
_ranging_listening_window_num = 0;
// Determine which antenna we are going to use for
// the response.
uint8_t max_packets = 0;
uint8_t max_index = 0;
for (uint8_t i=0; i<NUM_ANTENNAS; i++) {
if (_anchor_antenna_recv_num[i] > max_packets) {
max_packets = _anchor_antenna_recv_num[i];
max_index = i;
}
}
pp_anc_final_pkt.final_antenna = max_index;
// Now we need to setup a timer to iterate through
// the response windows so we can send a packet
// back to the tag
timer_start(_anchor_timer,
_ranging_operation_config.anchor_reply_window_in_us,
ranging_listening_window_task);
}
// Update the Antenna and Channel settings to correspond with the settings
// for the given subsequence number.
//
// role: anchor or tag
// subseq_num: where in the sequence we are
void oneway_set_ranging_broadcast_subsequence_settings (dw1000_role_e role,
uint8_t subseq_num) {
// Stop the transceiver on the anchor. Don't know why.
if (role == ANCHOR) {
dwt_forcetrxoff();
}
// Change the channel depending on what subsequence number we're at
dw1000_update_channel(subsequence_number_to_channel(subseq_num));
// Change what antenna we're listening/sending on
dw1000_choose_antenna(oneway_subsequence_number_to_antenna(role, subseq_num));
}
void oneway_anchor_init () {
// Make sure the SPI speed is slow for this function
dw1000_spi_slow();
// Setup callbacks to this ANCHOR
dwt_setcallbacks(anchor_txcallback, anchor_rxcallback);
// Make sure the radio starts off
dwt_forcetrxoff();
// Set the anchor so it only receives data and ack packets
dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN);
// // Set the ID and PAN ID for this anchor
uint8_t eui_array[8];
dw1000_read_eui(eui_array);
// dwt_seteui(eui_array);
// dwt_setpanid(POLYPOINT_PANID);
// Automatically go back to receive
dwt_setautorxreenable(TRUE);
// Don't use these
dwt_setdblrxbuffmode(FALSE);
dwt_setrxtimeout(FALSE);
// Load our EUI into the outgoing packet
dw1000_read_eui(pp_anc_final_pkt.ieee154_header_unicast.sourceAddr);
// Need a timer
if (_anchor_timer == NULL) {
_anchor_timer = timer_init();
}
// Init the PRNG for determining when to respond to the tag
raninit(&_prng_state, eui_array[0]<<8|eui_array[1]);
// Make SPI fast now that everything has been setup
dw1000_spi_fast();
// Reset our state because nothing should be in progress if we call init()
_state = ASTATE_IDLE;
}
dw1000_err_e dw1000_anchor_init () {
uint8_t eui_array[8];
// Make sure the radio starts off
dwt_forcetrxoff();
// Set the anchor so it only receives data and ack packets
dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN);
// Set the ID and PAN ID for this anchor
dw1000_read_eui(eui_array);
dwt_seteui(eui_array);
dwt_setpanid(POLYPOINT_PANID);
// Automatically go back to receive
dwt_setautorxreenable(TRUE);
// Don't use these
dwt_setdblrxbuffmode(FALSE);
dwt_setrxtimeout(FALSE);
// Don't receive at first
dwt_rxenable(FALSE);
// Load our EUI into the outgoing packet
dw1000_read_eui(pp_anc_final_pkt.ieee154_header_unicast.sourceAddr);
// Need a timer
_ranging_broadcast_timer = timer_init();
// Init the PRNG for determining when to respond to the tag
raninit(&_prng_state, eui_array[0]<<8|eui_array[1]);
// Make SPI fast now that everything has been setup
dw1000_spi_fast();
return DW1000_NO_ERR;
}
int process_usbmessage(void)
{
int result = 0;
switch(application_mode)
{
case STAND_ALONE:
{
if(local_buff_length == 5)
{
//d (from "deca")
if((local_buff[0] == 100) && (result == 0)) //d (from "deca")
{
if(local_buff[4] == 63)
{
int i = sizeof(SOFTWARE_VER_STRINGUSB);
//change mode to USB_TO_SPI and send a reply "y"
tx_buff[0] = 121;
memcpy(&tx_buff[1], SOFTWARE_VER_STRINGUSB, i);
tx_buff[i+2] = 0;
tx_buff_length = i + 2;
application_mode = USB_TO_SPI;
result = 2;
//led_off(LED_ALL);
led_on(LED_PC7); //turn on LED to indicate connection to PC application
}
}
}
}
break;
case USB_TO_SPI:
{
//first byte specifies the SPI speed and SPI read/write operation
//bit 0 = 1 for write, 0 for read
//bit 1 = 1 for high speed, 0 for low speed
//<STX> <ETX>
//
//to read from the device (e.g. 4 bytes), total length is = 1 + 1 + length_of_command (2) + length_of_data_to_read (2) + header length (1/2) + 1
//
//to write to the device (e.g. 4 bytes), total length is = 1 + 1 + length_of_command (2) + length_of_data_to_write (2) + header length (1/2) + data length + 1
//
//LBS comes first: 0x2, 0x2, 0x7, 0x0, 0x04, 0x00, 0x3
if(local_buff_length)
{
//0x2 = STX - start of SPI transaction data
if(local_buff[0] == 0x2)
{
//configure SPI speed
configSPIspeed(((local_buff[1]>>1) & 0x1));
if((local_buff[1] & 0x1) == 0) //SPI read
{
int msglength = local_buff[2] + (local_buff[3]<<8);
int datalength = local_buff[4] + (local_buff[5]<<8);
//led_on(LED_PC6);
tx_buff[0] = 0x2;
tx_buff[datalength+2] = 0x3;
//max data we can read in a single SPI transaction is 4093 as the USB/VCP tx buffer is only 4096 bytes long
if((local_buff[msglength-1] != 0x3) || (datalength > 4093))
{
tx_buff[1] = 0x1; // if no ETX (0x3) indicate error
}
else
{
// do the read from the SPI
readfromspi(msglength-7, &local_buff[6], datalength, &tx_buff[2]); // result is stored in the buffer
tx_buff[1] = 0x0; // no error
}
tx_buff_length = datalength + 3;
result = 2;
}
if((local_buff[1] & 0x1) == 1) //SPI write
{
int msglength = local_buff[2] + (local_buff[3]<<8);
int datalength = local_buff[4] + (local_buff[5]<<8);
int headerlength = msglength - 7 - datalength;
if(local_buff_length == msglength) //we got the whole message (sent from the PC)
{
local_buff_offset = 0;
led_off(LED_PC6);
tx_buff[0] = 0x2;
tx_buff[2] = 0x3;
if(local_buff[msglength-1] != 0x3)
{
tx_buff[1] = 0x1; // if no ETX (0x3) indicate error
}
else
{
// do the write to the SPI
writetospi(headerlength, &local_buff[6], datalength, &local_buff[6+headerlength]); // result is stored in the buffer
tx_buff[1] = 0x0; // no error
}
tx_buff_length = 3;
result = 2;
}
else //wait for the whole message
{
led_on(LED_PC6);
}
}
}
if((local_buff[0] == 100) && (result == 0)) //d (from "deca")
{
if(local_buff[4] == 63)
{
int i = sizeof(SOFTWARE_VER_STRINGUSB);
//change mode to USB_TO_SPI and send a reply "y"
tx_buff[0] = 121;
memcpy(&tx_buff[1], SOFTWARE_VER_STRINGUSB, i);
tx_buff[i+2] = 0;
tx_buff_length = i + 2;
application_mode = USB_TO_SPI;
result = 2;
//led_off(LED_ALL);
led_on(LED_PC7); //turn on LED to indicate connection to PC application
}
}
if((local_buff[0] == 114) && (result == 0)) //r - flush the USB buffers...
{
DCD_EP_Flush(&USB_OTG_dev, CDC_IN_EP);
result = 0;
}
}
}
break;
case USB_PRINT_ONLY:
if(local_buff_length && (result == 0))
{
if((local_buff[0] == 0x5) && (local_buff[5] == 0x5))
{
uint16 txantennadelay = local_buff[1] + (local_buff[2]<<8);
uint16 rxantennadelay = local_buff[3] + (local_buff[4]<<8);
instanceconfigantennadelays(txantennadelay, rxantennadelay);
}
if((local_buff[0] == 0x7) && (local_buff[5] == 0x7))
{
//not used in EVK
}
if((local_buff[0] == 0x6) && (local_buff[2] == 0x6))
{
uint8 switchS1 = local_buff[1];
//disable DW1000 IRQ
port_DisableEXT_IRQ(); //disable IRQ until we configure the device
//turn DW1000 off
dwt_forcetrxoff();
//re-configure the instance
inittestapplication(switchS1);
//save the new setting
s1configswitch = switchS1;
//set the LDC
setLCDline1(switchS1);
//enable DW1000 IRQ
port_EnableEXT_IRQ(); //enable IRQ before starting
ranging = 0;
}
//d (from "deca")
if(local_buff[0] == 100) //d (from "deca")
{
if(local_buff[4] == 63)
{
int i = sizeof(SOFTWARE_VER_STRINGUSB);
//send a reply "n"
tx_buff[0] = 110;
memcpy(&tx_buff[1], SOFTWARE_VER_STRINGUSB, i);
tx_buff[i+2] = 0;
tx_buff_length = i + 2;
result = 2;
}
if(local_buff[4] == 36) //"$"
{
//send a reply "n"
tx_buff[0] = 110;
memcpy(&tx_buff[1], version, version_size);
tx_buff[version_size+1] = s1configswitch & 0xff;
tx_buff[version_size+2] = '\r';
tx_buff[version_size+3] = '\n';
tx_buff_length = version_size + 4;
result = 2;
}
if(local_buff[4] == 33) //"!"
{
//send back the DW1000 partID and lotID
uint32 partID = dwt_getpartid();
uint32 lotID = dwt_getlotid();
tx_buff[0] = 110;
memcpy(&tx_buff[1], &partID, 4);
memcpy(&tx_buff[5], &lotID, 4);
tx_buff[9] = '\r';
tx_buff[10] = '\n';
tx_buff_length = 11;
result = 2;
}
}
}
break;
default:
break;
}
// Triggered when we receive a packet
void app_dw1000_rxcallback (const dwt_callback_data_t *rxd) {
// First grab a copy of local time when this arrived
rtimer_clock_t rt_timestamp = RTIMER_NOW();
DEBUG_B6_HIGH;
if (rxd->event == DWT_SIG_RX_OKAY) {
leds_toggle(LEDS_BLUE);
uint8_t packet_type;
uint64_t dw_timestamp;
uint8_t recv_pkt_buf[RX_PKT_BUF_LEN];
// Get the dw_timestamp first
uint8_t txTimeStamp[5] = {0, 0, 0, 0, 0};
dwt_readrxtimestamp(txTimeStamp);
dw_timestamp = ((uint64_t) (*((uint32_t*) txTimeStamp))) | (((uint64_t) txTimeStamp[4]) << 32);
// Get the packet
dwt_readrxdata(recv_pkt_buf, MIN(RX_PKT_BUF_LEN, rxd->datalength), 0);
packet_type = recv_pkt_buf[offsetof(struct pp_tag_poll, message_type)];
global_round_num = recv_pkt_buf[offsetof(struct pp_tag_poll, roundNum)];
if (packet_type == MSG_TYPE_PP_ONEWAY_TAG_POLL) {
struct pp_tag_poll* pkt = (struct pp_tag_poll*) recv_pkt_buf;
DEBUG_P("TAG_POLL rx: %u\r\n", pkt->subsequence);
if (global_subseq_num == pkt->subsequence) {
// Configurations matched, record arrival time
pp_anc_final_pkt.TOAs[global_subseq_num] = dw_timestamp;
} else {
// Tag/anchor weren't on same settings, so we
// drop this sample and catch the anchor up
global_subseq_num = pkt->subsequence;
}
if (!global_round_active) {
DEBUG_B4_LOW;
DEBUG_B5_LOW;
memset(antenna_statistics, 0, sizeof(antenna_statistics));
global_round_active = true;
start_of_new_subseq = true;
substate_timer_fired = true;
/*
subseq_start_time = rt_timestamp - US_TO_RT(TAG_SQ_START_TO_POLL_SFD_HIGH_US);
rtimer_clock_t set_to = subseq_start_time + US_TO_RT(POLL_TO_SS_US+SS_TO_SQ_US);
*/
rtimer_clock_t set_to = rt_timestamp + US_TO_RT(
POLL_TO_SS_US + SS_TO_SQ_US
- TAG_SQ_START_TO_POLL_SFD_HIGH_US
- ANC_MYSTERY_STARTUP_DELAY_US);
rtimer_set(&subsequence_timer,
set_to,
1,
(rtimer_callback_t)subsequence_task,
NULL);
}
//Keep a running total of the number of packets seen from each antenna
antenna_statistics[subseq_num_to_anchor_sel(global_subseq_num)]++;
} else if (packet_type == MSG_TYPE_PP_ONEWAY_TAG_FINAL) {
if (!global_round_active) {
// The first packet we happened to receive was
// an ANC_FINAL. We have nothing interesting to
// reply with, so don't. But we *do* need to set
// receive mode again so that a new poll will be
// caught
dwt_rxenable(0);
return;
}
//We're likely in RX mode, so we need to exit before transmission
dwt_forcetrxoff();
pp_anc_final_pkt.TOAs[NUM_MEASUREMENTS] = dw_timestamp;
pp_anc_final_pkt.header.seqNum++;
const uint16 frame_len = sizeof(struct pp_anc_final);
dwt_writetxfctrl(frame_len, 0);
//Schedule this transmission for our scheduled time slot
DEBUG_B6_LOW;
uint32_t temp = dwt_readsystimestamphi32();
uint32_t delay_time = temp +
DW_DELAY_FROM_US(
ANC_FINAL_INITIAL_DELAY_HACK_VALUE +
(ANC_FINAL_RX_TIME_ON_TAG*(ANCHOR_EUI-1))
);
/* I don't understand what exactly is going on
* here. The chip seems to want way longer for
* this preamble than others -- maybe something
* to do with the large payload? From empirical
* measurements, the 300 base delay is about the
* minimum (250 next tested as not working)
DW_DELAY_FROM_US(
REVISED_DELAY_FROM_PKT_LEN_US(frame_len) +
(2*ANC_FINAL_RX_TIME_ON_TAG*(ANCHOR_EUI-1))
);
*/
delay_time &= 0xFFFFFFFE;
pp_anc_final_pkt.dw_time_sent = delay_time;
dwt_setdelayedtrxtime(delay_time);
int err = dwt_starttx(DWT_START_TX_DELAYED);
dwt_settxantennadelay(TX_ANTENNA_DELAY);
dwt_writetxdata(frame_len, (uint8_t*) &pp_anc_final_pkt, 0);
DEBUG_B6_HIGH;
#ifdef DW_DEBUG
// No printing until after all dwt timing op's
struct pp_tag_poll* pkt = (struct pp_tag_poll*) recv_pkt_buf;
DEBUG_P("TAG_FINAL rx: %u\r\n", pkt->subsequence);
#endif
if (err) {
#ifdef DW_DEBUG
uint32_t now = dwt_readsystimestamphi32();
DEBUG_P("Could not send anchor response\r\n");
DEBUG_P(" -- sched time %lu, now %lu (diff %lu)\r\n", delay_time, now, now-delay_time);
leds_on(LEDS_RED);
#endif
} else {
DEBUG_P("Send ANC_FINAL\r\n");
leds_off(LEDS_RED);
}
} else {
DEBUG_P("*** ERR: RX Unknown packet type: 0x%X\r\n", packet_type);
}
} else {
// Called when the radio has received a packet.
void dw1000_anchor_rxcallback (const dwt_callback_data_t *rxd) {
if (rxd->event == DWT_SIG_RX_OKAY) {
// Read in parameters of this packet reception
uint64_t dw_rx_timestamp;
uint8_t buf[DW1000_ANCHOR_MAX_RX_PKT_LEN];
uint8_t message_type;
// Get the received time of this packet first
dwt_readrxtimestamp(buf);
dw_rx_timestamp = DW_TIMESTAMP_TO_UINT64(buf);
// Get the actual packet bytes
dwt_readrxdata(buf, MIN(DW1000_ANCHOR_MAX_RX_PKT_LEN, rxd->datalength), 0);
// We process based on the first byte in the packet. How very active
// message like...
message_type = buf[offsetof(struct pp_tag_poll, message_type)];
if (message_type == MSG_TYPE_PP_NOSLOTS_TAG_POLL) {
// This is one of the broadcast ranging packets from the tag
struct pp_tag_poll* rx_poll_pkt = (struct pp_tag_poll*) buf;
// Decide what to do with this packet
if (_state == ASTATE_IDLE) {
// We are currently not ranging with any tags.
if (rx_poll_pkt->subsequence < NUM_RANGING_CHANNELS) {
// We are idle and this is one of the first packets
// that the tag sent. Start listening for this tag's
// ranging broadcast packets.
_state = ASTATE_RANGING;
// Clear memory for this new tag ranging event
memset(pp_anc_final_pkt.TOAs, 0, sizeof(pp_anc_final_pkt.TOAs));
memset(_anchor_antenna_recv_num, 0, sizeof(_anchor_antenna_recv_num));
// Record the EUI of the tag so that we don't get mixed up
memcpy(pp_anc_final_pkt.ieee154_header_unicast.destAddr, rx_poll_pkt->header.sourceAddr, 8);
// Record which ranging subsequence the tag is on
_ranging_broadcast_ss_num = rx_poll_pkt->subsequence;
// Record the timestamp
pp_anc_final_pkt.TOAs[_ranging_broadcast_ss_num] = dw_rx_timestamp;
// Also record parameters the tag has sent us about how to respond
// (or other operational parameters).
_ranging_operation_config.reply_after_subsequence = rx_poll_pkt->reply_after_subsequence;
_ranging_operation_config.anchor_reply_window_in_us = rx_poll_pkt->anchor_reply_window_in_us;
_ranging_operation_config.anchor_reply_slot_time_in_us = rx_poll_pkt->anchor_reply_slot_time_in_us;
// Update the statistics we keep about which antenna
// receives the most packets from the tag
uint8_t recv_antenna_index = subsequence_number_to_antenna(ANCHOR, rx_poll_pkt->subsequence);
_anchor_antenna_recv_num[recv_antenna_index]++;
// Now we need to start our own state machine to iterate
// through the antenna / channel combinations while listening
// for packets from the same tag.
_ranging_broadcast_ss_num++;
dw1000_set_ranging_broadcast_subsequence_settings(ANCHOR, _ranging_broadcast_ss_num, FALSE);
timer_start(_ranging_broadcast_timer, RANGING_BROADCASTS_PERIOD_US, ranging_broadcast_subsequence_task);
} else {
// We found this tag ranging sequence late. We don't want
// to use this because we won't get enough range estimates.
// Just stay idle.
}
} else if (_state == ASTATE_RANGING) {
// We are currently ranging with a tag, waiting for the various
// ranging broadcast packets.
// First check if this is from the same tag
if (memcmp(pp_anc_final_pkt.ieee154_header_unicast.destAddr, rx_poll_pkt->header.sourceAddr, 8) == 0) {
// Same tag
if (rx_poll_pkt->subsequence == _ranging_broadcast_ss_num) {
// This is the packet we were expecting from the tag.
// Record the TOA.
pp_anc_final_pkt.TOAs[_ranging_broadcast_ss_num] = dw_rx_timestamp;
// Update the statistics we keep about which antenna
// receives the most packets from the tag
uint8_t recv_antenna_index = subsequence_number_to_antenna(ANCHOR, _ranging_broadcast_ss_num);
_anchor_antenna_recv_num[recv_antenna_index]++;
} else {
// Some how we got out of sync with the tag. Ignore the
// range and catch up.
_ranging_broadcast_ss_num = rx_poll_pkt->subsequence;
}
// Check to see if we got the last of the ranging broadcasts
if (_ranging_broadcast_ss_num == _ranging_operation_config.reply_after_subsequence) {
// We did!
// Stop iterating through timing channels
timer_stop(_ranging_broadcast_timer);
// We no longer need to receive and need to instead
// start transmitting.
dwt_forcetrxoff();
// Update our state to the TX response state
_state = ASTATE_RESPONDING;
// Set the listening window index
_ranging_listening_window_num = 0;
// Determine which antenna we are going to use for
// the response.
uint8_t max_packets = 0;
uint8_t max_index = 0;
for (uint8_t i=0; i<NUM_ANTENNAS; i++) {
if (_anchor_antenna_recv_num[i] > max_packets) {
max_packets = _anchor_antenna_recv_num[i];
max_index = i;
}
}
pp_anc_final_pkt.final_antenna = max_index;
// Now we need to setup a timer to iterate through
// the response windows so we can send a packet
// back to the tag
timer_start(_ranging_broadcast_timer,
_ranging_operation_config.anchor_reply_window_in_us,
ranging_listening_window_task);
}
} else {
// Not the same tag, ignore
}
} else {
// We are in some other state, not sure what that means
}
} else {
// Other message types go here, if they get added
}
} else {
