void DWM1000_Anchor::init() {
INFO("HSPI");
//_________________________________________________INIT SPI ESP8266
resetChip();
initSpi();
enableIsr();
uint64_t eui = 0xF1F2F3F4F5F6F7F;
dwt_seteui((uint8_t*) &eui);
dwt_geteui((uint8_t*) &eui);
LOG<< HEX << "EUID : "<< eui <<FLUSH;
dwt_seteui((uint8_t*) &eui);
dwt_geteui((uint8_t*) &eui);
LOG<< HEX << "EUID : "<< eui <<FLUSH;
// dwt_softreset();
deca_sleep(100);
if (dwt_initialise(DWT_LOADUCODE)) {
LOG<< " dwt_initialise failed " << FLUSH;
} else
LOG<< " dwt_initialise done." << FLUSH;
if (dwt_configure(&config)) {
LOG<< " dwt_configure failed " << FLUSH;
} else
LOG<< " dwt_configure done." << FLUSH;
uint32_t device_id = dwt_readdevid();
uint32_t part_id = dwt_getpartid();
uint32_t lot_id = dwt_getlotid();
LOG<< HEX << " device id : " << device_id << ", part id : " << part_id << ", lot_id :" << lot_id <<FLUSH;
/* Apply default antenna delay value. See NOTE 1 below. */
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);
/* Set expected response's delay and timeout. See NOTE 4 and 5 below.
* As this example only handles one incoming frame with always the same delay and timeout, those values can be set here once for all. */
dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);
dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
dwt_initialise(DWT_LOADUCODE);
// Configure the callbacks from the dwt library
dwt_setcallbacks(txcallback, rxcallback);
_count = 0;
}
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;
}
int app_dw1000_init (
int HACK_role,
int HACK_EUI,
void (*txcallback)(const dwt_callback_data_t *),
void (*rxcallback)(const dwt_callback_data_t *)
) {
uint32_t devID;
int err;
// Start off DW1000 comms slow
REG(SSI0_BASE + SSI_CR1) = 0;
REG(SSI0_BASE + SSI_CPSR) = 8;
REG(SSI0_BASE + SSI_CR1) |= SSI_CR1_SSE;
// Reset the DW1000...for some reason
dw1000_reset();
// Make sure we can talk to the DW1000
devID = dwt_readdevid();
if (devID != DWT_DEVICE_ID) {
#ifdef DW_DEBUG
printf("Could not read Device ID from the DW1000\r\n");
printf("Possible the chip is asleep...\r\n");
#endif
return -1;
}
// Select which of the three antennas on the board to use
dw1000_choose_antenna(0);
// Init the dw1000 hardware
err = dwt_initialise(DWT_LOADUCODE |
DWT_LOADLDO |
DWT_LOADTXCONFIG |
DWT_LOADXTALTRIM);
if (err != DWT_SUCCESS) {
return -1;
}
// Setup interrupts
// Note: using auto rx re-enable so don't need to trigger on error frames
dwt_setinterrupt(DWT_INT_TFRS |
DWT_INT_RFCG |
DWT_INT_SFDT |
DWT_INT_RFTO |
DWT_INT_RPHE |
DWT_INT_RFCE |
DWT_INT_RFSL |
DWT_INT_RXPTO |
DWT_INT_SFDT, 1);
// Configure the callbacks from the dwt library
dwt_setcallbacks(txcallback, rxcallback);
// Set the parameters of ranging and channel and whatnot
global_ranging_config.chan = 2;
global_ranging_config.prf = DWT_PRF_64M;
global_ranging_config.txPreambLength = DWT_PLEN_64;//DWT_PLEN_4096
// global_ranging_config.txPreambLength = DWT_PLEN_256;
global_ranging_config.rxPAC = DWT_PAC8;
global_ranging_config.txCode = 9; // preamble code
global_ranging_config.rxCode = 9; // preamble code
global_ranging_config.nsSFD = 0;
global_ranging_config.dataRate = DWT_BR_6M8;
global_ranging_config.phrMode = DWT_PHRMODE_EXT; //Enable extended PHR mode (up to 1024-byte packets)
global_ranging_config.smartPowerEn = 1;
global_ranging_config.sfdTO = 64+8+1;//(1025 + 64 - 32);
dwt_configure(&global_ranging_config, 0);//(DWT_LOADANTDLY | DWT_LOADXTALTRIM));
dwt_setsmarttxpower(global_ranging_config.smartPowerEn);
// Configure TX power
{
global_tx_config.PGdly = pgDelay[global_ranging_config.chan];
global_tx_config.power = txPower[global_ranging_config.chan];
dwt_configuretxrf(&global_tx_config);
}
/* All constants same anyway
if(DW1000_ROLE_TYPE == TAG)
dwt_xtaltrim(xtaltrim[0]);
else
dwt_xtaltrim(xtaltrim[ANCHOR_EUI]);
*/
dwt_xtaltrim(xtaltrim[0]);
////TEST 1: XTAL trim calibration
//dwt_configcwmode(global_ranging_config.chan);
//dwt_xtaltrim(8);
//while(1);
//{
// //TEST 2: TX Power level calibration
// uint8_t msg[127] = "The quick brown fox jumps over the lazy dog. The quick brown fox jumps over the lazy dog. The quick brown fox jumps over the l";
// dwt_configcontinuousframemode(0x1000);
// dwt_writetxdata(127, (uint8 *) msg, 0) ;
// dwt_writetxfctrl(127, 0);
// dwt_starttx(DWT_START_TX_IMMEDIATE);
// while(1);
//}
// Configure the antenna delay settings
{
uint16_t antenna_delay;
//Antenna delay not really necessary if we're doing an end-to-end calibration
antenna_delay = 0;
dwt_setrxantennadelay(antenna_delay);
dwt_settxantennadelay(antenna_delay);
//global_tx_antenna_delay = antenna_delay;
//// Shift this over a bit for some reason. Who knows.
//// instance_common.c:508
//antenna_delay = dwt_readantennadelay(global_ranging_config.prf) >> 1;
//if (antenna_delay == 0) {
// printf("resetting antenna delay\r\n");
// // If it's not in the OTP, use a magic value from instance_calib.c
// antenna_delay = ((DWT_PRF_64M_RFDLY/ 2.0) * 1e-9 / DWT_TIME_UNITS);
// dwt_setrxantennadelay(antenna_delay);
// dwt_settxantennadelay(antenna_delay);
//}
//global_tx_antenna_delay = antenna_delay;
//printf("tx antenna delay: %u\r\n", antenna_delay);
}
// // Set the sleep delay. Not sure what this does actually.
// instancesettagsleepdelay(POLL_SLEEP_DELAY, BLINK_SLEEP_DELAY);
// Configure as either a tag or anchor
if (HACK_role == ANCHOR) {
uint8_t eui_array[8];
// Enable frame filtering
dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN);
dw1000_populate_eui(eui_array, HACK_EUI);
dwt_seteui(eui_array);
dwt_setpanid(DW1000_PANID);
// We do want to enable auto RX
dwt_setautorxreenable(1);
// Let's do double buffering
dwt_setdblrxbuffmode(0);
// Disable RX timeout by setting to 0
dwt_setrxtimeout(0);
// Go for receiving
dwt_rxenable(0);
} else if (HACK_role == TAG) {
uint8_t eui_array[8];
// Allow data and ack frames
dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN);
dw1000_populate_eui(eui_array, HACK_EUI);
dwt_seteui(eui_array);
dwt_setpanid(DW1000_PANID);
// Do this for the tag too
dwt_setautorxreenable(1);
dwt_setdblrxbuffmode(1);
dwt_enableautoack(5 /*ACK_RESPONSE_TIME*/);
// Configure sleep
{
int mode = DWT_LOADUCODE |
DWT_PRESRV_SLEEP |
DWT_CONFIG |
DWT_TANDV;
if (dwt_getldotune() != 0) {
// If we need to use LDO tune value from OTP kick it after sleep
mode |= DWT_LOADLDO;
}
// NOTE: on the EVK1000 the DEEPSLEEP is not actually putting the
// DW1000 into full DEEPSLEEP mode as XTAL is kept on
dwt_configuresleep(mode, DWT_WAKE_CS | DWT_SLP_EN);
}
}
// Make it fast
REG(SSI0_BASE + SSI_CR1) = 0;
REG(SSI0_BASE + SSI_CPSR) = 2;
REG(SSI0_BASE + SSI_CR1) |= SSI_CR1_SSE;
return 0;
}
void cs_listener_run(void) {
uint8_t ts[8];
uint32 chirp_ts;
int64_t rcv_tag_ts = 0;
uint32 sync_rate = cph_config->sender_period;
uint8 functionCode = 0;
double diff_val = 0;
double diff_val_prev = 0;
double diff_val_var = 0;
double diff_ts =0;
double adjusted_ts = 0;
double relative_tag_ts = 0;
int64_t rcv_curr_ts = 0;
int64_t rcv_prev_ts = 0;
int64_t rcv_diff_ts = 0;
double rcv_diff = 0;
int64_t blink_curr_ts = 0;
int64_t blink_prev_ts = 0;
int64_t blink_diff_ts = 0;
double blink_diff = 0;
double rcv_blink_diff = 0;
uint32 rcv_interval;
uint32 elapsed;
uint8 count = 1;
irq_init();
pio_disable_interrupt(DW_IRQ_PIO, DW_IRQ_MASK);
cph_deca_init_device();
cph_deca_init_network(cph_config->panid, cph_config->shortid);
printf("Device ID: %08X\r\n", dwt_readdevid());
// Enable external sync
uint32_t ec_ctrl;
dwt_readfromdevice(EXT_SYNC_ID, EC_CTRL_OFFSET, 4, (uint8_t*) &ec_ctrl);
ec_ctrl &= EC_CTRL_WAIT_MASK; // clear WAIT field
ec_ctrl |= EC_CTRL_OSTRM | (33 << 3); // turn on OSTRM and put 33 in WAIT field
dwt_writetodevice(EXT_SYNC_ID, EC_CTRL_OFFSET, 4, (uint8_t*) &ec_ctrl);
dwt_setcallbacks(0, rxcallback);
chirp_ts = cph_get_millis();
dwt_setinterrupt( DWT_INT_TFRS | DWT_INT_RFCG | (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO /*| DWT_INT_RXPTO*/), 1);
pio_enable_interrupt(DW_IRQ_PIO, DW_IRQ_MASK);
dwt_rxenable(0);
while(1)
{
if(AWAKE_CHIRP) {
elapsed = cph_get_millis() - chirp_ts;
if(elapsed > CHIRP_PERIOD) {
TRACE("Awake %d\r\n", count);
count++;
chirp_ts = cph_get_millis();
}
}
if(irq_status == STATUS_RCV){
functionCode = ((cph_deca_msg_header_t*)rx_buffer)->functionCode;
switch(functionCode){
case FUNC_CS_SYNC:
dwt_readrxtimestamp(ts);
TRACE("end sys: ");
for (int i = 4; i >= 0; i--) {
TRACE("%02X", ts[i]);
}
TRACE("\r\n");
// // Difference in RX timestamps
// rcv_curr_ts = get_rx_timestamp();
// rcv_diff_ts = rcv_curr_ts - rcv_prev_ts;
// rcv_prev_ts = rcv_curr_ts;
// rcv_diff = (int64_t)rcv_diff_ts * DWT_TIME_UNITS * 1000;
//
// // Difference in TX timestamps
// blink_curr_ts = ((int64_t)((cph_deca_msg_blink_t*)rx_buffer)->blinkTxTs) << 8;
// blink_diff_ts = blink_curr_ts - blink_prev_ts;
// blink_prev_ts = blink_curr_ts;
// blink_diff = blink_diff_ts * DWT_TIME_UNITS * 1000;
//
// rcv_blink_diff = rcv_diff - blink_diff;
//
//// TRACE("TS0: %08X \t TS1: %08X \t V: %08X \t %+.08f ms\r\n", blink_ts_prev, blink_ts, blink_ts_var, blink_diff);
// TRACE("Bdiff: %+.08f ms \t Rdiff: %+.08f ms \t RBdiff: %+.08f ms\r\n", blink_diff, rcv_diff, rcv_blink_diff);
break;
case FUNC_CS_TAG:
rcv_tag_ts = get_rx_timestamp();
rcv_tag_ts = rcv_tag_ts - rcv_curr_ts;
relative_tag_ts = DWU_to_MS(rcv_tag_ts);
// TRACE("%02X : Raw Tag TS: %.08f ", ((cph_deca_msg_header_t*)rx_buffer)->seq, relative_tag_ts);
if(relative_tag_ts < 100) {
// relative_tag_ts = ((relative_tag_ts - diff_ts) - (int)(relative_tag_ts - diff_ts)) * 1000000; //Truncates leading value, and converts to ns
relative_tag_ts = ((relative_tag_ts - diff_ts) - (int)(relative_tag_ts - diff_ts)) * 1000; //Truncates leading value, and converts to μs
// relative_tag_ts = (relative_tag_ts - diff_ts); //Account for offset from source clock in ms
TRACE("%02X:%.08f\r\n", ((cph_deca_msg_header_t*)rx_buffer)->seq, relative_tag_ts);
}
break;
case FUNC_CS_COORD:
break;
default:
break;
}
irq_status = STATUS_EMPTY;
dwt_rxenable(0);
} else if (irq_status == STATUS_ERR) {
// TRACE("INVALID LENGTH: %d\r\n", frame_len);
irq_status = STATUS_EMPTY;
dwt_rxenable(0);
}
}
}
// First (generic) init of the DW1000
dw1000_err_e dw1000_init () {
// Do the STM setup that initializes pin and peripherals and whatnot.
if (!_stm_dw1000_interface_setup) {
setup();
}
// Reset the dw1000...for some reason
dw1000_reset();
uDelay(100);
// Make sure we can talk to the DW1000
uint32_t devID;
devID = dwt_readdevid();
if (devID != DWT_DEVICE_ID) {
//if we can't talk to dw1000, return with an error
uDelay(1000);
return DW1000_COMM_ERR;
}
GPIO_WriteBit(STM_GPIO3_PORT, STM_GPIO3_PIN, Bit_SET);
uDelay(1000);
GPIO_WriteBit(STM_GPIO3_PORT, STM_GPIO3_PIN, Bit_RESET);
// Choose antenna 0 as a default
dw1000_choose_antenna(0);
// Initialize the dw1000 hardware
uint32_t err;
err = dwt_initialise(DWT_LOADUCODE |
DWT_LOADLDO |
DWT_LOADTXCONFIG |
DWT_LOADXTALTRIM);
if (err != DWT_SUCCESS) {
return DW1000_COMM_ERR;
}
// Configure interrupts and callbacks
dwt_setinterrupt(0xFFFFFFFF, 0);
dwt_setinterrupt(DWT_INT_TFRS |
DWT_INT_RFCG |
DWT_INT_RPHE |
DWT_INT_RFCE |
DWT_INT_RFSL |
DWT_INT_RFTO |
DWT_INT_RXPTO |
DWT_INT_SFDT |
DWT_INT_ARFE, 1);
dwt_setcallbacks(txcallback, rxcallback);
// Set the parameters of ranging and channel and whatnot
global_ranging_config.chan = 2;
global_ranging_config.prf = DWT_PRF_64M;
global_ranging_config.txPreambLength = DWT_PLEN_64;
global_ranging_config.rxPAC = DWT_PAC8;
global_ranging_config.txCode = 9; // preamble code
global_ranging_config.rxCode = 9; // preamble code
global_ranging_config.nsSFD = 0;
global_ranging_config.dataRate = DWT_BR_6M8;
global_ranging_config.phrMode = DWT_PHRMODE_EXT; //Enable extended PHR mode (up to 1024-byte packets)
global_ranging_config.smartPowerEn = 1;
global_ranging_config.sfdTO = 64+8+1;//(1025 + 64 - 32);
#if DW1000_USE_OTP
dwt_configure(&global_ranging_config, (DWT_LOADANTDLY | DWT_LOADXTALTRIM));
#else
dwt_configure(&global_ranging_config, 0);
#endif
dwt_setsmarttxpower(global_ranging_config.smartPowerEn);
// Configure TX power based on the channel used
global_tx_config.PGdly = pgDelay[global_ranging_config.chan];
global_tx_config.power = txPower[global_ranging_config.chan];
dwt_configuretxrf(&global_tx_config);
// Need to set some radio properties. Ideally these would come from the
// OTP memory on the DW1000
#if DW1000_USE_OTP == 0
// This defaults to 8. Don't know why.
dwt_xtaltrim(8);
// Antenna delay we don't really care about so we just use 0
dwt_setrxantennadelay(DW1000_ANTENNA_DELAY_RX);
dwt_settxantennadelay(DW1000_ANTENNA_DELAY_TX);
#endif
return DW1000_NO_ERR;
}
void listener_run(void) {
uint32_t announce_coord_ts = 0;
uint32_t elapsed = 0;
uint32_t last_ts = 0;
uint32_t count = 0;
irq_init();
pio_disable_interrupt(DW_IRQ_PIO, DW_IRQ_MASK);
// Setup DW1000
dwt_txconfig_t txconfig;
// Setup DECAWAVE
reset_DW1000();
spi_set_rate_low();
dwt_initialise(DWT_LOADUCODE);
spi_set_rate_high();
dwt_configure(&cph_config->dwt_config);
dwt_setpanid(0x4350);
dwt_setaddress16(0x1234);
// Clear CLKPLL_LL
dwt_write32bitreg(SYS_STATUS_ID, 0x02000000);
uint32_t id = dwt_readdevid();
printf("Device ID: %08X\r\n", id);
#if 1
dwt_setcallbacks(0, rxcallback);
dwt_setinterrupt(
DWT_INT_TFRS | DWT_INT_RFCG
| (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO /*| DWT_INT_RXPTO*/),
1);
pio_enable_interrupt(DW_IRQ_PIO, DW_IRQ_MASK);
dwt_rxenable(0);
while (1) {
elapsed = cph_get_millis() - last_ts;
if (elapsed > 5000) {
printf("alive %d\r\n", count++);
last_ts = cph_get_millis();
}
if (trx_signal == SIGNAL_RCV) {
printf("[RCV] %d - ", frame_len);
for (int i = 0; i < frame_len; i++) {
printf("%02X ", rx_buffer[i]);
}
printf("\r\n");
trx_signal = SIGNAL_EMPTY;
dwt_rxenable(0);
}
else if(trx_signal == SIGNAL_ERR) {
printf("ERROR: %08X\r\n", error_status_reg);
trx_signal = SIGNAL_EMPTY;
dwt_rxenable(0);
}
else if(trx_signal == SIGNAL_ERR_LEN) {
printf("ERROR LENGTH: %08X\r\n", error_status_reg);
trx_signal = SIGNAL_EMPTY;
dwt_rxenable(0);
}
}
#else
while (1) {
/* Activate reception immediately. */
dwt_rxenable(0);
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))) {
};
if (status_reg & SYS_STATUS_RXFCG) {
uint32 frame_len;
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
if (frame_len <= MAXRXSIXZE) {
dwt_readrxdata(rx_buffer, frame_len, 0);
} else {
frame_len = 0;
}
if (frame_len > 0) {
printf("[RCV] ");
for (int i = 0; i < frame_len; i++) {
printf("%02X ", rx_buffer[i]);
}
printf("\r\n");
} else {
printf("ERROR: frame_len == %d\r\n", frame_len);
}
} else {
printf("ERROR: dwt_rxenable has status of %08X\r\n", status_reg);
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR | SYS_STATUS_CLKPLL_LL);
}
}
#endif
}
