Statistics
| Branch: | Tag: | Revision:

amiro-lld / source / DW1000 / v1 / deca_instance_tag_anchor_v1.c @ 26dead12

History | View | Annotate | Download (42.3 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
//
25
// function to construct the message/frame header bytes
26
//
27
// -------------------------------------------------------------------------------------------------------------------
28
//
29
void instanceconfigframeheader16(instance_data_t *inst){
30
  //set frame type (0-2), SEC (3), Pending (4), ACK (5), PanIDcomp(6)
31
  inst->msg_f.frameCtrl[0] = 0x1 /*frame type 0x1 == data*/ | 0x40 /*PID comp*/;
32

    
33
  //source/dest addressing modes and frame version
34
  inst->msg_f.frameCtrl[1] = 0x8 /*dest extended address (16bits)*/ | 0x80 /*src extended address (16bits)*/;
35

    
36
  inst->msg_f.panID[0] = (inst->panID) & 0xff;
37
  inst->msg_f.panID[1] = inst->panID >> 8;
38

    
39
  inst->msg_f.seqNum = 0;
40
}
41

    
42
int instancesenddlypacket(instance_data_t *inst, int delayedTx){
43
  int result = 0;
44

    
45
  dwt_writetxfctrl(inst->psduLength, 0, 1);
46
  if(delayedTx == DWT_START_TX_DELAYED){
47
    dwt_setdelayedtrxtime(inst->delayedReplyTime) ; //should be high 32-bits of delayed TX TS
48
  }
49

    
50
  //begin delayed TX of frame
51
  if (dwt_starttx((uint8_t)(delayedTx | inst->wait4ack))){ // delayed start was too late
52
    result = 1; //late/error
53
    inst->lateTX++;
54
  }
55
  else {
56
    inst->timeofTx = portGetTickCnt();
57
    inst->monitor = 1;
58
  }
59
  return result;                                              // state changes
60
}
61

    
62
int instance_calcranges(uint32_t *array, uint16_t size, int reportRange, uint8_t* mask){
63
  int i;
64
  int newRange = TOF_REPORT_NUL;
65
  int distance = 0;
66

    
67
  for(i=0; i<size; i++)  {
68
    uint32_t tofx = array[i];
69
    if(tofx != INVALID_TOF) { //if ToF == 0 - then no new range to report
70
      distance = reportTOF(i, tofx);
71
    }
72

    
73
    if(distance == 1){
74
      newRange = reportRange;
75
    }
76
    else {
77
      //clear mask
78
      *mask &= ~(0x1 << i) ;
79
      clearDistTable(i);
80
    }
81
    array[i] = INVALID_TOF;
82

    
83
    distance = 0;
84
  }
85

    
86
  return newRange;
87
}
88

    
89
// -------------------------------------------------------------------------------------------------------------------
90
//
91
// the main instance state machine (all the instance modes Tag, Anchor or Listener use the same statemachine....)
92
//
93
// -------------------------------------------------------------------------------------------------------------------
94
//
95
int testapprun(instance_data_t *inst, int message){
96

    
97
  switch (inst->testAppState){
98
  case TA_INIT :
99
    // printf("TA_INIT") ;
100
    switch (inst->mode) {
101
    case TAG: {
102
      uint16_t sleep_mode = 0;
103

    
104
      dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN); //allow data, ack frames;
105
      dwt_setpanid(inst->panID);
106

    
107
      memcpy(inst->eui64, &inst->instanceAddress16, ADDR_BYTE_SIZE_S);
108
      dwt_seteui(inst->eui64);
109

    
110
      //set source address
111
      inst->newRangeTagAddress = inst->instanceAddress16 ;
112
      dwt_setaddress16(inst->instanceAddress16);
113

    
114
      //Start off by Sleeping 1st -> set instToSleep to TRUE
115
      inst->nextState = TA_TXPOLL_WAIT_SEND;
116
      inst->testAppState = TA_TXE_WAIT;
117
      inst->instToSleep = TRUE ;
118

    
119
      inst->rangeNum = 0;
120
      inst->tagSleepCorrection = 0;
121

    
122
      sleep_mode = (DWT_LOADUCODE|DWT_PRESRV_SLEEP|DWT_CONFIG|DWT_TANDV);
123

    
124
      if(inst->configData.txPreambLength == DWT_PLEN_64)  //if using 64 length preamble then use the corresponding OPSet
125
        sleep_mode |= DWT_LOADOPSET;
126

    
127
#if (DEEP_SLEEP == 1)
128
      dwt_configuresleep(sleep_mode, DWT_WAKE_WK|DWT_WAKE_CS|DWT_SLP_EN); //configure the on wake parameters (upload the IC config settings)
129
#endif
130
      instanceconfigframeheader16(inst);
131
      inst->instanceWakeTime = portGetTickCnt();
132
    }
133
    break;
134
    case ANCHOR: {
135
      memcpy(inst->eui64, &inst->instanceAddress16, ADDR_BYTE_SIZE_S);
136
      dwt_seteui(inst->eui64);
137

    
138
      dwt_setpanid(inst->panID);
139

    
140
      //set source address
141
      inst->shortAdd_idx = (inst->instanceAddress16 & 0x3) ;
142
      dwt_setaddress16(inst->instanceAddress16);
143

    
144
      //if address = 0x8000
145
      if(inst->instanceAddress16 == GATEWAY_ANCHOR_ADDR){
146
        inst->gatewayAnchor = TRUE;
147
      }
148

    
149
      dwt_enableframefilter(DWT_FF_NOTYPE_EN); //allow data, ack frames;
150

    
151
      // First time anchor listens we don't do a delayed RX
152
      dwt_setrxaftertxdelay(0);
153
      //change to next state - wait to receive a message
154
      inst->testAppState = TA_RXE_WAIT ;
155

    
156
      dwt_setrxtimeout(0);
157
      dwt_setpreambledetecttimeout(0);
158
      instanceconfigframeheader16(inst);
159

    
160
    }
161
    break;
162
    case LISTENER:{
163
      dwt_enableframefilter(DWT_FF_NOTYPE_EN); //disable frame filtering
164
      dwt_setrxaftertxdelay(0); //no delay of turning on of RX
165
      dwt_setrxtimeout(0);
166
      dwt_setpreambledetecttimeout(0);
167
      //change to next state - wait to receive a message
168
      inst->testAppState = TA_RXE_WAIT ;
169
    }
170
    break ; // end case TA_INIT
171
    default:
172
      break;
173
    }
174
    break; // end case TA_INIT
175

    
176
    case TA_SLEEP_DONE : {
177
      event_data_t* dw_event = instance_getevent(10); //clear the event from the queue
178
      // waiting for timout from application to wakup IC
179
      if (dw_event->type != DWT_SIG_RX_TIMEOUT){
180
        // if no pause and no wake-up timeout continu waiting for the sleep to be done.
181
        inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //wait here for sleep timeout
182
        break;
183
      }
184

    
185
      inst->done = INST_NOT_DONE_YET;
186
      inst->instToSleep = FALSE ;
187
      inst->testAppState = inst->nextState;
188
      inst->nextState = 0; //clear
189
      inst->instanceWakeTime = portGetTickCnt(); // Record the time count when we wake-up
190
#if (DEEP_SLEEP == 1)
191
      {
192
        uint32 x = 0;
193

    
194
        //wake up device from low power mode
195
        //NOTE - in the ARM  code just drop chip select for 200us
196
        led_on(LED_PC9);
197
        port_SPIx_clear_chip_select();  //CS low
198
        instance_data[0].dwIDLE = 0; //reset DW1000 IDLE flag
199

    
200
        setup_DW1000RSTnIRQ(1); //enable RSTn IRQ
201

    
202
        Sleep(2);   //200 us to wake up - need 2 as Sleep(1) is ~ 175 us
203
//        chThdSleepMilliseconds(2);
204
        //then wait 5ms for DW1000 XTAL to stabilise - instead of wait we wait for RSTn to go high
205
        //Sleep(5);
206

    
207
        //need to poll to check when the DW1000 is in IDLE, the CPLL interrupt is not reliable
208
        //when RSTn goes high the DW1000 is in INIT, it will enter IDLE after PLL lock (in 5 us)
209
        while(instance_data[0].dwIDLE == 0) // this variable will be sent in the IRQ (process_dwRSTn_irq)
210
        {
211
          //wait for DW1000 to go to IDLE state RSTn pin to go high
212
          x++;
213
        }
214
        setup_DW1000RSTnIRQ(0); //disable RSTn IRQ
215
        port_SPIx_set_chip_select();  //CS high
216

    
217
        //!!! NOTE it takes ~35us for the DW1000 to download AON and lock the PLL and be in IDLE state
218
        //do some dummy reads of the dev ID register to make sure DW1000 is in IDLE before setting LEDs
219
        x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz)
220
        x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz)
221
        x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz)
222
        x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz)
223

    
224
        x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz)
225
        /*if(x != DWT_DEVICE_ID)
226
                {
227
                    x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz)
228
                }*/
229
        led_off(LED_PC9);
230
        //this is platform dependent - only program if DW EVK/EVB
231
        dwt_setleds(1);
232

    
233
        //MP bug - TX antenna delay needs reprogramming as it is not preserved (only RX)
234
        dwt_settxantennadelay(inst->txAntennaDelay) ;
235

    
236
        //set EUI as it will not be preserved unless the EUI is programmed and loaded from NVM
237
        dwt_seteui(inst->eui64);
238
      }
239
#else
240
      Sleep(3); //to approximate match the time spent in the #if above
241
#endif
242

    
243
      instancesetantennadelays(); //this will update the antenna delay if it has changed
244
      instancesettxpower(); //configure TX power if it has changed
245

    
246
    }
247
    break;
248

    
249
    case TA_TXE_WAIT : //either go to sleep or proceed to TX a message
250
      // printf("TA_TXE_WAIT") ;
251
      //if we are scheduled to go to sleep before next transmission then sleep first.
252
      if((inst->nextState == TA_TXPOLL_WAIT_SEND)
253
          && (inst->instToSleep)  //go to sleep before sending the next poll/ starting new ranging exchange
254
      ){
255
        inst->rangeNum++; //increment the range number before going to sleep
256
        //the app should put chip into low power state and wake up after tagSleepTime_ms time...
257
        //the app could go to *_IDLE state and wait for uP to wake it up...
258
        inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT_TO; //don't sleep here but kick off the Sleep timer countdown
259
        inst->testAppState = TA_SLEEP_DONE; {
260
#if (DEEP_SLEEP == 1)
261
          //put device into low power mode
262
          dwt_entersleep(); //go to sleep
263
#endif
264
          //DW1000 gone to sleep - report the received range
265
          inst->newRange = instance_calcranges(&inst->tofArray[0], MAX_ANCHOR_LIST_SIZE, TOF_REPORT_T2A, &inst->rxResponseMask);
266
          inst->rxResponseMaskReport = inst->rxResponseMask;
267
          inst->rxResponseMask = 0;
268
          inst->newRangeTime = portGetTickCnt() ;
269
        }
270
      }
271
      else {  //proceed to configuration and transmission of a frame
272
        inst->testAppState = inst->nextState;
273
        inst->nextState = 0; //clear
274
      }
275
      break ; // end case TA_TXE_WAIT
276

    
277
    case TA_TXPOLL_WAIT_SEND : {
278

    
279
      inst->msg_f.messageData[POLL_RNUM] = (inst->mode == TAG) ? inst->rangeNum : inst->rangeNumAnc; //copy new range number
280
      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...)
281
      inst->psduLength = (TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC);
282
      inst->msg_f.seqNum = inst->frameSN++; //copy sequence number and then increment
283
      inst->msg_f.sourceAddr[0] = inst->eui64[0]; //copy the address
284
      inst->msg_f.sourceAddr[1] = inst->eui64[1]; //copy the address
285
      inst->msg_f.destAddr[0] = 0xff;  //set the destination address (broadcast == 0xffff)
286
      inst->msg_f.destAddr[1] = 0xff;  //set the destination address (broadcast == 0xffff)
287
      dwt_writetxdata(inst->psduLength, (uint8_t *)  &inst->msg_f, 0) ;        // write the frame data
288

    
289
      //set the delayed rx on time (the response message will be sent after this delay (from A0))
290
      dwt_setrxaftertxdelay((uint32_t)RX_RESPONSE1_TURNAROUND);  //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
291

    
292
      if(inst->mode == TAG){
293
        inst->rxResps[inst->rangeNum] = 0; //reset the number of received responses
294
        inst->responseTO = MAX_ANCHOR_LIST_SIZE; //expecting 4 responses
295
        dwt_setrxtimeout((uint16_t)inst->fwtoTime_sy * MAX_ANCHOR_LIST_SIZE);  //configure the RX FWTO
296
      }
297
      else {
298
        inst->rxResps[inst->rangeNumAnc] = 0; //reset number of responses
299
        inst->responseTO = NUM_EXPECTED_RESPONSES_ANC0; //2 responses A1, A2
300
        dwt_setrxtimeout((uint16_t)inst->fwtoTime_sy * (NUM_EXPECTED_RESPONSES_ANC0));  //units are
301
      }
302

    
303
      inst->rxResponseMask = 0;        //reset/clear the mask of received responses when tx poll
304
      inst->rxResponseMaskAnc = 0;
305

    
306
      inst->wait4ack = DWT_RESPONSE_EXPECTED; //response is expected - automatically enable the receiver
307

    
308
      dwt_writetxfctrl(inst->psduLength, 0, 1); //write frame control
309

    
310
      dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED); //transmit the frame
311

    
312
      inst->testAppState = TA_TX_WAIT_CONF ;  // wait confirmation
313
      inst->previousState = TA_TXPOLL_WAIT_SEND ;
314
      inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set above)
315
    }
316
    break;
317

    
318
    case TA_TXFINAL_WAIT_SEND : {
319
      //the final has the same range number as the poll (part of the same ranging exchange)
320
      inst->msg_f.messageData[POLL_RNUM] = (inst->mode == TAG) ? inst->rangeNum : inst->rangeNumAnc;
321
      //the mask is sent so the anchors know whether the response RX time is valid
322
      inst->msg_f.messageData[VRESP] = (inst->mode == TAG) ? inst->rxResponseMask : inst->rxResponseMaskAnc;
323
      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...)
324
      inst->psduLength = (TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC);
325
      inst->msg_f.seqNum = inst->frameSN++;
326
      dwt_writetxdata(inst->psduLength, (uint8_t *)  &inst->msg_f, 0) ;        // write the frame data
327

    
328
      inst->wait4ack = 0; //clear the flag not using wait for response as this message ends the ranging exchange
329

    
330
      if(instancesenddlypacket(inst, DWT_START_TX_DELAYED)) {
331
        // initiate the re-transmission
332
        if(inst->mode == TAG){
333
          inst->testAppState = TA_TXE_WAIT ; //go to TA_TXE_WAIT first to check if it's sleep time
334
          inst->nextState = TA_TXPOLL_WAIT_SEND ;
335
        }
336
        else {
337
          //A0 - failed to send Final
338
          //A1 - failed to send Final
339
          //go back to RX and behave as anchor
340
          instance_backtoanchor(inst);
341
        }
342
        break; //exit this switch case...
343
      }
344
      else {
345
        inst->testAppState = TA_TX_WAIT_CONF;                                               // wait confirmation
346
        inst->previousState = TA_TXFINAL_WAIT_SEND;
347
      }
348
      if(inst->mode == TAG){
349
        inst->instToSleep = TRUE ;
350
      }
351
      inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set above)
352
    }
353
    break;
354

    
355
    case TA_TX_WAIT_CONF : {
356
      //printf("TA_TX_WAIT_CONF %d m%d %d states %08x %08x\n", inst->previousState, message, inst->newReportSent, dwt_read32bitreg(0x19), dwt_read32bitreg(0x0f)) ;
357

    
358
      event_data_t* dw_event = instance_getevent(11); //get and clear this event
359

    
360
      //NOTE: Can get the ACK before the TX confirm event for the frame requesting the ACK
361
      //this happens because if polling the ISR the RX event will be processed 1st and then the TX event
362
      //thus the reception of the ACK will be processed before the TX confirmation of the frame that requested it.
363
      if(dw_event->type != DWT_SIG_TX_DONE) { //wait for TX done confirmation
364
        if(dw_event->type != 0) {
365
          if(dw_event->type == DWT_SIG_RX_TIMEOUT){   //got RX timeout - i.e. did not get the response (e.g. ACK)
366
            //printf("RX timeout in TA_TX_WAIT_CONF (%d)\n", inst->previousState);
367
            //we need to wait for SIG_TX_DONE and then process the timeout and re-send the frame if needed
368
            inst->gotTO = 1;
369
          }
370
          else{
371
            inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT;
372
          }
373
        }
374

    
375
        inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT;
376
        break;
377

    
378
      }
379

    
380
      inst->done = INST_NOT_DONE_YET;
381

    
382
      if(inst->previousState == TA_TXFINAL_WAIT_SEND) {
383
        if(inst->mode == TAG){
384
          inst->testAppState = TA_TXE_WAIT ;
385
          inst->nextState = TA_TXPOLL_WAIT_SEND ;
386
          break;
387
        }
388
        else{
389
          instance_backtoanchor(inst);
390
        }
391
      }
392
      else if (inst->gotTO == 1) { //timeout
393
        //printf("got TO in TA_TX_WAIT_CONF\n");
394
        inst_processrxtimeout(inst);
395
        inst->gotTO = 0;
396
        inst->wait4ack = 0 ; //clear this
397
        break;
398
      }
399
      else{
400
        inst->txu.txTimeStamp = dw_event->timeStamp;
401

    
402
        if(inst->previousState == TA_TXPOLL_WAIT_SEND){
403
          uint64_t tagCalculatedFinalTxTime ;
404
          // Embed into Final message: 40-bit pollTXTime,  40-bit respRxTime,  40-bit finalTxTime
405
          if(inst->mode == TAG){
406
            tagCalculatedFinalTxTime =  (inst->txu.txTimeStamp + inst->pollTx2FinalTxDelay) & MASK_TXDTS;
407
          }
408
          else {  //for anchor make the final half the delay ..... (this is ok, as A0 awaits 2 responses)
409
            tagCalculatedFinalTxTime =  (inst->txu.txTimeStamp + inst->pollTx2FinalTxDelayAnc) & MASK_TXDTS;
410
          }
411
          inst->delayedReplyTime = (uint32_t)(tagCalculatedFinalTxTime >> 8); //high 32-bits
412
          // Calculate Time Final message will be sent and write this field of Final message
413
          // Sending time will be delayedReplyTime, snapped to ~125MHz or ~250MHz boundary by
414
          // zeroing its low 9 bits, and then having the TX antenna delay added
415
          // getting antenna delay from the device and add it to the Calculated TX Time
416
          tagCalculatedFinalTxTime = tagCalculatedFinalTxTime + inst->txAntennaDelay;
417
          tagCalculatedFinalTxTime &= MASK_40BIT;
418

    
419
          // Write Calculated TX time field of Final message
420
          memcpy(&(inst->msg_f.messageData[FTXT]), (uint8_t *)&tagCalculatedFinalTxTime, 5);
421
          // Write Poll TX time field of Final message
422
          memcpy(&(inst->msg_f.messageData[PTXT]), (uint8_t *)&inst->txu.tagPollTxTime, 5);
423

    
424
          //change the w4r for the second and remaining anchors to 50 us
425
          //dwt_setrxaftertxdelay((uint32)RX_RESPONSEX_TURNAROUND);  //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
426
        }
427

    
428
        if(inst->previousState == TA_TXRESPONSE_SENT_TORX) {
429
          inst->previousState = TA_TXRESPONSE_WAIT_SEND ;
430
        }
431
        inst->testAppState = TA_RXE_WAIT ;                      // After sending, tag expects response/report, anchor waits to receive a final/new poll
432

    
433
        message = 0;
434
        //fall into the next case (turn on the RX)
435
      }
436
    }
437
    break ; // end case TA_TX_WAIT_CONF
438

    
439

    
440
    case TA_RXE_WAIT : {
441
      // printf("TA_RXE_WAIT") ;
442
      if(inst->wait4ack == 0) {  //if this is set the RX will turn on automatically after TX
443
        //turn RX on
444
        dwt_rxenable(DWT_START_RX_IMMEDIATE) ;  // turn RX on, without delay
445
      }
446
      else{
447
        inst->wait4ack = 0 ; //clear the flag, the next time we want to turn the RX on it might not be auto
448
      }
449

    
450
      if (inst->mode != LISTENER){
451
        inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //using RX FWTO
452
      }
453

    
454
      inst->testAppState = TA_RX_WAIT_DATA;   // let this state handle it
455

    
456
      // end case TA_RXE_WAIT, don't break, but fall through into the TA_RX_WAIT_DATA state to process it immediately.
457
      if(message == 0) break;
458
    }
459
    break;
460

    
461
    case TA_RX_WAIT_DATA :   // Wait RX data
462
      //printf("TA_RX_WAIT_DATA %d", message) ;
463
      switch (message){
464

    
465
      //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
466
      case DWT_SIG_RX_OKAY : {
467
        event_data_t* dw_event = instance_getevent(15); //get and clear this event
468
        uint8_t  srcAddr[8] = {0,0,0,0,0,0,0,0};
469
        uint8_t  dstAddr[8] = {0,0,0,0,0,0,0,0};
470
        int fcode = 0;
471
        int fn_code = 0;
472
        //int srclen = 0;
473
        //int fctrladdr_len;
474
        uint8_t tof_idx  = 0;
475
        uint8_t *messageData = NULL;
476

    
477
        inst->stopTimer = 0; //clear the flag, as we have received a message
478

    
479
        // handle 16 and 64 bit source and destination addresses
480
        switch(dw_event->msgu.frame[1] & 0xCC){
481
        case 0xCC: //
482
          memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ll.sourceAddr[0]), ADDR_BYTE_SIZE_L);
483
          memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_ll.destAddr[0]), ADDR_BYTE_SIZE_L);
484
          fn_code = dw_event->msgu.rxmsg_ll.messageData[FCODE];
485
          messageData = &dw_event->msgu.rxmsg_ll.messageData[0];
486
          //srclen = ADDR_BYTE_SIZE_L;
487
          //fctrladdr_len = FRAME_CRTL_AND_ADDRESS_L;
488
          break;
489
        case 0xC8: //
490
          memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_sl.sourceAddr[0]), ADDR_BYTE_SIZE_L);
491
          memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_sl.destAddr[0]), ADDR_BYTE_SIZE_S);
492
          fn_code = dw_event->msgu.rxmsg_sl.messageData[FCODE];
493
          messageData = &dw_event->msgu.rxmsg_sl.messageData[0];
494
          //srclen = ADDR_BYTE_SIZE_L;
495
          //fctrladdr_len = FRAME_CRTL_AND_ADDRESS_LS;
496
          break;
497
        case 0x8C: //
498
          memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ls.sourceAddr[0]), ADDR_BYTE_SIZE_S);
499
          memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_ls.destAddr[0]), ADDR_BYTE_SIZE_L);
500
          fn_code = dw_event->msgu.rxmsg_ls.messageData[FCODE];
501
          messageData = &dw_event->msgu.rxmsg_ls.messageData[0];
502
          //srclen = ADDR_BYTE_SIZE_S;
503
          //fctrladdr_len = FRAME_CRTL_AND_ADDRESS_LS;
504
          break;
505
        case 0x88: //
506
          memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ss.sourceAddr[0]), ADDR_BYTE_SIZE_S);
507
          memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_ss.destAddr[0]), ADDR_BYTE_SIZE_S);
508
          fn_code = dw_event->msgu.rxmsg_ss.messageData[FCODE];
509
          messageData = &dw_event->msgu.rxmsg_ss.messageData[0];
510
          //srclen = ADDR_BYTE_SIZE_S;
511
          //fctrladdr_len = FRAME_CRTL_AND_ADDRESS_S;
512
          break;
513
        }
514

    
515
        if((inst->instToSleep == FALSE) && (inst->mode == LISTENER)){ //update received data, and go back to receiving frames
516
          //do something with message data (e.g. could extract any ToFs and print them)
517
          inst->testAppState = TA_RXE_WAIT ;              // wait for next frame
518
          dwt_setrxaftertxdelay(0);
519
        }
520
        else{
521
          //process ranging messages
522
          fcode = fn_code;
523
          tof_idx = srcAddr[0] & 0x3 ;
524

    
525
          switch(fcode){
526

    
527
          case RTLS_DEMO_MSG_ANCH_POLL:
528
          case RTLS_DEMO_MSG_TAG_POLL: {
529
            inst->tagPollRxTime = dw_event->timeStamp ; //save Poll's Rx time
530
            if(fcode == RTLS_DEMO_MSG_TAG_POLL){ //got poll from Tag
531
              inst->rangeNumA[srcAddr[0]&0x7] = messageData[POLL_RNUM]; //when anchor receives a poll, we need to remember the new range number
532
            }
533
            else{ //got poll from Anchor (initiator)
534
              inst->rangeNumAAnc[tof_idx] = messageData[POLL_RNUM]; //when anchor receives poll from another anchor - save the range number
535
            }
536

    
537
            if (A1_ANCHOR_ADDR == inst->instanceAddress16) { //this is A1
538

    
539
              if(GATEWAY_ANCHOR_ADDR == (srcAddr[0] | ((uint32_t)(srcAddr[1] << 8)))) { //poll is from A0
540

    
541
                //configure the time A1 will poll A2 (it should be in half slot time from now)
542
                inst->a1SlotTime = dw_event->uTimeStamp + (inst->slotPeriod);
543

    
544
                //inst->instanceTimerEn = 1; - THIS IS ENABLED BELOW AFTER FINAL
545
                // - means that if final is not received then A1 will not range to A2
546
              }
547
            }
548

    
549
            //the response has been sent - await TX done event
550
            if(dw_event->type_pend == DWT_SIG_TX_PENDING){
551
              inst->testAppState = TA_TX_WAIT_CONF;                // wait confirmation
552
              inst->previousState = TA_TXRESPONSE_SENT_POLLRX ;    //wait for TX confirmation of sent response
553
            }
554
            //already re-enabled the receiver
555
            else if (dw_event->type_pend == DWT_SIG_RX_PENDING){
556
              //stay in RX wait for next frame...
557
              //RX is already enabled...
558
              inst->testAppState = TA_RX_WAIT_DATA ;              // wait for next frame
559
            }
560
            else{ //the DW1000 is idle (re-enable from the application level)
561
              //stay in RX wait for next frame...
562
              inst->testAppState = TA_RXE_WAIT ;              // wait for next frame
563
            }
564

    
565

    
566
          }
567
          break; //RTLS_DEMO_MSG_TAG_POLL
568

    
569
          case RTLS_DEMO_MSG_ANCH_RESP2:
570
          case RTLS_DEMO_MSG_ANCH_RESP:{
571
            uint8_t currentRangeNum = (messageData[TOFRN] + 1); //current = previous + 1
572

    
573
            if(GATEWAY_ANCHOR_ADDR == (srcAddr[0] | ((uint32_t)(srcAddr[1] << 8)))){ //if response from gateway then use the correction factor
574
              if(inst->mode == TAG){
575
                // casting received bytes to int because this is a signed correction -0.5 periods to +1.5 periods
576
                inst->tagSleepCorrection = (int16_t) (((uint16_t) messageData[RES_TAG_SLP1] << 8) + messageData[RES_TAG_SLP0]);
577
                inst->tagSleepRnd = 0; // once we have initial response from Anchor #0 the slot correction acts and we don't need this anymore
578
              }
579
            }
580

    
581
            //the response has been sent - await TX done event
582
            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
583
              inst->testAppState = TA_TX_WAIT_CONF;                // wait confirmation
584
              inst->previousState = TA_TXRESPONSE_SENT_RESPRX ;    //wait for TX confirmation of sent response
585
            }
586
            //already re-enabled the receiver
587
            else if(dw_event->type_pend == DWT_SIG_RX_PENDING) {
588
              // stay in TA_RX_WAIT_DATA - receiver is already enabled.
589
            }
590
            //DW1000 idle - send the final
591
            else { //if(dw_event->type_pend == DWT_SIG_DW_IDLE)
592

    
593
              if(((TAG == inst->mode) && (inst->rxResponseMask & 0x1)) //if A0's response received send the final
594
                  || ((A1_ANCHOR_ADDR == inst->instanceAddress16) && (inst->rxResponseMaskAnc & 0x4))
595
                  || ((GATEWAY_ANCHOR_ADDR == inst->instanceAddress16) && (inst->rxResponseMaskAnc & 0x2)) ) { //if A1's response received
596

    
597
                inst->testAppState = TA_TXFINAL_WAIT_SEND ; // send our response / the final
598
              }
599
              else { //go to sleep
600

    
601
                if(TAG == inst->mode){
602
                  inst->testAppState = TA_TXE_WAIT ; //go to TA_TXE_WAIT first to check if it's sleep time
603
                  inst->nextState = TA_TXPOLL_WAIT_SEND ;
604
                  inst->instToSleep = TRUE;
605
                }
606
                else {
607
                  instance_backtoanchor(inst);
608
                }
609
              }
610
            }
611

    
612
            if(fcode == RTLS_DEMO_MSG_ANCH_RESP) { //tag to anchor mode
613
              if(currentRangeNum == inst->rangeNum) { //these are the previous ranges...
614
                //copy the ToF and put into array (array holds last 4 ToFs)
615
                memcpy(&inst->tofArray[(srcAddr[0]&0x3)], &(messageData[TOFR]), 4);
616

    
617
                //check if the ToF is valid, this makes sure we only report valid ToFs
618
                //e.g. consider the case of reception of response from anchor a1 (we are anchor a2)
619
                //if a1 got a Poll with previous Range number but got no Final, then the response will have
620
                //the correct range number but the range will be INVALID_TOF
621
                if(inst->tofArray[(srcAddr[0]&0x3)] != INVALID_TOF){
622
                  inst->rxResponseMask |= (0x1 << (srcAddr[0]&0x3));
623
                }
624

    
625
              }
626
              else {
627
                if(inst->tofArray[(srcAddr[0]&0x3)] != INVALID_TOF) {
628
                  inst->tofArray[(srcAddr[0]&0x3)] = INVALID_TOF;
629
                }
630
              }
631

    
632

    
633
            }
634
            else { //anchor to anchor (only gateway processes anchor to anchor ToFs)
635
              //report the correct set of ranges (ranges from anchors A1, A2 need to match owns range number)
636
                if((inst->gatewayAnchor)&&(currentRangeNum == inst->rangeNumAnc)) { //these are the previous ranges...
637
                inst->rangeNumAAnc[0] = inst->rangeNumAnc ;
638

    
639
                //once A0 receives A2's response then it can report the 3 ToFs.
640
                if(inst->rxResps[inst->rangeNumAnc] == 3)
641
                  //if(A2_ANCHOR_ADDR == (srcAddr[0] | ((uint32)(srcAddr[1] << 8))))
642
                {
643
                  //copy the ToF and put into array, the array should have 3 ToFs A0-A1, A0-A2 and A1-A2
644
                  memcpy(&inst->tofArrayAnc[(srcAddr[0]+dstAddr[0])&0x3], &(messageData[TOFR]), 4);
645
                  //calculate all anchor - anchor ranges... and report
646
                  inst->newRange = instance_calcranges(&inst->tofArrayAnc[0], MAX_ANCHOR_LIST_SIZE, TOF_REPORT_A2A, &inst->rxResponseMaskAnc);
647
                  inst->rxResponseMaskReport = inst->rxResponseMaskAnc;
648
                  inst->rxResponseMaskAnc = 0;
649
                  inst->newRangeTime = dw_event->uTimeStamp ;
650
                }
651
                else {
652
                  //copy the ToF and put into array (array holds last 4 ToFs)
653
                  memcpy(&inst->tofArrayAnc[(srcAddr[0]+dstAddr[0])&0x3], &(messageData[TOFR]), 4);
654
                }
655
              }
656
            }
657

    
658
          }
659
          break; //RTLS_DEMO_MSG_ANCH_RESP
660

    
661

    
662
          case RTLS_DEMO_MSG_ANCH_FINAL:
663
          case RTLS_DEMO_MSG_TAG_FINAL: {
664
            int64_t Rb, Da, Ra, Db ;
665
            uint64_t tagFinalTxTime  = 0;
666
            uint64_t tagFinalRxTime  = 0;
667
            uint64_t tagPollTxTime  = 0;
668
            uint64_t anchorRespRxTime  = 0;
669
            int32_t tof = (int32_t)INVALID_TOF;
670

    
671
            double RaRbxDaDb = 0;
672
            double RbyDb = 0;
673
            double RayDa = 0;
674

    
675
            uint8_t validResp = messageData[VRESP];
676
            uint8_t index = RRXT0 + 5*(inst->shortAdd_idx);
677

    
678
            if((RTLS_DEMO_MSG_TAG_FINAL == fcode) &&
679
                (inst->rangeNumA[srcAddr[0]&0x7] != messageData[POLL_RNUM])) { //Final's range number needs to match Poll's or else discard this message
680
              inst->testAppState = TA_RXE_WAIT ;              // wait for next frame
681
              break;
682
            }
683

    
684
            if((RTLS_DEMO_MSG_ANCH_FINAL == fcode) &&
685
                (((inst->rangeNumAAnc[tof_idx] != messageData[POLL_RNUM]) //Final's range number needs to match Poll's or else discard this message
686
                    || inst->gatewayAnchor) //gateway can ignore the Final (from A1 to A2 exchange)
687
                    || (A3_ANCHOR_ADDR == inst->instanceAddress16))) //A3 does not care about Final from A1 or A0
688
            {
689
              inst->testAppState = TA_RXE_WAIT ;              // wait for next frame
690
              break;
691
            }
692

    
693
            if (A1_ANCHOR_ADDR == inst->instanceAddress16) { //this is A1
694
              if(GATEWAY_ANCHOR_ADDR == (srcAddr[0] | ((uint32_t)(srcAddr[1] << 8)))) { //final is from A0
695
                //ENABLE TIMER ONLY IF FINAL RECEIVED
696
                inst->instanceTimerEn = 1;
697
              }
698
            }
699
            //output data over USB...
700
            inst->newRangeAncAddress = inst->instanceAddress16;
701

    
702
            //if we got the final, maybe the tag did not get our response, so
703
            //we can use other anchors responses/ToF if there are any.. and output..
704
            //but we cannot calculate new range
705
            if(((validResp & (0x1<<(inst->shortAdd_idx))) != 0)) {
706
              // time of arrival of Final message
707
              tagFinalRxTime = dw_event->timeStamp ; //Final's Rx time
708

    
709
              //printf("FinalRx Timestamp: %4.15e\n", convertdevicetimetosecu(dw_event.timeStamp));
710
              inst->delayedReplyTime = 0 ;
711

    
712
              // times measured at Tag extracted from the message buffer
713
              // extract 40bit times
714
              memcpy(&tagPollTxTime, &(messageData[PTXT]), 5);
715
              memcpy(&anchorRespRxTime, &(messageData[index]), 5);
716
              memcpy(&tagFinalTxTime, &(messageData[FTXT]), 5);
717

    
718
              // poll response round trip delay time is calculated as
719
              // (anchorRespRxTime - tagPollTxTime) - (anchorRespTxTime - tagPollRxTime)
720
              Ra = (int64_t)((anchorRespRxTime - tagPollTxTime) & MASK_40BIT);
721
              Db = (int64_t)((inst->txu.anchorRespTxTime - inst->tagPollRxTime) & MASK_40BIT);
722

    
723
              // response final round trip delay time is calculated as
724
              // (tagFinalRxTime - anchorRespTxTime) - (tagFinalTxTime - anchorRespRxTime)
725
              Rb = (int64_t)((tagFinalRxTime - inst->txu.anchorRespTxTime) & MASK_40BIT);
726
              Da = (int64_t)((tagFinalTxTime - anchorRespRxTime) & MASK_40BIT);
727

    
728
              RaRbxDaDb = (((double)Ra))*(((double)Rb))
729
                                                - (((double)Da))*(((double)Db));
730

    
731
              RbyDb = ((double)Rb + (double)Db);
732

    
733
              RayDa = ((double)Ra + (double)Da);
734

    
735
              tof = (int32_t)(RaRbxDaDb/(RbyDb + RayDa));
736
            }
737

    
738
            //tag to anchor ranging
739
            if(RTLS_DEMO_MSG_TAG_FINAL == fcode) {
740
              inst->newRangeTagAddress = srcAddr[0] + ((uint16_t) srcAddr[1] << 8);
741
              //time-of-flight
742
              inst->tof[inst->newRangeTagAddress & 0x7] = (uint32_t)tof;
743
              //calculate all tag - anchor ranges... and report
744
              inst->newRange = instance_calcranges(&inst->tofArray[0], MAX_ANCHOR_LIST_SIZE, TOF_REPORT_T2A, &inst->rxResponseMask);
745
              inst->rxResponseMaskReport = inst->rxResponseMask; //copy the valid mask to report
746
              inst->rxResponseMask = 0;
747
              //we have our range - update the own mask entry...
748
              if(tof != (int32_t)INVALID_TOF) { //check the last ToF entry is valid and copy into the current array
749
                setTagDist(srcAddr[0], inst->shortAdd_idx); //copy distance from this anchor to the tag into array
750

    
751
                inst->rxResponseMask = (uint8_t)(0x1 << inst->shortAdd_idx);
752
                inst->tofArray[inst->shortAdd_idx] = (uint32_t)tof;
753
              }
754
              inst->newRangeTime = dw_event->uTimeStamp ;
755
            }
756
            else { //anchor to anchor ranging
757
              inst->newRangeTagAddress = srcAddr[0] + ((uint16_t) srcAddr[1] << 8);
758
              //time-of-flight
759
              inst->tofAnc[tof_idx] = (uint32_t)tof;
760
            }
761

    
762
            //reset the response count
763
            if(inst->rxResps[inst->rxRespsIdx] >= 0) {
764
              inst->rxResps[inst->rxRespsIdx] = -1 * inst->rxResps[inst->rxRespsIdx];
765
              if(inst->rxResps[inst->rxRespsIdx] == 0) //as A0 will have this as 0 when ranging to A1
766
                inst->rxResps[inst->rxRespsIdx] = -1 ;
767
            }
768

    
769
            instancesetantennadelays(); //this will update the antenna delay if it has changed
770
            instancesettxpower(); // configure TX power if it has changed
771

    
772
            inst->testAppState = TA_RXE_WAIT ;              // wait for next frame
773

    
774
          }
775
          break; //RTLS_DEMO_MSG_TAG_FINAL
776

    
777

    
778
          default: {
779
            //only enable receiver when not using double buffering
780
            inst->testAppState = TA_RXE_WAIT ;              // wait for next frame
781
            dwt_setrxaftertxdelay(0);
782

    
783
          }
784
          break;
785
          } //end switch (fcode)
786

    
787
          if(dw_event->msgu.frame[0] & 0x20){
788
            //as we only pass the received frame with the ACK request bit set after the ACK has been sent
789
            instance_getevent(16); //get and clear the ACK sent event
790
          }
791
        } //end else
792

    
793
      }
794
      break ; //end of DWT_SIG_RX_OKAY
795

    
796
      case DWT_SIG_RX_TIMEOUT :{
797

    
798
        event_data_t* dw_event = instance_getevent(17); //get and clear this event
799

    
800
        //printf("PD_DATA_TIMEOUT %d\n", inst->previousState) ;
801

    
802
        //Anchor can time out and then need to send response - so will be in TX pending
803
        if(dw_event->type_pend == DWT_SIG_TX_PENDING) {
804
          inst->testAppState = TA_TX_WAIT_CONF;                                               // wait confirmation
805
          inst->previousState = TA_TXRESPONSE_SENT_TORX ;    //wait for TX confirmation of sent response
806
        }
807
        else if(dw_event->type_pend == DWT_SIG_DW_IDLE) { //if timed out and back in receive then don't process as timeout
808
          inst_processrxtimeout(inst);
809
        }
810
        //else if RX_PENDING then wait for next RX event...
811
        message = 0; //clear the message as we have processed the event
812
      }
813
      break ;
814

    
815
      case DWT_SIG_TX_AA_DONE: //ignore this event - just process the rx frame that was received before the ACK response
816
      case 0:
817
      default :{
818
          if(message) { // == DWT_SIG_TX_DONE)
819
          inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT;
820
        }
821

    
822
        if(inst->done == INST_NOT_DONE_YET) inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT;
823
      }
824
      break;
825

    
826
      }
827
      break ; // end case TA_RX_WAIT_DATA
828
      default:
829
        //printf("\nERROR - invalid state %d - what is going on??\n", inst->testAppState) ;
830
        break;
831
  } // end switch on testAppState
832

    
833
  return inst->done;
834
} // end testapprun()
835

    
836
// -------------------------------------------------------------------------------------------------------------------
837
#if NUM_INST != 1
838
#error These functions assume one instance only
839
#else
840

    
841

    
842
// -------------------------------------------------------------------------------------------------------------------
843
// function to set the fixed reply delay time (in us)
844
//
845
// This sets delay for RX to TX - Delayed Send, and for TX to RX delayed receive (wait for response) functionality,
846
// and the frame wait timeout value to use.  This is a function of data rate, preamble length, and PRF
847

    
848
extern uint8_t dwnsSFDlen[];
849

    
850
void instancesetreplydelay(int delayus) { //delay in us
851

    
852
  int instance = 0;
853
  int margin = 3000; //2000 symbols
854
  int respframe = 0;
855
  int respframe_sy = 0;
856

    
857
  //configure the rx delay receive delay time, it is dependent on the message length
858
  float msgdatalen = 0;
859
  float preamblelen = 0;
860
  int sfdlen = 0;
861
  float x = 0;
862

    
863
  //Set the RX timeouts based on the longest expected message - the Final message
864
  //Poll = 13, Response = 20, Final = 44 bytes
865
  //msgdatalen = TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
866
  msgdatalen = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
867

    
868
  x = (float)ceil((double)(msgdatalen*8)/(double)330.0f);
869

    
870
  msgdatalen = msgdatalen*8 + x*48;
871

    
872
  //add some margin so we don't timeout too soon
873
  margin = 0; //(TAG_FINAL_MSG_LEN - TAG_POLL_MSG_LEN);
874

    
875
  x = (float) ceil((double)(margin*8)/(double)330.0f);
876

    
877
  margin = (int) (margin*8 + x*48);
878

    
879
  //assume PHR length is 172308ns for 110k and 21539ns for 850k/6.81M
880
  if(instance_data[instance].configData.dataRate == DWT_BR_110K) {
881
    msgdatalen *= 8205.13f;
882
    msgdatalen += 172308; // PHR length in nanoseconds
883

    
884
    margin *= 8205.13f;
885

    
886
  }
887
  else if(instance_data[instance].configData.dataRate == DWT_BR_850K) {
888
    msgdatalen *= 1025.64f;
889
    msgdatalen += 21539; // PHR length in nanoseconds
890

    
891
    margin *= 1025.64f;
892
  }
893
  else {
894
    msgdatalen *= 128.21f;
895
    msgdatalen += 21539; // PHR length in nanoseconds
896

    
897
    margin *= 128.21f;
898
  }
899

    
900
  //SFD length is 64 for 110k (always)
901
  //SFD length is 8 for 6.81M, and 16 for 850k, but can vary between 8 and 16 bytes
902
  sfdlen = dwnsSFDlen[instance_data[instance].configData.dataRate];
903

    
904
  switch (instance_data[instance].configData.txPreambLength) {
905
  case DWT_PLEN_4096 : preamblelen = 4096.0f; break;
906
  case DWT_PLEN_2048 : preamblelen = 2048.0f; break;
907
  case DWT_PLEN_1536 : preamblelen = 1536.0f; break;
908
  case DWT_PLEN_1024 : preamblelen = 1024.0f; break;
909
  case DWT_PLEN_512  : preamblelen = 512.0f; break;
910
  case DWT_PLEN_256  : preamblelen = 256.0f; break;
911
  case DWT_PLEN_128  : preamblelen = 128.0f; break;
912
  case DWT_PLEN_64   : preamblelen = 64.0f; break;
913
  }
914

    
915
  //preamble  = plen * (994 or 1018) depending on 16 or 64 PRF
916
  if(instance_data[instance].configData.prf == DWT_PRF_16M) {
917
    preamblelen = (sfdlen + preamblelen) * 0.99359f;
918
  }
919
  else {
920
    preamblelen = (sfdlen + preamblelen) * 1.01763f;
921
  }
922

    
923
  respframe_sy = (16 + (int)((double)((double)preamblelen + ((double)(msgdatalen + margin)/1000.0))/ 1.0256)) ;
924

    
925
  //this is the delay used for the delayed transmit (when sending the response, and final messages)
926
  instance_data[instance].pollTx2FinalTxDelay = convertmicrosectodevicetimeu (delayus);
927
  //the anchor to anchor ranging consist of A0 ranging to A1 and A2 and A1 ranging to A2
928
  //as there are less messages the ranging time is shorter (thus divide by 2)
929
  instance_data[instance].pollTx2FinalTxDelayAnc = convertmicrosectodevicetimeu (delayus/2 + 100);
930

    
931
  //this is the delay the anchors 1, 2, etc.. will send the response back at...
932
  //anchor 2 will have the delay set to 2 * fixedReplyDelayAnc
933
  //andhor 3 will have the delay set to 3 * fixedReplyDelayAnc and so on...
934
  //this delay depends on how quickly the tag can receive and process the message from previous anchor
935
  //(and also the frame length of course)
936
  respframe = (int)((double)preamblelen + ((double)msgdatalen/1000.0)); //length of response frame (micro seconds)
937
  if(instance_data[instance].configData.dataRate == DWT_BR_110K) {
938

    
939
    //set the frame wait timeout time - total time the frame takes in symbols
940
    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
941

    
942
    instance_data[instance].fwtoTimeAnc_sy = respframe_sy; //add some margin so we don't timeout too soon
943
    instance_data[instance].fixedReplyDelayAnc = convertmicrosectodevicetimeu (respframe + RX_RESPONSE1_TURNAROUND_110K);
944
    instance_data[instance].fixedReplyDelayAncP = (uint32_t) (((uint64_t) convertmicrosectodevicetimeu ((double)preamblelen)) >> 8) + 16;
945

    
946
    instance_data[instance].ancRespRxDelay = RX_RESPONSE1_TURNAROUND_110K ;
947
  }
948
  else {
949

    
950
    //set the frame wait timeout time - total time the frame takes in symbols
951
    instance_data[instance].fwtoTime_sy = respframe_sy + RX_RESPONSE1_TURNAROUND_6M81; //add some margin because of the resp to resp RX turn on time
952

    
953
    instance_data[instance].fwtoTimeAnc_sy =  respframe_sy;
954
    instance_data[instance].fixedReplyDelayAnc = convertmicrosectodevicetimeu (respframe + RX_RESPONSE1_TURNAROUND_6M81);
955
    instance_data[instance].fixedReplyDelayAncP = (uint32_t) (((uint64_t) convertmicrosectodevicetimeu ((double)preamblelen)) >> 8) + 16;
956

    
957
    instance_data[instance].ancRespRxDelay = RX_RESPONSE1_TURNAROUND_6M81 ;
958
  }
959
}
960

    
961
// -------------------------------------------------------------------------------------------------------------------
962
//
963
// Set Payload parameters for the instance
964
//
965
// -------------------------------------------------------------------------------------------------------------------
966
void instancesetaddresses(uint16_t address) {
967
  int instance = 0 ;
968

    
969
  instance_data[instance].instanceAddress16 = address ;       // copy configurations
970
}
971

    
972

    
973
#endif
974

    
975

    
976

    
977

    
978
/* ==========================================================
979

980
Notes:
981

982
Previously code handled multiple instances in a single console application
983

984
Now have changed it to do a single instance only. With minimal code changes...(i.e. kept [instance] index but it is always 0.
985

986
Windows application should call instance_init() once and then in the "main loop" call instance_run().
987

988
*/
989

    
990

    
991
#endif /* defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 1) */