amiro-lld / source / DW1000 / v1 / deca_instance_tag_anchor_v1.c @ 33f54213
History | View | Annotate | Download (43.5 KB)
| 1 |
/*! ----------------------------------------------------------------------------
|
|---|---|
| 2 |
* @file instance_tag.c
|
| 3 |
* @brief Decawave tag application state machine for TREK demo
|
| 4 |
*
|
| 5 |
* @attention
|
| 6 |
*
|
| 7 |
* Copyright 2016 (c) Decawave Ltd, Dublin, Ireland.
|
| 8 |
*
|
| 9 |
* All rights reserved.
|
| 10 |
*
|
| 11 |
* @author Decawave
|
| 12 |
*/
|
| 13 |
|
| 14 |
#include <alld_DW1000.h> |
| 15 |
|
| 16 |
#if (defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 1)) || defined(__DOXYGEN__) |
| 17 |
|
| 18 |
#include <v1/deca_instance_v1.h> |
| 19 |
#include <string.h> |
| 20 |
#include<math.h> |
| 21 |
#include "module.h" |
| 22 |
|
| 23 |
// -------------------------------------------------------------------------------------------------------------------
|
| 24 |
// Data Definitions
|
| 25 |
// -------------------------------------------------------------------------------------------------------------------
|
| 26 |
|
| 27 |
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
|
| 28 |
// NOTE: the maximum RX timeout is ~ 65ms
|
| 29 |
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
|
| 30 |
|
| 31 |
|
| 32 |
// -------------------------------------------------------------------------------------------------------------------
|
| 33 |
// Functions
|
| 34 |
// -------------------------------------------------------------------------------------------------------------------
|
| 35 |
|
| 36 |
// -------------------------------------------------------------------------------------------------------------------
|
| 37 |
//
|
| 38 |
// function to construct the message/frame header bytes
|
| 39 |
//
|
| 40 |
// -------------------------------------------------------------------------------------------------------------------
|
| 41 |
//
|
| 42 |
void instanceconfigframeheader16(instance_data_t *inst){
|
| 43 |
//set frame type (0-2), SEC (3), Pending (4), ACK (5), PanIDcomp(6)
|
| 44 |
inst->msg_f.frameCtrl[0] = 0x1 /*frame type 0x1 == data*/ | 0x40 /*PID comp*/; |
| 45 |
|
| 46 |
//source/dest addressing modes and frame version
|
| 47 |
inst->msg_f.frameCtrl[1] = 0x8 /*dest extended address (16bits)*/ | 0x80 /*src extended address (16bits)*/; |
| 48 |
|
| 49 |
inst->msg_f.panID[0] = (inst->panID) & 0xff; |
| 50 |
inst->msg_f.panID[1] = inst->panID >> 8; |
| 51 |
|
| 52 |
inst->msg_f.seqNum = 0;
|
| 53 |
} |
| 54 |
|
| 55 |
int instancesenddlypacket(instance_data_t *inst, int delayedTx){ |
| 56 |
int result = 0; |
| 57 |
|
| 58 |
dwt_writetxfctrl(inst->psduLength, 0, 1); |
| 59 |
if(delayedTx == DWT_START_TX_DELAYED){
|
| 60 |
dwt_setdelayedtrxtime(inst->delayedReplyTime) ; //should be high 32-bits of delayed TX TS
|
| 61 |
} |
| 62 |
|
| 63 |
//begin delayed TX of frame
|
| 64 |
if (dwt_starttx(delayedTx | inst->wait4ack)){ // delayed start was too late |
| 65 |
result = 1; //late/error |
| 66 |
inst->lateTX++; |
| 67 |
} |
| 68 |
else {
|
| 69 |
inst->timeofTx = portGetTickCnt(); |
| 70 |
inst->monitor = 1;
|
| 71 |
} |
| 72 |
return result; // state changes |
| 73 |
} |
| 74 |
|
| 75 |
|
| 76 |
int instance_calcranges(uint32_t *array, uint16_t size, int reportRange, uint8_t* mask){ |
| 77 |
int i;
|
| 78 |
int newRange = TOF_REPORT_NUL;
|
| 79 |
int distance = 0; |
| 80 |
|
| 81 |
for(i=0; i<size; i++) { |
| 82 |
uint32_t tofx = array[i]; |
| 83 |
if(tofx != INVALID_TOF) { //if ToF == 0 - then no new range to report |
| 84 |
distance = reportTOF(i, tofx); |
| 85 |
} |
| 86 |
|
| 87 |
if(distance == 1){ |
| 88 |
newRange = reportRange; |
| 89 |
} |
| 90 |
else {
|
| 91 |
//clear mask
|
| 92 |
*mask &= ~(0x1 << i) ;
|
| 93 |
clearDistTable(i); |
| 94 |
} |
| 95 |
array[i] = INVALID_TOF; |
| 96 |
|
| 97 |
distance = 0;
|
| 98 |
} |
| 99 |
|
| 100 |
return newRange;
|
| 101 |
} |
| 102 |
|
| 103 |
// -------------------------------------------------------------------------------------------------------------------
|
| 104 |
//
|
| 105 |
// the main instance state machine (all the instance modes Tag, Anchor or Listener use the same statemachine....)
|
| 106 |
//
|
| 107 |
// -------------------------------------------------------------------------------------------------------------------
|
| 108 |
//
|
| 109 |
int testapprun(instance_data_t *inst, int message){ |
| 110 |
|
| 111 |
switch (inst->testAppState){
|
| 112 |
case TA_INIT :
|
| 113 |
// printf("TA_INIT") ;
|
| 114 |
switch (inst->mode) {
|
| 115 |
case TAG: {
|
| 116 |
uint16_t sleep_mode = 0;
|
| 117 |
|
| 118 |
dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN); //allow data, ack frames;
|
| 119 |
dwt_setpanid(inst->panID); |
| 120 |
|
| 121 |
memcpy(inst->eui64, &inst->instanceAddress16, ADDR_BYTE_SIZE_S); |
| 122 |
dwt_seteui(inst->eui64); |
| 123 |
|
| 124 |
//set source address
|
| 125 |
inst->newRangeTagAddress = inst->instanceAddress16 ; |
| 126 |
dwt_setaddress16(inst->instanceAddress16); |
| 127 |
|
| 128 |
//Start off by Sleeping 1st -> set instToSleep to TRUE
|
| 129 |
inst->nextState = TA_TXPOLL_WAIT_SEND; |
| 130 |
inst->testAppState = TA_TXE_WAIT; |
| 131 |
inst->instToSleep = TRUE ; |
| 132 |
|
| 133 |
inst->rangeNum = 0;
|
| 134 |
inst->tagSleepCorrection = 0;
|
| 135 |
|
| 136 |
sleep_mode = (DWT_LOADUCODE|DWT_PRESRV_SLEEP|DWT_CONFIG|DWT_TANDV); |
| 137 |
|
| 138 |
/* TODO: No need anymore since the driver handle itself */
|
| 139 |
// if((dwt_getldotune() != 0)) //if we need to use LDO tune value from OTP kick it after sleep
|
| 140 |
// sleep_mode |= DWT_LOADOPSET;
|
| 141 |
|
| 142 |
if(inst->configData.txPreambLength == DWT_PLEN_64) //if using 64 length preamble then use the corresponding OPSet |
| 143 |
sleep_mode |= DWT_LOADOPSET; |
| 144 |
|
| 145 |
#if (DEEP_SLEEP == 1) |
| 146 |
dwt_configuresleep(sleep_mode, DWT_WAKE_WK|DWT_WAKE_CS|DWT_SLP_EN); //configure the on wake parameters (upload the IC config settings)
|
| 147 |
#endif
|
| 148 |
instanceconfigframeheader16(inst); |
| 149 |
inst->instanceWakeTime = portGetTickCnt(); |
| 150 |
} |
| 151 |
break;
|
| 152 |
case ANCHOR: {
|
| 153 |
memcpy(inst->eui64, &inst->instanceAddress16, ADDR_BYTE_SIZE_S); |
| 154 |
dwt_seteui(inst->eui64); |
| 155 |
|
| 156 |
dwt_setpanid(inst->panID); |
| 157 |
|
| 158 |
//set source address
|
| 159 |
inst->shortAdd_idx = (inst->instanceAddress16 & 0x3) ;
|
| 160 |
dwt_setaddress16(inst->instanceAddress16); |
| 161 |
|
| 162 |
//if address = 0x8000
|
| 163 |
if(inst->instanceAddress16 == GATEWAY_ANCHOR_ADDR){
|
| 164 |
inst->gatewayAnchor = TRUE; |
| 165 |
} |
| 166 |
|
| 167 |
dwt_enableframefilter(DWT_FF_NOTYPE_EN); //allow data, ack frames;
|
| 168 |
|
| 169 |
// First time anchor listens we don't do a delayed RX
|
| 170 |
dwt_setrxaftertxdelay(0);
|
| 171 |
//change to next state - wait to receive a message
|
| 172 |
inst->testAppState = TA_RXE_WAIT ; |
| 173 |
|
| 174 |
dwt_setrxtimeout(0);
|
| 175 |
dwt_setpreambledetecttimeout(0);
|
| 176 |
instanceconfigframeheader16(inst); |
| 177 |
|
| 178 |
} |
| 179 |
break;
|
| 180 |
case LISTENER:{
|
| 181 |
dwt_enableframefilter(DWT_FF_NOTYPE_EN); //disable frame filtering
|
| 182 |
dwt_setrxaftertxdelay(0); //no delay of turning on of RX |
| 183 |
dwt_setrxtimeout(0);
|
| 184 |
dwt_setpreambledetecttimeout(0);
|
| 185 |
//change to next state - wait to receive a message
|
| 186 |
inst->testAppState = TA_RXE_WAIT ; |
| 187 |
} |
| 188 |
break ; // end case TA_INIT |
| 189 |
default:
|
| 190 |
break;
|
| 191 |
} |
| 192 |
break; // end case TA_INIT |
| 193 |
|
| 194 |
case TA_SLEEP_DONE : {
|
| 195 |
event_data_t* dw_event = instance_getevent(10); //clear the event from the queue |
| 196 |
// waiting for timout from application to wakup IC
|
| 197 |
if (dw_event->type != DWT_SIG_RX_TIMEOUT){
|
| 198 |
// if no pause and no wake-up timeout continu waiting for the sleep to be done.
|
| 199 |
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //wait here for sleep timeout
|
| 200 |
break;
|
| 201 |
} |
| 202 |
|
| 203 |
inst->done = INST_NOT_DONE_YET; |
| 204 |
inst->instToSleep = FALSE ; |
| 205 |
inst->testAppState = inst->nextState; |
| 206 |
inst->nextState = 0; //clear |
| 207 |
inst->instanceWakeTime = portGetTickCnt(); // Record the time count when we wake-up
|
| 208 |
#if (DEEP_SLEEP == 1) |
| 209 |
{
|
| 210 |
uint32 x = 0;
|
| 211 |
|
| 212 |
//wake up device from low power mode
|
| 213 |
//NOTE - in the ARM code just drop chip select for 200us
|
| 214 |
led_on(LED_PC9); |
| 215 |
port_SPIx_clear_chip_select(); //CS low
|
| 216 |
instance_data[0].dwIDLE = 0; //reset DW1000 IDLE flag |
| 217 |
|
| 218 |
setup_DW1000RSTnIRQ(1); //enable RSTn IRQ |
| 219 |
|
| 220 |
Sleep(2); //200 us to wake up - need 2 as Sleep(1) is ~ 175 us |
| 221 |
// chThdSleepMilliseconds(2);
|
| 222 |
//then wait 5ms for DW1000 XTAL to stabilise - instead of wait we wait for RSTn to go high
|
| 223 |
//Sleep(5);
|
| 224 |
|
| 225 |
//need to poll to check when the DW1000 is in IDLE, the CPLL interrupt is not reliable
|
| 226 |
//when RSTn goes high the DW1000 is in INIT, it will enter IDLE after PLL lock (in 5 us)
|
| 227 |
while(instance_data[0].dwIDLE == 0) // this variable will be sent in the IRQ (process_dwRSTn_irq) |
| 228 |
{
|
| 229 |
//wait for DW1000 to go to IDLE state RSTn pin to go high
|
| 230 |
x++; |
| 231 |
} |
| 232 |
setup_DW1000RSTnIRQ(0); //disable RSTn IRQ |
| 233 |
port_SPIx_set_chip_select(); //CS high
|
| 234 |
|
| 235 |
//!!! NOTE it takes ~35us for the DW1000 to download AON and lock the PLL and be in IDLE state
|
| 236 |
//do some dummy reads of the dev ID register to make sure DW1000 is in IDLE before setting LEDs
|
| 237 |
x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz)
|
| 238 |
x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz)
|
| 239 |
x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz)
|
| 240 |
x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz)
|
| 241 |
|
| 242 |
x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz)
|
| 243 |
/*if(x != DWT_DEVICE_ID)
|
| 244 |
{
|
| 245 |
x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz)
|
| 246 |
}*/
|
| 247 |
led_off(LED_PC9); |
| 248 |
//this is platform dependent - only program if DW EVK/EVB
|
| 249 |
dwt_setleds(1);
|
| 250 |
|
| 251 |
//MP bug - TX antenna delay needs reprogramming as it is not preserved (only RX)
|
| 252 |
dwt_settxantennadelay(inst->txAntennaDelay) ; |
| 253 |
|
| 254 |
//set EUI as it will not be preserved unless the EUI is programmed and loaded from NVM
|
| 255 |
dwt_seteui(inst->eui64); |
| 256 |
} |
| 257 |
#else
|
| 258 |
Sleep(3); //to approximate match the time spent in the #if above //TODO: 3ms default |
| 259 |
#endif
|
| 260 |
|
| 261 |
instancesetantennadelays(); //this will update the antenna delay if it has changed
|
| 262 |
instancesettxpower(); //configure TX power if it has changed
|
| 263 |
|
| 264 |
} |
| 265 |
break;
|
| 266 |
|
| 267 |
case TA_TXE_WAIT : //either go to sleep or proceed to TX a message |
| 268 |
// printf("TA_TXE_WAIT") ;
|
| 269 |
//if we are scheduled to go to sleep before next transmission then sleep first.
|
| 270 |
if((inst->nextState == TA_TXPOLL_WAIT_SEND)
|
| 271 |
&& (inst->instToSleep) //go to sleep before sending the next poll/ starting new ranging exchange
|
| 272 |
){
|
| 273 |
inst->rangeNum++; //increment the range number before going to sleep
|
| 274 |
//the app should put chip into low power state and wake up after tagSleepTime_ms time...
|
| 275 |
//the app could go to *_IDLE state and wait for uP to wake it up...
|
| 276 |
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT_TO; //don't sleep here but kick off the Sleep timer countdown
|
| 277 |
inst->testAppState = TA_SLEEP_DONE; {
|
| 278 |
#if (DEEP_SLEEP == 1) |
| 279 |
//put device into low power mode
|
| 280 |
dwt_entersleep(); //go to sleep
|
| 281 |
#endif
|
| 282 |
//DW1000 gone to sleep - report the received range
|
| 283 |
inst->newRange = instance_calcranges(&inst->tofArray[0], MAX_ANCHOR_LIST_SIZE, TOF_REPORT_T2A, &inst->rxResponseMask);
|
| 284 |
inst->rxResponseMaskReport = inst->rxResponseMask; |
| 285 |
inst->rxResponseMask = 0;
|
| 286 |
inst->newRangeTime = portGetTickCnt() ; |
| 287 |
} |
| 288 |
} |
| 289 |
else { //proceed to configuration and transmission of a frame |
| 290 |
inst->testAppState = inst->nextState; |
| 291 |
inst->nextState = 0; //clear |
| 292 |
} |
| 293 |
break ; // end case TA_TXE_WAIT |
| 294 |
|
| 295 |
case TA_TXPOLL_WAIT_SEND : {
|
| 296 |
|
| 297 |
inst->msg_f.messageData[POLL_RNUM] = (inst->mode == TAG) ? inst->rangeNum : inst->rangeNumAnc; //copy new range number
|
| 298 |
inst->msg_f.messageData[FCODE] = (inst->mode == TAG) ? RTLS_DEMO_MSG_TAG_POLL : RTLS_DEMO_MSG_ANCH_POLL; //message function code (specifies if message is a poll, response or other...)
|
| 299 |
inst->psduLength = (TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC); |
| 300 |
inst->msg_f.seqNum = inst->frameSN++; //copy sequence number and then increment
|
| 301 |
inst->msg_f.sourceAddr[0] = inst->eui64[0]; //copy the address |
| 302 |
inst->msg_f.sourceAddr[1] = inst->eui64[1]; //copy the address |
| 303 |
inst->msg_f.destAddr[0] = 0xff; //set the destination address (broadcast == 0xffff) |
| 304 |
inst->msg_f.destAddr[1] = 0xff; //set the destination address (broadcast == 0xffff) |
| 305 |
dwt_writetxdata(inst->psduLength, (uint8_t *) &inst->msg_f, 0) ; // write the frame data |
| 306 |
|
| 307 |
//set the delayed rx on time (the response message will be sent after this delay (from A0))
|
| 308 |
dwt_setrxaftertxdelay((uint32_t)RX_RESPONSE1_TURNAROUND); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
|
| 309 |
|
| 310 |
if(inst->mode == TAG){
|
| 311 |
inst->rxResps[inst->rangeNum] = 0; //reset the number of received responses |
| 312 |
inst->responseTO = MAX_ANCHOR_LIST_SIZE; //expecting 4 responses
|
| 313 |
dwt_setrxtimeout((uint16_t)inst->fwtoTime_sy * MAX_ANCHOR_LIST_SIZE); //configure the RX FWTO
|
| 314 |
} |
| 315 |
else {
|
| 316 |
inst->rxResps[inst->rangeNumAnc] = 0; //reset number of responses |
| 317 |
inst->responseTO = NUM_EXPECTED_RESPONSES_ANC0; //2 responses A1, A2
|
| 318 |
dwt_setrxtimeout((uint16_t)inst->fwtoTime_sy * (NUM_EXPECTED_RESPONSES_ANC0)); //units are
|
| 319 |
} |
| 320 |
|
| 321 |
inst->rxResponseMask = 0; //reset/clear the mask of received responses when tx poll |
| 322 |
inst->rxResponseMaskAnc = 0;
|
| 323 |
|
| 324 |
inst->wait4ack = DWT_RESPONSE_EXPECTED; //response is expected - automatically enable the receiver
|
| 325 |
|
| 326 |
dwt_writetxfctrl(inst->psduLength, 0, 1); //write frame control |
| 327 |
|
| 328 |
dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED); //transmit the frame
|
| 329 |
|
| 330 |
inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation
|
| 331 |
inst->previousState = TA_TXPOLL_WAIT_SEND ; |
| 332 |
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set above)
|
| 333 |
} |
| 334 |
break;
|
| 335 |
|
| 336 |
case TA_TXFINAL_WAIT_SEND : {
|
| 337 |
//the final has the same range number as the poll (part of the same ranging exchange)
|
| 338 |
inst->msg_f.messageData[POLL_RNUM] = (inst->mode == TAG) ? inst->rangeNum : inst->rangeNumAnc; |
| 339 |
//the mask is sent so the anchors know whether the response RX time is valid
|
| 340 |
inst->msg_f.messageData[VRESP] = (inst->mode == TAG) ? inst->rxResponseMask : inst->rxResponseMaskAnc; |
| 341 |
inst->msg_f.messageData[FCODE] = (inst->mode == TAG) ? RTLS_DEMO_MSG_TAG_FINAL : RTLS_DEMO_MSG_ANCH_FINAL; //message function code (specifies if message is a poll, response or other...)
|
| 342 |
inst->psduLength = (TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC); |
| 343 |
inst->msg_f.seqNum = inst->frameSN++; |
| 344 |
dwt_writetxdata(inst->psduLength, (uint8_t *) &inst->msg_f, 0) ; // write the frame data |
| 345 |
|
| 346 |
inst->wait4ack = 0; //clear the flag not using wait for response as this message ends the ranging exchange |
| 347 |
|
| 348 |
if(instancesenddlypacket(inst, DWT_START_TX_DELAYED)) {
|
| 349 |
// initiate the re-transmission
|
| 350 |
if(inst->mode == TAG){
|
| 351 |
inst->testAppState = TA_TXE_WAIT ; //go to TA_TXE_WAIT first to check if it's sleep time
|
| 352 |
inst->nextState = TA_TXPOLL_WAIT_SEND ; |
| 353 |
} |
| 354 |
else {
|
| 355 |
//A0 - failed to send Final
|
| 356 |
//A1 - failed to send Final
|
| 357 |
//go back to RX and behave as anchor
|
| 358 |
instance_backtoanchor(inst); |
| 359 |
} |
| 360 |
break; //exit this switch case... |
| 361 |
} |
| 362 |
else {
|
| 363 |
inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
|
| 364 |
inst->previousState = TA_TXFINAL_WAIT_SEND; |
| 365 |
} |
| 366 |
if(inst->mode == TAG){
|
| 367 |
inst->instToSleep = TRUE ; |
| 368 |
} |
| 369 |
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set above)
|
| 370 |
} |
| 371 |
break;
|
| 372 |
|
| 373 |
case TA_TX_WAIT_CONF : {
|
| 374 |
//printf("TA_TX_WAIT_CONF %d m%d %d states %08x %08x\n", inst->previousState, message, inst->newReportSent, dwt_read32bitreg(0x19), dwt_read32bitreg(0x0f)) ;
|
| 375 |
|
| 376 |
event_data_t* dw_event = instance_getevent(11); //get and clear this event |
| 377 |
|
| 378 |
//NOTE: Can get the ACK before the TX confirm event for the frame requesting the ACK
|
| 379 |
//this happens because if polling the ISR the RX event will be processed 1st and then the TX event
|
| 380 |
//thus the reception of the ACK will be processed before the TX confirmation of the frame that requested it.
|
| 381 |
if(dw_event->type != DWT_SIG_TX_DONE) { //wait for TX done confirmation |
| 382 |
if(dw_event->type != 0) { |
| 383 |
if(dw_event->type == DWT_SIG_RX_TIMEOUT){ //got RX timeout - i.e. did not get the response (e.g. ACK) |
| 384 |
//printf("RX timeout in TA_TX_WAIT_CONF (%d)\n", inst->previousState);
|
| 385 |
//we need to wait for SIG_TX_DONE and then process the timeout and re-send the frame if needed
|
| 386 |
inst->gotTO = 1;
|
| 387 |
} |
| 388 |
else{
|
| 389 |
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; |
| 390 |
} |
| 391 |
} |
| 392 |
|
| 393 |
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; |
| 394 |
break;
|
| 395 |
|
| 396 |
} |
| 397 |
|
| 398 |
inst->done = INST_NOT_DONE_YET; |
| 399 |
|
| 400 |
if(inst->previousState == TA_TXFINAL_WAIT_SEND) {
|
| 401 |
if(inst->mode == TAG){
|
| 402 |
inst->testAppState = TA_TXE_WAIT ; |
| 403 |
inst->nextState = TA_TXPOLL_WAIT_SEND ; |
| 404 |
break;
|
| 405 |
} |
| 406 |
else{
|
| 407 |
instance_backtoanchor(inst); |
| 408 |
} |
| 409 |
} |
| 410 |
else if (inst->gotTO == 1) { //timeout |
| 411 |
//printf("got TO in TA_TX_WAIT_CONF\n");
|
| 412 |
inst_processrxtimeout(inst); |
| 413 |
inst->gotTO = 0;
|
| 414 |
inst->wait4ack = 0 ; //clear this |
| 415 |
break;
|
| 416 |
} |
| 417 |
else{
|
| 418 |
inst->txu.txTimeStamp = dw_event->timeStamp; |
| 419 |
|
| 420 |
if(inst->previousState == TA_TXPOLL_WAIT_SEND){
|
| 421 |
uint64_t tagCalculatedFinalTxTime ; |
| 422 |
// Embed into Final message: 40-bit pollTXTime, 40-bit respRxTime, 40-bit finalTxTime
|
| 423 |
if(inst->mode == TAG){
|
| 424 |
tagCalculatedFinalTxTime = (inst->txu.txTimeStamp + inst->pollTx2FinalTxDelay) & MASK_TXDTS; |
| 425 |
} |
| 426 |
else { //for anchor make the final half the delay ..... (this is ok, as A0 awaits 2 responses) |
| 427 |
tagCalculatedFinalTxTime = (inst->txu.txTimeStamp + inst->pollTx2FinalTxDelayAnc) & MASK_TXDTS; |
| 428 |
} |
| 429 |
inst->delayedReplyTime = tagCalculatedFinalTxTime >> 8; //high 32-bits |
| 430 |
// Calculate Time Final message will be sent and write this field of Final message
|
| 431 |
// Sending time will be delayedReplyTime, snapped to ~125MHz or ~250MHz boundary by
|
| 432 |
// zeroing its low 9 bits, and then having the TX antenna delay added
|
| 433 |
// getting antenna delay from the device and add it to the Calculated TX Time
|
| 434 |
tagCalculatedFinalTxTime = tagCalculatedFinalTxTime + inst->txAntennaDelay; |
| 435 |
tagCalculatedFinalTxTime &= MASK_40BIT; |
| 436 |
|
| 437 |
// Write Calculated TX time field of Final message
|
| 438 |
memcpy(&(inst->msg_f.messageData[FTXT]), (uint8_t *)&tagCalculatedFinalTxTime, 5);
|
| 439 |
// Write Poll TX time field of Final message
|
| 440 |
memcpy(&(inst->msg_f.messageData[PTXT]), (uint8_t *)&inst->txu.tagPollTxTime, 5);
|
| 441 |
|
| 442 |
//change the w4r for the second and remaining anchors to 50 us
|
| 443 |
//dwt_setrxaftertxdelay((uint32)RX_RESPONSEX_TURNAROUND); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
|
| 444 |
} |
| 445 |
|
| 446 |
if(inst->previousState == TA_TXRESPONSE_SENT_TORX) {
|
| 447 |
inst->previousState = TA_TXRESPONSE_WAIT_SEND ; |
| 448 |
} |
| 449 |
inst->testAppState = TA_RXE_WAIT ; // After sending, tag expects response/report, anchor waits to receive a final/new poll
|
| 450 |
|
| 451 |
message = 0;
|
| 452 |
//fall into the next case (turn on the RX)
|
| 453 |
} |
| 454 |
|
| 455 |
} |
| 456 |
// TODO: fall through to the next case statement in case of debugging
|
| 457 |
break ; // end case TA_TX_WAIT_CONF |
| 458 |
|
| 459 |
|
| 460 |
case TA_RXE_WAIT : {
|
| 461 |
// printf("TA_RXE_WAIT") ;
|
| 462 |
if(inst->wait4ack == 0) { //if this is set the RX will turn on automatically after TX |
| 463 |
//turn RX on
|
| 464 |
dwt_rxenable(DWT_START_RX_IMMEDIATE) ; // turn RX on, without delay
|
| 465 |
} |
| 466 |
else{
|
| 467 |
inst->wait4ack = 0 ; //clear the flag, the next time we want to turn the RX on it might not be auto |
| 468 |
} |
| 469 |
|
| 470 |
if (inst->mode != LISTENER){
|
| 471 |
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //using RX FWTO
|
| 472 |
} |
| 473 |
|
| 474 |
inst->testAppState = TA_RX_WAIT_DATA; // let this state handle it
|
| 475 |
|
| 476 |
// end case TA_RXE_WAIT, don't break, but fall through into the TA_RX_WAIT_DATA state to process it immediately.
|
| 477 |
if(message == 0) break; |
| 478 |
} |
| 479 |
// TODO: fall through to the next case statement in case of debugging
|
| 480 |
break;
|
| 481 |
|
| 482 |
case TA_RX_WAIT_DATA : // Wait RX data |
| 483 |
//printf("TA_RX_WAIT_DATA %d", message) ;
|
| 484 |
switch (message){
|
| 485 |
|
| 486 |
//if we have received a DWT_SIG_RX_OKAY event - this means that the message is IEEE data type - need to check frame control to know which addressing mode is used
|
| 487 |
case DWT_SIG_RX_OKAY : {
|
| 488 |
event_data_t* dw_event = instance_getevent(15); //get and clear this event |
| 489 |
uint8_t srcAddr[8] = {0,0,0,0,0,0,0,0}; |
| 490 |
uint8_t dstAddr[8] = {0,0,0,0,0,0,0,0}; |
| 491 |
int fcode = 0; |
| 492 |
int fn_code = 0; |
| 493 |
//int srclen = 0;
|
| 494 |
//int fctrladdr_len;
|
| 495 |
uint8_t tof_idx = 0;
|
| 496 |
uint8_t *messageData; |
| 497 |
|
| 498 |
inst->stopTimer = 0; //clear the flag, as we have received a message |
| 499 |
|
| 500 |
// handle 16 and 64 bit source and destination addresses
|
| 501 |
switch(dw_event->msgu.frame[1] & 0xCC){ |
| 502 |
case 0xCC: // |
| 503 |
memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ll.sourceAddr[0]), ADDR_BYTE_SIZE_L); |
| 504 |
memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_ll.destAddr[0]), ADDR_BYTE_SIZE_L); |
| 505 |
fn_code = dw_event->msgu.rxmsg_ll.messageData[FCODE]; |
| 506 |
messageData = &dw_event->msgu.rxmsg_ll.messageData[0];
|
| 507 |
//srclen = ADDR_BYTE_SIZE_L;
|
| 508 |
//fctrladdr_len = FRAME_CRTL_AND_ADDRESS_L;
|
| 509 |
break;
|
| 510 |
case 0xC8: // |
| 511 |
memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_sl.sourceAddr[0]), ADDR_BYTE_SIZE_L); |
| 512 |
memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_sl.destAddr[0]), ADDR_BYTE_SIZE_S); |
| 513 |
fn_code = dw_event->msgu.rxmsg_sl.messageData[FCODE]; |
| 514 |
messageData = &dw_event->msgu.rxmsg_sl.messageData[0];
|
| 515 |
//srclen = ADDR_BYTE_SIZE_L;
|
| 516 |
//fctrladdr_len = FRAME_CRTL_AND_ADDRESS_LS;
|
| 517 |
break;
|
| 518 |
case 0x8C: // |
| 519 |
memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ls.sourceAddr[0]), ADDR_BYTE_SIZE_S); |
| 520 |
memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_ls.destAddr[0]), ADDR_BYTE_SIZE_L); |
| 521 |
fn_code = dw_event->msgu.rxmsg_ls.messageData[FCODE]; |
| 522 |
messageData = &dw_event->msgu.rxmsg_ls.messageData[0];
|
| 523 |
//srclen = ADDR_BYTE_SIZE_S;
|
| 524 |
//fctrladdr_len = FRAME_CRTL_AND_ADDRESS_LS;
|
| 525 |
break;
|
| 526 |
case 0x88: // |
| 527 |
memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ss.sourceAddr[0]), ADDR_BYTE_SIZE_S); |
| 528 |
memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_ss.destAddr[0]), ADDR_BYTE_SIZE_S); |
| 529 |
fn_code = dw_event->msgu.rxmsg_ss.messageData[FCODE]; |
| 530 |
messageData = &dw_event->msgu.rxmsg_ss.messageData[0];
|
| 531 |
//srclen = ADDR_BYTE_SIZE_S;
|
| 532 |
//fctrladdr_len = FRAME_CRTL_AND_ADDRESS_S;
|
| 533 |
break;
|
| 534 |
} |
| 535 |
|
| 536 |
if((inst->instToSleep == FALSE) && (inst->mode == LISTENER)){ //update received data, and go back to receiving frames |
| 537 |
//do something with message data (e.g. could extract any ToFs and print them)
|
| 538 |
inst->testAppState = TA_RXE_WAIT ; // wait for next frame
|
| 539 |
dwt_setrxaftertxdelay(0);
|
| 540 |
} |
| 541 |
else{
|
| 542 |
//process ranging messages
|
| 543 |
fcode = fn_code; |
| 544 |
tof_idx = srcAddr[0] & 0x3 ; |
| 545 |
|
| 546 |
switch(fcode){
|
| 547 |
|
| 548 |
case RTLS_DEMO_MSG_ANCH_POLL:
|
| 549 |
case RTLS_DEMO_MSG_TAG_POLL: {
|
| 550 |
inst->tagPollRxTime = dw_event->timeStamp ; //save Poll's Rx time
|
| 551 |
if(fcode == RTLS_DEMO_MSG_TAG_POLL){ //got poll from Tag |
| 552 |
inst->rangeNumA[srcAddr[0]&0x7] = messageData[POLL_RNUM]; //when anchor receives a poll, we need to remember the new range number |
| 553 |
} |
| 554 |
else{ //got poll from Anchor (initiator) |
| 555 |
inst->rangeNumAAnc[tof_idx] = messageData[POLL_RNUM]; //when anchor receives poll from another anchor - save the range number
|
| 556 |
} |
| 557 |
|
| 558 |
if (A1_ANCHOR_ADDR == inst->instanceAddress16) { //this is A1 |
| 559 |
|
| 560 |
if(GATEWAY_ANCHOR_ADDR == (srcAddr[0] | ((uint32_t)(srcAddr[1] << 8)))) { //poll is from A0 |
| 561 |
|
| 562 |
//configure the time A1 will poll A2 (it should be in half slot time from now)
|
| 563 |
inst->a1SlotTime = dw_event->uTimeStamp + (inst->slotPeriod); |
| 564 |
|
| 565 |
//inst->instanceTimerEn = 1; - THIS IS ENABLED BELOW AFTER FINAL
|
| 566 |
// - means that if final is not received then A1 will not range to A2
|
| 567 |
} |
| 568 |
} |
| 569 |
|
| 570 |
//the response has been sent - await TX done event
|
| 571 |
if(dw_event->type_pend == DWT_SIG_TX_PENDING){
|
| 572 |
inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
|
| 573 |
inst->previousState = TA_TXRESPONSE_SENT_POLLRX ; //wait for TX confirmation of sent response
|
| 574 |
} |
| 575 |
//already re-enabled the receiver
|
| 576 |
else if (dw_event->type_pend == DWT_SIG_RX_PENDING){ |
| 577 |
//stay in RX wait for next frame...
|
| 578 |
//RX is already enabled...
|
| 579 |
inst->testAppState = TA_RX_WAIT_DATA ; // wait for next frame
|
| 580 |
} |
| 581 |
else{ //the DW1000 is idle (re-enable from the application level) |
| 582 |
//stay in RX wait for next frame...
|
| 583 |
inst->testAppState = TA_RXE_WAIT ; // wait for next frame
|
| 584 |
} |
| 585 |
|
| 586 |
|
| 587 |
} |
| 588 |
break; //RTLS_DEMO_MSG_TAG_POLL |
| 589 |
|
| 590 |
case RTLS_DEMO_MSG_ANCH_RESP2:
|
| 591 |
case RTLS_DEMO_MSG_ANCH_RESP:{
|
| 592 |
uint8_t currentRangeNum = (messageData[TOFRN] + 1); //current = previous + 1 |
| 593 |
|
| 594 |
if(GATEWAY_ANCHOR_ADDR == (srcAddr[0] | ((uint32_t)(srcAddr[1] << 8)))){ //if response from gateway then use the correction factor |
| 595 |
if(inst->mode == TAG){
|
| 596 |
// casting received bytes to int because this is a signed correction -0.5 periods to +1.5 periods
|
| 597 |
inst->tagSleepCorrection = (int16_t) (((uint16_t) messageData[RES_TAG_SLP1] << 8) + messageData[RES_TAG_SLP0]);
|
| 598 |
inst->tagSleepRnd = 0; // once we have initial response from Anchor #0 the slot correction acts and we don't need this anymore |
| 599 |
} |
| 600 |
} |
| 601 |
|
| 602 |
//the response has been sent - await TX done event
|
| 603 |
if(dw_event->type_pend == DWT_SIG_TX_PENDING) { //anchor received response from anchor ID - 1 so is sending it's response now back to tag |
| 604 |
inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
|
| 605 |
inst->previousState = TA_TXRESPONSE_SENT_RESPRX ; //wait for TX confirmation of sent response
|
| 606 |
} |
| 607 |
//already re-enabled the receiver
|
| 608 |
else if(dw_event->type_pend == DWT_SIG_RX_PENDING) { |
| 609 |
// stay in TA_RX_WAIT_DATA - receiver is already enabled.
|
| 610 |
} |
| 611 |
//DW1000 idle - send the final
|
| 612 |
else { //if(dw_event->type_pend == DWT_SIG_DW_IDLE) |
| 613 |
|
| 614 |
if(((TAG == inst->mode) && (inst->rxResponseMask & 0x1)) //if A0's response received send the final |
| 615 |
|| ((A1_ANCHOR_ADDR == inst->instanceAddress16) && (inst->rxResponseMaskAnc & 0x4))
|
| 616 |
|| ((GATEWAY_ANCHOR_ADDR == inst->instanceAddress16) && (inst->rxResponseMaskAnc & 0x2)) ) { //if A1's response received |
| 617 |
|
| 618 |
inst->testAppState = TA_TXFINAL_WAIT_SEND ; // send our response / the final
|
| 619 |
} |
| 620 |
else { //go to sleep |
| 621 |
|
| 622 |
if(TAG == inst->mode){
|
| 623 |
inst->testAppState = TA_TXE_WAIT ; //go to TA_TXE_WAIT first to check if it's sleep time
|
| 624 |
inst->nextState = TA_TXPOLL_WAIT_SEND ; |
| 625 |
inst->instToSleep = TRUE; |
| 626 |
} |
| 627 |
else {
|
| 628 |
instance_backtoanchor(inst); |
| 629 |
} |
| 630 |
} |
| 631 |
} |
| 632 |
/*else
|
| 633 |
{
|
| 634 |
//stay in RX wait for next frame...
|
| 635 |
inst->testAppState = TA_RXE_WAIT ; // wait for next frame
|
| 636 |
}*/
|
| 637 |
|
| 638 |
if(fcode == RTLS_DEMO_MSG_ANCH_RESP) { //tag to anchor mode |
| 639 |
if(currentRangeNum == inst->rangeNum) { //these are the previous ranges... |
| 640 |
//copy the ToF and put into array (array holds last 4 ToFs)
|
| 641 |
memcpy(&inst->tofArray[(srcAddr[0]&0x3)], &(messageData[TOFR]), 4); |
| 642 |
|
| 643 |
//check if the ToF is valid, this makes sure we only report valid ToFs
|
| 644 |
//e.g. consider the case of reception of response from anchor a1 (we are anchor a2)
|
| 645 |
//if a1 got a Poll with previous Range number but got no Final, then the response will have
|
| 646 |
//the correct range number but the range will be INVALID_TOF
|
| 647 |
if(inst->tofArray[(srcAddr[0]&0x3)] != INVALID_TOF){ |
| 648 |
inst->rxResponseMask |= (0x1 << (srcAddr[0]&0x3)); |
| 649 |
} |
| 650 |
|
| 651 |
} |
| 652 |
else {
|
| 653 |
if(inst->tofArray[(srcAddr[0]&0x3)] != INVALID_TOF) { |
| 654 |
inst->tofArray[(srcAddr[0]&0x3)] = INVALID_TOF; |
| 655 |
} |
| 656 |
} |
| 657 |
|
| 658 |
|
| 659 |
} |
| 660 |
else { //anchor to anchor (only gateway processes anchor to anchor ToFs) |
| 661 |
//report the correct set of ranges (ranges from anchors A1, A2 need to match owns range number)
|
| 662 |
if((inst->gatewayAnchor)&&(currentRangeNum == inst->rangeNumAnc)) { //these are the previous ranges... |
| 663 |
inst->rangeNumAAnc[0] = inst->rangeNumAnc ;
|
| 664 |
|
| 665 |
//once A0 receives A2's response then it can report the 3 ToFs.
|
| 666 |
if(inst->rxResps[inst->rangeNumAnc] == 3) |
| 667 |
//if(A2_ANCHOR_ADDR == (srcAddr[0] | ((uint32)(srcAddr[1] << 8))))
|
| 668 |
{
|
| 669 |
//copy the ToF and put into array, the array should have 3 ToFs A0-A1, A0-A2 and A1-A2
|
| 670 |
memcpy(&inst->tofArrayAnc[(srcAddr[0]+dstAddr[0])&0x3], &(messageData[TOFR]), 4); |
| 671 |
//calculate all anchor - anchor ranges... and report
|
| 672 |
inst->newRange = instance_calcranges(&inst->tofArrayAnc[0], MAX_ANCHOR_LIST_SIZE, TOF_REPORT_A2A, &inst->rxResponseMaskAnc);
|
| 673 |
inst->rxResponseMaskReport = inst->rxResponseMaskAnc; |
| 674 |
inst->rxResponseMaskAnc = 0;
|
| 675 |
inst->newRangeTime = dw_event->uTimeStamp ; |
| 676 |
} |
| 677 |
else {
|
| 678 |
//copy the ToF and put into array (array holds last 4 ToFs)
|
| 679 |
memcpy(&inst->tofArrayAnc[(srcAddr[0]+dstAddr[0])&0x3], &(messageData[TOFR]), 4); |
| 680 |
} |
| 681 |
} |
| 682 |
} |
| 683 |
|
| 684 |
} |
| 685 |
break; //RTLS_DEMO_MSG_ANCH_RESP |
| 686 |
|
| 687 |
|
| 688 |
case RTLS_DEMO_MSG_ANCH_FINAL:
|
| 689 |
case RTLS_DEMO_MSG_TAG_FINAL: {
|
| 690 |
int64_t Rb, Da, Ra, Db ; |
| 691 |
uint64_t tagFinalTxTime = 0;
|
| 692 |
uint64_t tagFinalRxTime = 0;
|
| 693 |
uint64_t tagPollTxTime = 0;
|
| 694 |
uint64_t anchorRespRxTime = 0;
|
| 695 |
uint64_t tof = INVALID_TOF; |
| 696 |
|
| 697 |
double RaRbxDaDb = 0; |
| 698 |
double RbyDb = 0; |
| 699 |
double RayDa = 0; |
| 700 |
|
| 701 |
uint8_t validResp = messageData[VRESP]; |
| 702 |
uint8_t index = RRXT0 + 5*(inst->shortAdd_idx);
|
| 703 |
|
| 704 |
if((RTLS_DEMO_MSG_TAG_FINAL == fcode) &&
|
| 705 |
(inst->rangeNumA[srcAddr[0]&0x7] != messageData[POLL_RNUM])) { //Final's range number needs to match Poll's or else discard this message |
| 706 |
inst->testAppState = TA_RXE_WAIT ; // wait for next frame
|
| 707 |
break;
|
| 708 |
} |
| 709 |
|
| 710 |
if((RTLS_DEMO_MSG_ANCH_FINAL == fcode) &&
|
| 711 |
(((inst->rangeNumAAnc[tof_idx] != messageData[POLL_RNUM]) //Final's range number needs to match Poll's or else discard this message
|
| 712 |
|| inst->gatewayAnchor) //gateway can ignore the Final (from A1 to A2 exchange)
|
| 713 |
|| (A3_ANCHOR_ADDR == inst->instanceAddress16))) //A3 does not care about Final from A1 or A0
|
| 714 |
{
|
| 715 |
inst->testAppState = TA_RXE_WAIT ; // wait for next frame
|
| 716 |
break;
|
| 717 |
} |
| 718 |
|
| 719 |
if (A1_ANCHOR_ADDR == inst->instanceAddress16) { //this is A1 |
| 720 |
if(GATEWAY_ANCHOR_ADDR == (srcAddr[0] | ((uint32_t)(srcAddr[1] << 8)))) { //final is from A0 |
| 721 |
//ENABLE TIMER ONLY IF FINAL RECEIVED
|
| 722 |
inst->instanceTimerEn = 1;
|
| 723 |
} |
| 724 |
} |
| 725 |
//output data over USB...
|
| 726 |
inst->newRangeAncAddress = inst->instanceAddress16; |
| 727 |
|
| 728 |
//if we got the final, maybe the tag did not get our response, so
|
| 729 |
//we can use other anchors responses/ToF if there are any.. and output..
|
| 730 |
//but we cannot calculate new range
|
| 731 |
if(((validResp & (0x1<<(inst->shortAdd_idx))) != 0)) { |
| 732 |
// time of arrival of Final message
|
| 733 |
tagFinalRxTime = dw_event->timeStamp ; //Final's Rx time
|
| 734 |
|
| 735 |
//printf("FinalRx Timestamp: %4.15e\n", convertdevicetimetosecu(dw_event.timeStamp));
|
| 736 |
inst->delayedReplyTime = 0 ;
|
| 737 |
|
| 738 |
// times measured at Tag extracted from the message buffer
|
| 739 |
// extract 40bit times
|
| 740 |
memcpy(&tagPollTxTime, &(messageData[PTXT]), 5);
|
| 741 |
memcpy(&anchorRespRxTime, &(messageData[index]), 5);
|
| 742 |
memcpy(&tagFinalTxTime, &(messageData[FTXT]), 5);
|
| 743 |
|
| 744 |
// poll response round trip delay time is calculated as
|
| 745 |
// (anchorRespRxTime - tagPollTxTime) - (anchorRespTxTime - tagPollRxTime)
|
| 746 |
Ra = (int64_t)((anchorRespRxTime - tagPollTxTime) & MASK_40BIT); |
| 747 |
Db = (int64_t)((inst->txu.anchorRespTxTime - inst->tagPollRxTime) & MASK_40BIT); |
| 748 |
|
| 749 |
// response final round trip delay time is calculated as
|
| 750 |
// (tagFinalRxTime - anchorRespTxTime) - (tagFinalTxTime - anchorRespRxTime)
|
| 751 |
Rb = (int64_t)((tagFinalRxTime - inst->txu.anchorRespTxTime) & MASK_40BIT); |
| 752 |
Da = (int64_t)((tagFinalTxTime - anchorRespRxTime) & MASK_40BIT); |
| 753 |
|
| 754 |
RaRbxDaDb = (((double)Ra))*(((double)Rb)) |
| 755 |
- (((double)Da))*(((double)Db)); |
| 756 |
|
| 757 |
RbyDb = ((double)Rb + (double)Db); |
| 758 |
|
| 759 |
RayDa = ((double)Ra + (double)Da); |
| 760 |
|
| 761 |
tof = (int32_t) ( RaRbxDaDb/(RbyDb + RayDa) ); |
| 762 |
} |
| 763 |
|
| 764 |
//tag to anchor ranging
|
| 765 |
if(RTLS_DEMO_MSG_TAG_FINAL == fcode) {
|
| 766 |
inst->newRangeTagAddress = srcAddr[0] + ((uint16_t) srcAddr[1] << 8); |
| 767 |
//time-of-flight
|
| 768 |
inst->tof[inst->newRangeTagAddress & 0x7] = tof;
|
| 769 |
//calculate all tag - anchor ranges... and report
|
| 770 |
inst->newRange = instance_calcranges(&inst->tofArray[0], MAX_ANCHOR_LIST_SIZE, TOF_REPORT_T2A, &inst->rxResponseMask);
|
| 771 |
inst->rxResponseMaskReport = inst->rxResponseMask; //copy the valid mask to report
|
| 772 |
inst->rxResponseMask = 0;
|
| 773 |
//we have our range - update the own mask entry...
|
| 774 |
if(tof != INVALID_TOF) { //check the last ToF entry is valid and copy into the current array |
| 775 |
setTagDist(srcAddr[0], inst->shortAdd_idx); //copy distance from this anchor to the tag into array |
| 776 |
|
| 777 |
inst->rxResponseMask = (0x1 << inst->shortAdd_idx);
|
| 778 |
inst->tofArray[inst->shortAdd_idx] = tof; |
| 779 |
} |
| 780 |
inst->newRangeTime = dw_event->uTimeStamp ; |
| 781 |
} |
| 782 |
else { //anchor to anchor ranging |
| 783 |
inst->newRangeTagAddress = srcAddr[0] + ((uint16_t) srcAddr[1] << 8); |
| 784 |
//time-of-flight
|
| 785 |
inst->tofAnc[tof_idx] = tof; |
| 786 |
} |
| 787 |
|
| 788 |
//reset the response count
|
| 789 |
if(inst->rxResps[inst->rxRespsIdx] >= 0) { |
| 790 |
inst->rxResps[inst->rxRespsIdx] = -1 * inst->rxResps[inst->rxRespsIdx];
|
| 791 |
if(inst->rxResps[inst->rxRespsIdx] == 0) //as A0 will have this as 0 when ranging to A1 |
| 792 |
inst->rxResps[inst->rxRespsIdx] = -1 ;
|
| 793 |
} |
| 794 |
|
| 795 |
instancesetantennadelays(); //this will update the antenna delay if it has changed
|
| 796 |
instancesettxpower(); // configure TX power if it has changed
|
| 797 |
|
| 798 |
inst->testAppState = TA_RXE_WAIT ; // wait for next frame
|
| 799 |
|
| 800 |
} |
| 801 |
break; //RTLS_DEMO_MSG_TAG_FINAL |
| 802 |
|
| 803 |
|
| 804 |
default: {
|
| 805 |
//only enable receiver when not using double buffering
|
| 806 |
inst->testAppState = TA_RXE_WAIT ; // wait for next frame
|
| 807 |
dwt_setrxaftertxdelay(0);
|
| 808 |
|
| 809 |
} |
| 810 |
break;
|
| 811 |
} //end switch (fcode)
|
| 812 |
|
| 813 |
if(dw_event->msgu.frame[0] & 0x20){ |
| 814 |
//as we only pass the received frame with the ACK request bit set after the ACK has been sent
|
| 815 |
instance_getevent(16); //get and clear the ACK sent event |
| 816 |
} |
| 817 |
} //end else
|
| 818 |
|
| 819 |
} |
| 820 |
break ; //end of DWT_SIG_RX_OKAY |
| 821 |
|
| 822 |
case DWT_SIG_RX_TIMEOUT :{
|
| 823 |
|
| 824 |
event_data_t* dw_event = instance_getevent(17); //get and clear this event |
| 825 |
|
| 826 |
//printf("PD_DATA_TIMEOUT %d\n", inst->previousState) ;
|
| 827 |
|
| 828 |
//Anchor can time out and then need to send response - so will be in TX pending
|
| 829 |
if(dw_event->type_pend == DWT_SIG_TX_PENDING) {
|
| 830 |
inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
|
| 831 |
inst->previousState = TA_TXRESPONSE_SENT_TORX ; //wait for TX confirmation of sent response
|
| 832 |
} |
| 833 |
else if(dw_event->type_pend == DWT_SIG_DW_IDLE) { //if timed out and back in receive then don't process as timeout |
| 834 |
inst_processrxtimeout(inst); |
| 835 |
} |
| 836 |
//else if RX_PENDING then wait for next RX event...
|
| 837 |
message = 0; //clear the message as we have processed the event |
| 838 |
} |
| 839 |
break ;
|
| 840 |
|
| 841 |
case DWT_SIG_TX_AA_DONE: //ignore this event - just process the rx frame that was received before the ACK response |
| 842 |
case 0: |
| 843 |
default :{
|
| 844 |
if(message) { // == DWT_SIG_TX_DONE) |
| 845 |
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; |
| 846 |
} |
| 847 |
|
| 848 |
if(inst->done == INST_NOT_DONE_YET) inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT;
|
| 849 |
} |
| 850 |
break;
|
| 851 |
|
| 852 |
} |
| 853 |
break ; // end case TA_RX_WAIT_DATA |
| 854 |
default:
|
| 855 |
//printf("\nERROR - invalid state %d - what is going on??\n", inst->testAppState) ;
|
| 856 |
break;
|
| 857 |
} // end switch on testAppState
|
| 858 |
|
| 859 |
return inst->done;
|
| 860 |
} // end testapprun()
|
| 861 |
|
| 862 |
// -------------------------------------------------------------------------------------------------------------------
|
| 863 |
#if NUM_INST != 1 |
| 864 |
#error These functions assume one instance only
|
| 865 |
#else
|
| 866 |
|
| 867 |
|
| 868 |
// -------------------------------------------------------------------------------------------------------------------
|
| 869 |
// function to set the fixed reply delay time (in us)
|
| 870 |
//
|
| 871 |
// This sets delay for RX to TX - Delayed Send, and for TX to RX delayed receive (wait for response) functionality,
|
| 872 |
// and the frame wait timeout value to use. This is a function of data rate, preamble length, and PRF
|
| 873 |
|
| 874 |
extern uint8_t dwnsSFDlen[];
|
| 875 |
|
| 876 |
void instancesetreplydelay(int delayus) { //delay in us |
| 877 |
|
| 878 |
int instance = 0; |
| 879 |
int margin = 3000; //2000 symbols |
| 880 |
int respframe = 0; |
| 881 |
int respframe_sy = 0; |
| 882 |
|
| 883 |
//configure the rx delay receive delay time, it is dependent on the message length
|
| 884 |
float msgdatalen = 0; |
| 885 |
float preamblelen = 0; |
| 886 |
int sfdlen = 0; |
| 887 |
int x = 0; |
| 888 |
|
| 889 |
//Set the RX timeouts based on the longest expected message - the Final message
|
| 890 |
//Poll = 13, Response = 20, Final = 44 bytes
|
| 891 |
//msgdatalen = TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
|
| 892 |
msgdatalen = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC; |
| 893 |
|
| 894 |
x = (int) ceil(msgdatalen*8/330.0f); |
| 895 |
|
| 896 |
msgdatalen = msgdatalen*8 + x*48; |
| 897 |
|
| 898 |
//add some margin so we don't timeout too soon
|
| 899 |
margin = 0; //(TAG_FINAL_MSG_LEN - TAG_POLL_MSG_LEN); |
| 900 |
|
| 901 |
x = (int) ceil(margin*8/330.0f); |
| 902 |
|
| 903 |
margin = margin*8 + x*48; |
| 904 |
|
| 905 |
//assume PHR length is 172308ns for 110k and 21539ns for 850k/6.81M
|
| 906 |
if(instance_data[instance].configData.dataRate == DWT_BR_110K) {
|
| 907 |
msgdatalen *= 8205.13f; |
| 908 |
msgdatalen += 172308; // PHR length in nanoseconds |
| 909 |
|
| 910 |
margin *= 8205.13f; |
| 911 |
|
| 912 |
} |
| 913 |
else if(instance_data[instance].configData.dataRate == DWT_BR_850K) { |
| 914 |
msgdatalen *= 1025.64f; |
| 915 |
msgdatalen += 21539; // PHR length in nanoseconds |
| 916 |
|
| 917 |
margin *= 1025.64f; |
| 918 |
} |
| 919 |
else {
|
| 920 |
msgdatalen *= 128.21f; |
| 921 |
msgdatalen += 21539; // PHR length in nanoseconds |
| 922 |
|
| 923 |
margin *= 128.21f; |
| 924 |
} |
| 925 |
|
| 926 |
//SFD length is 64 for 110k (always)
|
| 927 |
//SFD length is 8 for 6.81M, and 16 for 850k, but can vary between 8 and 16 bytes
|
| 928 |
sfdlen = dwnsSFDlen[instance_data[instance].configData.dataRate]; |
| 929 |
|
| 930 |
switch (instance_data[instance].configData.txPreambLength) {
|
| 931 |
case DWT_PLEN_4096 : preamblelen = 4096.0f; break; |
| 932 |
case DWT_PLEN_2048 : preamblelen = 2048.0f; break; |
| 933 |
case DWT_PLEN_1536 : preamblelen = 1536.0f; break; |
| 934 |
case DWT_PLEN_1024 : preamblelen = 1024.0f; break; |
| 935 |
case DWT_PLEN_512 : preamblelen = 512.0f; break; |
| 936 |
case DWT_PLEN_256 : preamblelen = 256.0f; break; |
| 937 |
case DWT_PLEN_128 : preamblelen = 128.0f; break; |
| 938 |
case DWT_PLEN_64 : preamblelen = 64.0f; break; |
| 939 |
} |
| 940 |
|
| 941 |
//preamble = plen * (994 or 1018) depending on 16 or 64 PRF
|
| 942 |
if(instance_data[instance].configData.prf == DWT_PRF_16M) {
|
| 943 |
preamblelen = (sfdlen + preamblelen) * 0.99359f; |
| 944 |
} |
| 945 |
else {
|
| 946 |
preamblelen = (sfdlen + preamblelen) * 1.01763f; |
| 947 |
} |
| 948 |
|
| 949 |
respframe_sy = (16 + (int)((preamblelen + ((msgdatalen + margin)/1000.0))/ 1.0256)) ; |
| 950 |
|
| 951 |
//this is the delay used for the delayed transmit (when sending the response, and final messages)
|
| 952 |
instance_data[instance].pollTx2FinalTxDelay = convertmicrosectodevicetimeu (delayus); |
| 953 |
//the anchor to anchor ranging consist of A0 ranging to A1 and A2 and A1 ranging to A2
|
| 954 |
//as there are less messages the ranging time is shorter (thus divide by 2)
|
| 955 |
instance_data[instance].pollTx2FinalTxDelayAnc = convertmicrosectodevicetimeu (delayus/2 + 100); |
| 956 |
|
| 957 |
//this is the delay the anchors 1, 2, etc.. will send the response back at...
|
| 958 |
//anchor 2 will have the delay set to 2 * fixedReplyDelayAnc
|
| 959 |
//andhor 3 will have the delay set to 3 * fixedReplyDelayAnc and so on...
|
| 960 |
//this delay depends on how quickly the tag can receive and process the message from previous anchor
|
| 961 |
//(and also the frame length of course)
|
| 962 |
respframe = (int)(preamblelen + (msgdatalen/1000.0)); //length of response frame (micro seconds) |
| 963 |
if(instance_data[instance].configData.dataRate == DWT_BR_110K) {
|
| 964 |
|
| 965 |
//set the frame wait timeout time - total time the frame takes in symbols
|
| 966 |
instance_data[instance].fwtoTime_sy = respframe_sy + RX_RESPONSE1_TURNAROUND_110K + 400; //add some margin because of the resp to resp RX turn on time |
| 967 |
|
| 968 |
instance_data[instance].fwtoTimeAnc_sy = respframe_sy; //add some margin so we don't timeout too soon
|
| 969 |
instance_data[instance].fixedReplyDelayAnc = convertmicrosectodevicetimeu (respframe + RX_RESPONSE1_TURNAROUND_110K); |
| 970 |
instance_data[instance].fixedReplyDelayAncP = (uint32_t) (((uint64_t) convertmicrosectodevicetimeu (preamblelen)) >> 8) + 16; |
| 971 |
|
| 972 |
instance_data[instance].ancRespRxDelay = RX_RESPONSE1_TURNAROUND_110K ; |
| 973 |
} |
| 974 |
else {
|
| 975 |
|
| 976 |
//set the frame wait timeout time - total time the frame takes in symbols
|
| 977 |
instance_data[instance].fwtoTime_sy = respframe_sy + RX_RESPONSE1_TURNAROUND_6M81; //add some margin because of the resp to resp RX turn on time
|
| 978 |
|
| 979 |
instance_data[instance].fwtoTimeAnc_sy = respframe_sy; |
| 980 |
instance_data[instance].fixedReplyDelayAnc = convertmicrosectodevicetimeu (respframe + RX_RESPONSE1_TURNAROUND_6M81); |
| 981 |
instance_data[instance].fixedReplyDelayAncP = (uint32_t) (((uint64_t) convertmicrosectodevicetimeu (preamblelen)) >> 8) + 16; |
| 982 |
|
| 983 |
instance_data[instance].ancRespRxDelay = RX_RESPONSE1_TURNAROUND_6M81 ; |
| 984 |
} |
| 985 |
|
| 986 |
} |
| 987 |
|
| 988 |
// -------------------------------------------------------------------------------------------------------------------
|
| 989 |
//
|
| 990 |
// Set Payload parameters for the instance
|
| 991 |
//
|
| 992 |
// -------------------------------------------------------------------------------------------------------------------
|
| 993 |
void instancesetaddresses(uint16_t address) {
|
| 994 |
int instance = 0 ; |
| 995 |
|
| 996 |
instance_data[instance].instanceAddress16 = address ; // copy configurations
|
| 997 |
} |
| 998 |
|
| 999 |
|
| 1000 |
#endif
|
| 1001 |
|
| 1002 |
|
| 1003 |
|
| 1004 |
|
| 1005 |
/* ==========================================================
|
| 1006 |
|
| 1007 |
Notes:
|
| 1008 |
|
| 1009 |
Previously code handled multiple instances in a single console application
|
| 1010 |
|
| 1011 |
Now have changed it to do a single instance only. With minimal code changes...(i.e. kept [instance] index but it is always 0.
|
| 1012 |
|
| 1013 |
Windows application should call instance_init() once and then in the "main loop" call instance_run().
|
| 1014 |
|
| 1015 |
*/
|
| 1016 |
|
| 1017 |
|
| 1018 |
#endif /* defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 1) */ |