Statistics
| Branch: | Tag: | Revision:

amiro-lld / source / DW1000 / v1 / deca_instance_common_v1.c @ 69a601a5

History | View | Annotate | Download (65.9 KB)

1
/*! ----------------------------------------------------------------------------
2
 *  @file    instance_common.c
3
 *  @brief   DecaWave application level common instance functions
4
 *
5
 * @attention
6
 *
7
 * Copyright 2015 (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 "module.h"
19
#include <string.h>
20
#include <math.h>
21

    
22

    
23
extern double dwt_getrangebias(uint8_t chan, float range, uint8_t prf);
24

    
25
extern const uint16_t rfDelays[2];
26
extern const uint16_t rfDelaysTREK[2];
27
extern const tx_struct txSpectrumConfig[8];
28

    
29

    
30

    
31
// -------------------------------------------------------------------------------------------------------------------
32
// Deca Calibration Values
33
// -------------------------------------------------------------------------------------------------------------------
34

    
35
#define DWT_PRF_64M_RFDLY   (514.462f)
36
#define DWT_PRF_16M_RFDLY   (513.9067f)
37

    
38
// -------------------------------------------------------------------------------------------------------------------
39

    
40
//The table below specifies the default TX spectrum configuration parameters... this has been tuned for DW EVK hardware units
41
//the table is set for smart power - see below in the instance_config function how this is used when not using smart power
42
const tx_struct txSpectrumConfig[8] =
43
{
44
    //Channel 0 ----- this is just a place holder so the next array element is channel 1
45
    {
46
            0x0,   //0
47
            {
48
                    0x0, //0
49
                    0x0 //0
50
            }
51
    },
52
    //Channel 1
53
    {
54
            0xc9,   //PG_DELAY
55
            {
56
                    0x15355575, //16M prf power
57
                    0x07274767 //64M prf power
58
            }
59

    
60
    },
61
    //Channel 2
62
    {
63
            0xc2,   //PG_DELAY
64
            {
65
                    0x15355575, //16M prf power
66
                    0x07274767 //64M prf power
67
            }
68
    },
69
    //Channel 3
70
    {
71
            0xc5,   //PG_DELAY
72
            {
73
                    0x0f2f4f6f, //16M prf power
74
                    0x2b4b6b8b //64M prf power
75
            }
76
    },
77
    //Channel 4
78
    {
79
            0x95,   //PG_DELAY
80
            {
81
                    0x1f1f3f5f, //16M prf power
82
                    0x3a5a7a9a //64M prf power
83
            }
84
    },
85
    //Channel 5
86
    {
87
            0xc0,   //PG_DELAY
88
            {
89
                    0x0E082848, //16M prf power
90
                    0x25456585 //64M prf power
91
            }
92
    },
93
    //Channel 6 ----- this is just a place holder so the next array element is channel 7
94
    {
95
            0x0,   //0
96
            {
97
                    0x0, //0
98
                    0x0 //0
99
            }
100
    },
101
    //Channel 7
102
    {
103
            0x93,   //PG_DELAY
104
            {
105
                    0x32527292, //16M prf power
106
                    0x5171B1d1 //64M prf power
107
            }
108
    }
109
};
110

    
111
//these are default antenna delays for EVB1000, these can be used if there is no calibration data in the DW1000,
112
//or instead of the calibration data
113
const uint16_t rfDelays[2] = {
114
        (uint16_t) ((DWT_PRF_16M_RFDLY/ 2.0) * 1e-9 / DWT_TIME_UNITS),//PRF 16
115
        (uint16_t) ((DWT_PRF_64M_RFDLY/ 2.0) * 1e-9 / DWT_TIME_UNITS)
116
};
117

    
118
//these are default TREK Tag/Anchor antenna delays
119
const uint16_t rfDelaysTREK[2] = {
120
        (uint16_t) ((514.83f/ 2.0) * 1e-9 / DWT_TIME_UNITS),//channel 2
121
        (uint16_t) ((514.65f/ 2.0) * 1e-9 / DWT_TIME_UNITS) //channel 5
122
};
123

    
124
//int instance_starttxtest(int framePeriod)
125
//{
126
//    //define some test data for the tx buffer
127
//    uint8 msg[127] = "The quick brown fox jumps over the lazy dog. The quick brown fox jumps over the lazy dog. The quick brown fox jumps over the l";
128

    
129
//    //NOTE: SPI frequency must be < 3MHz
130
//    port_set_dw1000_slowrate();  //max SPI before PLLs configured is ~4M
131

    
132
//    // the value here 0x1000 gives a period of 32.82 ┬Ás
133
//    //this is setting 0x1000 as frame period (125MHz clock cycles) (time from Tx en - to next - Tx en)
134
//    dwt_configcontinuousframemode(framePeriod);
135

    
136
//    dwt_writetxdata(127, (uint8 *)  msg, 0) ;
137
//    dwt_writetxfctrl(127, 0, 0);
138

    
139
//    //to start the first frame - set TXSTRT
140
//    dwt_starttx(DWT_START_TX_IMMEDIATE);
141

    
142
//    //measure the power
143
//    //Spectrum Analyser set:
144
//    //FREQ to be channel default e.g. 3.9936 GHz for channel 2
145
//    //SPAN to 1GHz
146
//    //SWEEP TIME 1s
147
//    //RBW and VBW 1MHz
148
//    //measure channel power
149

    
150
//    return DWT_SUCCESS ;
151
//}
152

    
153
// -------------------------------------------------------------------------------------------------------------------
154
//      Data Definitions
155
// -------------------------------------------------------------------------------------------------------------------
156

    
157
instance_data_t instance_data[NUM_INST] ;
158

    
159
static double inst_tdist[MAX_TAG_LIST_SIZE] ;
160
static double inst_idist[MAX_ANCHOR_LIST_SIZE] ;
161
static double inst_idistraw[MAX_ANCHOR_LIST_SIZE] ;
162

    
163
// TODO: Instance_data declaration in .c. Not on .h (extern only)
164
instance_data_t instance_data[NUM_INST] ;
165

    
166
/* TODO: Custom data declarations */
167
typedef struct
168
{
169
    uint32_t      deviceID ;
170
    uint8_t       chan;               // added chan here - used in the reading of acc
171
    uint16_t      rfrxDly;            // rf delay (delay though the RF blocks before the signal comes out of the antenna i.e. "antenna delay")
172
    uint16_t      rftxDly;            // rf delay (delay though the RF blocks before the signal comes out of the antenna i.e. "antenna delay")
173
    uint32_t      antennaDly;         // antenna delay read from OTP 64 PRF value is in high 16 bits and 16M PRF in low 16 bits
174
    uint32_t      antennaCals[4];     // antenna delays for the TREKs (Anchor high 16-bits, Tag low 16-bits)
175
    uint32_t      txPowCfg[12];       // stores the Tx power configuration read from OTP (6 channels consecutively with PRF16 then 64, e.g. Ch 1 PRF16 is index 0 and 64 index 1)
176
    uint32_t      states[3] ;         //MP workaround debug states register
177
    uint8_t       statescount ;
178
    int           prfIndex ;
179
    uint32_t      ldoTune ;           //low 32 bits of LDO tune value
180
} platform_local_data_t ;
181

    
182
static platform_local_data_t platformLocalData ; // Static local device data
183

    
184
// -------------------------------------------------------------------------------------------------------------------
185
// Functions
186
// -------------------------------------------------------------------------------------------------------------------
187

    
188
// -------------------------------------------------------------------------------------------------------------------
189
// convert microseconds to device time
190
uint64_t convertmicrosectodevicetimeu (double microsecu){
191
  uint64_t dt;
192
  long double dtime;
193

    
194
  dtime = (microsecu / (double) DWT_TIME_UNITS) / 1e6 ;
195

    
196
  dt =  (uint64_t) (dtime) ;
197

    
198
  return dt;
199
}
200

    
201
double convertdevicetimetosec(int32_t dt){
202
  double f = 0;
203

    
204
  f =  dt * DWT_TIME_UNITS ;  // seconds #define TIME_UNITS          (1.0/499.2e6/128.0) = 15.65e-12
205

    
206
  return f ;
207
}
208

    
209
#pragma GCC optimize ("O3")
210
int reportTOF(int idx, uint32_t tofx){
211
  double distance ;
212
  double distance_to_correct;
213
  double tof ;
214
  int32_t tofi ;
215

    
216
  // check for negative results and accept them making them proper negative integers
217
  tofi = (int32_t) tofx ; // make it signed
218
  if (tofi > 0x7FFFFFFF){  // close up TOF may be negative
219
    tofi -= 0x80000000 ;  //
220
  }
221

    
222
  // convert to seconds (as floating point)
223
  tof = convertdevicetimetosec(tofi) ;          //this is divided by 4 to get single time of flight
224
  inst_idistraw[idx] = distance = tof * SPEED_OF_LIGHT;
225

    
226
#if (CORRECT_RANGE_BIAS == 1)
227
  //for the 6.81Mb data rate we assume gating gain of 6dB is used,
228
  //thus a different range bias needs to be applied
229
  //if(inst->configData.dataRate == DWT_BR_6M8)
230
  if(instance_data[0].configData.smartPowerEn){
231
    //1.31 for channel 2 and 1.51 for channel 5
232
    if(instance_data[0].configData.chan == 5){
233
      distance_to_correct = distance/1.51;
234
    }
235
    else{   //channel 2
236
      distance_to_correct = distance/1.31;
237
    }
238
  }
239
  else{
240
    distance_to_correct = distance;
241
  }
242

    
243
  distance = distance - dwt_getrangebias(instance_data[0].configData.chan, (float) distance_to_correct, instance_data[0].configData.prf);
244
#endif
245

    
246
  if ((distance < 0) || (distance > 20000.000))    // discard any results less than <0 cm or >20 km
247
    return 0;
248

    
249
  inst_idist[idx] = distance;
250

    
251
  instance_data[0].longTermRangeCount++ ;                          // for computing a long term average
252

    
253
  return 1;
254
}// end of reportTOF
255

    
256
void setTagDist(int tidx, int aidx){
257
  inst_tdist[tidx] = inst_idist[aidx];
258
}
259

    
260
double getTagDist(int idx){
261
  return inst_tdist[idx];
262
}
263

    
264
void clearDistTable(int idx){
265
  inst_idistraw[idx] = 0;
266
  inst_idist[idx] = 0;
267
}
268

    
269
void instancecleardisttableall(void){
270
  int i;
271

    
272
  for(i=0; i<MAX_ANCHOR_LIST_SIZE; i++)  {
273
    inst_idistraw[i] = 0xffff;
274
    inst_idist[i] = 0xffff;
275
  }
276
}
277

    
278
// -------------------------------------------------------------------------------------------------------------------
279
#if NUM_INST != 1
280
#error These functions assume one instance only
281
#else
282

    
283

    
284
// -------------------------------------------------------------------------------------------------------------------
285
// Set this instance role as the Tag, Anchor or Listener
286
void instancesetrole(int inst_mode){
287
  // assume instance 0, for this
288
  instance_data[0].mode =  inst_mode;                   // set the role
289
}
290

    
291
int instancegetrole(void){
292
  return instance_data[0].mode;
293
}
294

    
295
int instancenewrange(void){
296
  int x = instance_data[0].newRange;
297
  instance_data[0].newRange = TOF_REPORT_NUL;
298
  return x;
299
}
300

    
301
int instancenewrangeancadd(void){
302
  return instance_data[0].newRangeAncAddress;
303
}
304

    
305
int instancenewrangetagadd(void){
306
  return instance_data[0].newRangeTagAddress;
307
}
308

    
309
int instancenewrangetim(void){
310
  return instance_data[0].newRangeTime;
311
}
312

    
313
// -------------------------------------------------------------------------------------------------------------------
314
// function to clear counts/averages/range values
315
//
316
void instanceclearcounts(void){
317
  int instance = 0 ;
318
  int i= 0 ;
319

    
320
  instance_data[instance].rxTimeouts = 0 ;
321

    
322
  dwt_configeventcounters(1); //enable and clear - NOTE: the counters are not preserved when in DEEP SLEEP
323

    
324
  instance_data[instance].frameSN = 0;
325

    
326
  instance_data[instance].longTermRangeCount  = 0;
327

    
328

    
329
  for(i=0; i<MAX_ANCHOR_LIST_SIZE; i++){
330
    instance_data[instance].tofArray[i] = INVALID_TOF;
331
  }
332

    
333
  for(i=0; i<MAX_TAG_LIST_SIZE; i++){
334
    instance_data[instance].tof[i] = INVALID_TOF;
335
  }
336

    
337
} // end instanceclearcounts()
338

    
339

    
340
// -------------------------------------------------------------------------------------------------------------------
341
// function to initialise instance structures
342
//
343
// Returns 0 on success and -1 on error
344
int instance_init(void){
345
  int instance = 0 ;
346
  int i;
347
  int result;
348

    
349
  instance_data[instance].mode =  ANCHOR;                                // assume listener,
350
  instance_data[instance].testAppState = TA_INIT ;
351
  instance_data[instance].instToSleep = FALSE;
352

    
353

    
354
  // Reset the IC (might be needed if not getting here from POWER ON)
355
  // ARM code: Remove soft reset here as using hard reset in the inittestapplication() in the main.c file
356
  //dwt_softreset();
357

    
358
  //this initialises DW1000 and uses specified configurations from OTP/ROM
359
  result = dwt_initialise(DWT_LOADUCODE) ;
360

    
361
  //this is platform dependent - only program if DW EVK/EVB
362
  dwt_setleds(3) ; //configure the GPIOs which control the leds on EVBs
363

    
364
  if (DWT_SUCCESS != result){
365
    return (-1) ;   // device initialise has failed
366
  }
367

    
368

    
369
  instanceclearcounts() ;
370

    
371
  instance_data[instance].panID = 0xdeca ;
372

    
373
  instance_data[instance].wait4ack = 0;
374
  instance_data[instance].stopTimer = 0;
375
  instance_data[instance].instanceTimerEn = 0;
376

    
377
  instance_clearevents();
378

    
379
  //dwt_geteui(instance_data[instance].eui64);
380
  memset(instance_data[instance].eui64, 0, ADDR_BYTE_SIZE_L);
381

    
382
  instance_data[instance].tagSleepCorrection = 0;
383

    
384
//  dwt_setautorxreenable(0); //disable auto RX re-enable
385
  dwt_setdblrxbuffmode(0); //disable double RX buffer
386

    
387
  // if using auto CRC check (DWT_INT_RFCG and DWT_INT_RFCE) are used instead of DWT_INT_RDFR flag
388
  // other errors which need to be checked (as they disable receiver) are
389
  //dwt_setinterrupt(DWT_INT_TFRS | DWT_INT_RFCG | (DWT_INT_SFDT | DWT_INT_RFTO /*| DWT_INT_RXPTO*/), 1);
390
  dwt_setinterrupt(DWT_INT_TFRS | DWT_INT_RFCG | (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO | DWT_INT_RXPTO), 1);
391

    
392
  dwt_setcallbacks(instance_txcallback, instance_rxcallback, instance_rxtimeoutcallback,instance_rxerrorcallback );
393

    
394
  instance_data[instance].monitor = 0;
395

    
396
  instance_data[instance].lateTX = 0;
397
  instance_data[instance].lateRX = 0;
398

    
399
  instance_data[instance].responseTO = -1; //initialise
400
  for(i=0; i<256; i++)  {
401
    instance_data[instance].rxResps[i] = -10;
402
  }
403

    
404
  instance_data[instance].delayedReplyTime = 0;
405

    
406
  return 0 ;
407
}
408

    
409
// -------------------------------------------------------------------------------------------------------------------
410
//
411
// Return the Device ID register value, enables higher level validation of physical device presence
412
//
413

    
414
uint32_t instancereaddeviceid(void){
415
  return dwt_readdevid() ;
416
}
417

    
418

    
419
// -------------------------------------------------------------------------------------------------------------------
420
//
421
// function to allow application configuration be passed into instance and affect underlying device operation
422
//
423
void instance_config(instanceConfig_t *config, sfConfig_t *sfConfig){
424
  int instance = 0 ;
425
  uint32_t power = 0;
426
  uint8_t otprev ;
427

    
428
  instance_data[instance].configData.chan = config->channelNumber ;
429
  instance_data[instance].configData.rxCode =  config->preambleCode ;
430
  instance_data[instance].configData.txCode = config->preambleCode ;
431
  instance_data[instance].configData.prf = config->pulseRepFreq ;
432
  instance_data[instance].configData.dataRate = config->dataRate ;
433
  instance_data[instance].configData.txPreambLength = config->preambleLen ;
434
  instance_data[instance].configData.rxPAC = config->pacSize ;
435
  instance_data[instance].configData.nsSFD = config->nsSFD ;
436
  instance_data[instance].configData.phrMode = DWT_PHRMODE_STD ;
437
  instance_data[instance].configData.sfdTO = config->sfdTO;
438

    
439
  //the DW1000 will automatically use gating gain for frames < 1ms duration (i.e. 6.81Mbps data rate)
440
  //smartPowerEn should be set based on the frame length, but we can also use dtaa rate.
441
  if(instance_data[instance].configData.dataRate == DWT_BR_6M8)  {
442
    instance_data[instance].configData.smartPowerEn = 1;
443
  }
444
  else{
445
    instance_data[instance].configData.smartPowerEn = 0;
446
  }
447

    
448
  //configure the channel parameters
449
  dwt_configure(&instance_data[instance].configData) ;
450

    
451
  instance_data[instance].configTX.PGdly = txSpectrumConfig[config->channelNumber].PGdelay ;    //Default
452

    
453
  //firstly check if there are calibrated TX power value in the DW1000 OTP
454
  power = dwt_getotptxpower(config->pulseRepFreq, instance_data[instance].configData.chan);
455

    
456
  if((power == 0x0) || (power == 0xFFFFFFFF)) {    //if there are no calibrated values... need to use defaults
457
    power = txSpectrumConfig[config->channelNumber].txPwr[config->pulseRepFreq- DWT_PRF_16M];
458
  }
459

    
460
  //Configure TX power
461
  instance_data[instance].configTX.power = power;
462

    
463
  //configure the tx spectrum parameters (power and PG delay)
464
  dwt_configuretxrf(&instance_data[instance].configTX);
465

    
466
  otprev = dwt_otprevision() ;  // this revision tells us how OTP is programmed.
467

    
468
  if ((2 == otprev) || (3 == otprev))  {  // board is calibrated with TREK1000 with antenna delays set for each use case)
469
    uint8_t mode = (instance_data[instance].mode == ANCHOR ? 1 : 0);
470
    uint8_t chanindex = 0;
471

    
472
    instance_data[instance].txAntennaDelay
473
    = dwt_getTREKOTPantennadelay(mode,
474
                                 instance_data[instance].configData.chan,
475
                                 instance_data[instance].configData.dataRate) ;
476

    
477
    // if nothing was actually programmed then set a reasonable value anyway
478
    if ((instance_data[instance].txAntennaDelay == 0)
479
        || (instance_data[instance].txAntennaDelay == 0xffff)){
480
      if(instance_data[instance].configData.chan == 5){
481
        chanindex = 1;
482
      }
483

    
484
      instance_data[instance].txAntennaDelay = rfDelaysTREK[chanindex];
485
    }
486

    
487
  }
488
  else {  // assume it is older EVK1000 programming.
489
    //get the antenna delay that was read from the OTP calibration area
490
    instance_data[instance].txAntennaDelay = dwt_readantennadelay(config->pulseRepFreq) >> 1;
491

    
492
    // if nothing was actually programmed then set a reasonable value anyway
493
    if ((instance_data[instance].txAntennaDelay == 0)
494
        || (instance_data[instance].txAntennaDelay == 0xffff)){
495
      instance_data[instance].txAntennaDelay = rfDelays[config->pulseRepFreq - DWT_PRF_16M];
496
    }
497
  }
498

    
499
  // -------------------------------------------------------------------------------------------------------------------
500
  // set the antenna delay, we assume that the RX is the same as TX.
501
  dwt_setrxantennadelay(instance_data[instance].txAntennaDelay);
502
  dwt_settxantennadelay(instance_data[instance].txAntennaDelay);
503

    
504
  instance_data[instance].rxAntennaDelay = instance_data[instance].txAntennaDelay;
505

    
506
  if(config->preambleLen == DWT_PLEN_64)  {  //if preamble length is 64
507
    //reduce SPI to < 3MHz
508
    setHighSpeed_SPI(FALSE);  //TODO: workaround using flag to change speed
509
    dwt_loadopsettabfromotp(0);
510
    //increase SPI to max
511
    setHighSpeed_SPI(TRUE);  //TODO: workaround using flag to change speed
512
  }
513

    
514
  instancesettagsleepdelay(sfConfig->pollSleepDly); //set the Tag sleep time
515
  instance_data[instance].sframePeriod = sfConfig->sfPeriod;
516
  instance_data[instance].slotPeriod = sfConfig->slotPeriod;
517
  instance_data[instance].tagSleepRnd = sfConfig->slotPeriod;
518
  instance_data[instance].numSlots = sfConfig->numSlots;
519

    
520
  //last two slots are used for anchor to anchor ranging
521
  instance_data[instance].a0SlotTime = (instance_data[instance].numSlots-2) * instance_data[instance].slotPeriod;
522

    
523
  //set the default response delays
524
  instancesetreplydelay(sfConfig->replyDly);
525

    
526
}
527

    
528
// -------------------------------------------------------------------------------------------------------------------
529
// function to set the tag sleep time (in ms)
530
//
531
void instancesettagsleepdelay(int sleepdelay) {  //sleep in ms
532
  int instance = 0 ;
533
  instance_data[instance].tagSleepTime_ms = sleepdelay; //subtract the micro system delays (time it takes to switch states etc.)
534
}
535

    
536

    
537
int instancegetrnum(void) {  //get ranging number
538
  return instance_data[0].rangeNum;
539
}
540

    
541
int instancegetrnuma(int idx){  //get ranging number
542
  return instance_data[0].rangeNumA[idx];
543
}
544

    
545
int instancegetrnumanc(int idx){  //get ranging number
546
  return instance_data[0].rangeNumAAnc[idx];
547
}
548

    
549
int instancegetlcount(void) {  //get count of ranges used for calculation of lt avg
550
  int x = instance_data[0].longTermRangeCount;
551

    
552
  return (x);
553
}
554

    
555
double instancegetidist(int idx) {  //get instantaneous range
556
  double x ;
557

    
558
  idx &= (MAX_ANCHOR_LIST_SIZE - 1);
559

    
560
  x = inst_idist[idx];
561

    
562
  return (x);
563
}
564

    
565
double instancegetidistraw(int idx) { //get instantaneous range (uncorrected)
566
  double x ;
567

    
568
  idx &= (MAX_ANCHOR_LIST_SIZE - 1);
569

    
570
  x = inst_idistraw[idx];
571

    
572
  return (x);
573
}
574

    
575
int instancegetidist_mm(int idx) {  //get instantaneous range
576
  int x ;
577

    
578
  idx &= (MAX_ANCHOR_LIST_SIZE - 1);
579

    
580
  x = (int)(inst_idist[idx]*1000);
581

    
582
  return (x);
583
}
584

    
585
int instancegetidistraw_mm(int idx) {  //get instantaneous range (uncorrected)
586
  int x ;
587

    
588
  idx &= (MAX_ANCHOR_LIST_SIZE - 1);
589

    
590
  x = (int)(inst_idistraw[idx]*1000);
591

    
592
  return (x);
593
}
594

    
595
void instance_backtoanchor(instance_data_t *inst){
596
  //stay in RX and behave as anchor
597
  inst->testAppState = TA_RXE_WAIT ;
598
  inst->mode = ANCHOR ;
599
  dwt_setrxtimeout(0);
600
  dwt_setpreambledetecttimeout(0);
601
  dwt_setrxaftertxdelay(0);
602
}
603

    
604

    
605
#pragma GCC optimize ("O3")
606
void inst_processrxtimeout(instance_data_t *inst){
607

    
608
  //inst->responseTimeouts ++ ;
609
  inst->rxTimeouts ++ ;
610
  inst->done = INST_NOT_DONE_YET;
611

    
612
  if(inst->mode == ANCHOR) { //we did not receive the final - wait for next poll
613
    //only enable receiver when not using double buffering
614
    inst->testAppState = TA_RXE_WAIT ;              // wait for next frame
615
    dwt_setrxtimeout(0);
616
  }
617

    
618
  else if(inst->mode == TAG) {
619
    //if tag times out - no response (check if we are to send a final)
620
    //send the final only if it has received response from anchor 0
621
    if((inst->previousState == TA_TXPOLL_WAIT_SEND) && ((inst->rxResponseMask & 0x1) == 0)) {
622
      inst->instToSleep = TRUE ; //set sleep to TRUE so that tag will go to DEEP SLEEP before next ranging attempt
623
      inst->testAppState = TA_TXE_WAIT ;
624
      inst->nextState = TA_TXPOLL_WAIT_SEND ;
625
    }
626
    else if (inst->previousState == TA_TXFINAL_WAIT_SEND) {  //got here from main (error sending final - handle as timeout)
627
      dwt_forcetrxoff();        //this will clear all events
628
      inst->instToSleep = TRUE ;
629
      // initiate the re-transmission of the poll that was not responded to
630
      inst->testAppState = TA_TXE_WAIT ;
631
      inst->nextState = TA_TXPOLL_WAIT_SEND ;
632
    }
633
    else {  //send the final
634
      // initiate the re-transmission of the poll that was not responded to
635
      inst->testAppState = TA_TXE_WAIT ;
636
      inst->nextState = TA_TXFINAL_WAIT_SEND ;
637
    }
638

    
639
  }
640
  else {  //ANCHOR_RNG
641
    //no Response form the other anchor
642
    if(
643
        ((inst->previousState == TA_TXPOLL_WAIT_SEND)
644
            && ((A1_ANCHOR_ADDR == inst->instanceAddress16) && ((inst->rxResponseMaskAnc & 0x4) == 0)))
645
            ||
646
            ((inst->previousState == TA_TXPOLL_WAIT_SEND)
647
                && ((GATEWAY_ANCHOR_ADDR == inst->instanceAddress16) && ((inst->rxResponseMaskAnc & 0x2) == 0)))
648
    )  {
649
      instance_backtoanchor(inst);
650
    }
651
    else if (inst->previousState == TA_TXFINAL_WAIT_SEND) {  //got here from main (error ending final - handle as timeout)
652
      instance_backtoanchor(inst);
653
    }
654
    else {  //send the final
655
      // initiate the re-transmission of the poll that was not responded to
656
      inst->testAppState = TA_TXE_WAIT ;
657
      inst->nextState = TA_TXFINAL_WAIT_SEND ;
658
    }
659
  }
660

    
661
  //timeout - disable the radio (if using SW timeout the rx will not be off)
662
  //dwt_forcetrxoff() ;
663
}
664

    
665
//
666
// NB: This function is called from the (TX) interrupt handler
667
//
668
#pragma GCC optimize ("O3")
669
void instance_txcallback(const dwt_cb_data_t *txd){
670

    
671
  (void) txd;
672
  int instance = 0;
673
  uint8_t txTimeStamp[5] = {0, 0, 0, 0, 0};
674
//  uint8 txevent = txd->event;
675
  event_data_t dw_event;
676

    
677
  dw_event.uTimeStamp = portGetTickCnt();
678

    
679
//  chprintf((BaseSequentialStream *)&SD2, " TX callback! \r\n");
680

    
681
//  if(txevent == DWT_SIG_TX_DONE) {
682
    //uint32_t txtimestamp = 0;
683

    
684
    //NOTE - we can only get TX good (done) while here
685
    //dwt_readtxtimestamp((uint8*) &instance_data[instance].txu.txTimeStamp);
686

    
687
    dwt_readtxtimestamp(txTimeStamp) ;
688
    dw_event.timeStamp32l = (uint32_t)txTimeStamp[0] + ((uint32_t)txTimeStamp[1] << 8) + ((uint32_t)txTimeStamp[2] << 16) + ((uint32_t)txTimeStamp[3] << 24);
689
    dw_event.timeStamp = txTimeStamp[4];
690
    dw_event.timeStamp <<= 32;
691
    dw_event.timeStamp += dw_event.timeStamp32l;
692
    dw_event.timeStamp32h = ((uint32_t)txTimeStamp[4] << 24) + (dw_event.timeStamp32l >> 8);
693

    
694
    instance_data[instance].stopTimer = 0;
695

    
696
    dw_event.rxLength = instance_data[instance].psduLength;
697
    dw_event.type =  0;
698
    dw_event.type_pend =  0;
699
    dw_event.type_save = DWT_SIG_TX_DONE;
700

    
701
    memcpy((uint8_t *)&dw_event.msgu.frame[0], (uint8_t *)&instance_data[instance].msg_f, instance_data[instance].psduLength);
702

    
703
    instance_putevent(dw_event, DWT_SIG_TX_DONE);
704

    
705
    instance_data[instance].txMsgCount++;
706
//  }
707
//  else if(txevent == DWT_SIG_TX_AA_DONE)  {
708
//    //auto ACK confirmation
709
//    dw_event.rxLength = 0;
710
//    dw_event.type =  0;
711
//    dw_event.type_save = DWT_SIG_TX_AA_DONE;
712
//
713
//    instance_putevent(dw_event, DWT_SIG_TX_AA_DONE);
714
//
715
//    //printf("TX AA time %f ecount %d\n",convertdevicetimetosecu(instance_data[instance].txu.txTimeStamp), instance_data[instance].dweventCnt);
716
//  }
717

    
718
  instance_data[instance].monitor = 0;
719
}
720

    
721
/**
722
 * @brief function to re-enable the receiver and also adjust the timeout before sending the final message
723
 * if it is time so send the final message, the callback will notify the application, else the receiver is
724
 * automatically re-enabled
725
 *
726
 * this function is only used for tag when ranging to other anchors
727
 */
728
uint8_t tagrxreenable(uint16_t sourceAddress){
729
  uint8_t type_pend = DWT_SIG_DW_IDLE;
730
  uint8_t anc = sourceAddress & 0x3;
731
  int instance = 0;
732

    
733
  switch(anc){
734
  //if we got Response from anchor 3 - this is the last expected response - send the final
735
  case 3:
736
    type_pend = DWT_SIG_DW_IDLE;
737
    break;
738

    
739
    //if we got Response from anchor 0, 1, or 2 - go back to wait for next anchor's response
740
  case 0:
741
  case 1:
742
  case 2:
743
  default:
744
    if(instance_data[instance].responseTO > 0) {  //can get here as result of error frame so need to check
745
      dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTime_sy * instance_data[instance].responseTO); //reconfigure the timeout
746
      dwt_rxenable(DWT_START_RX_IMMEDIATE) ;
747
      type_pend = DWT_SIG_RX_PENDING ;
748
    }
749
    else //last response was not received (got error/frame was corrupt)
750
    {
751
      type_pend = DWT_SIG_DW_IDLE; //report timeout - send the final
752
    }
753
    break;
754
  }
755

    
756
  return type_pend;
757
}
758

    
759
/**
760
 * @brief function to re-enable the receiver and also adjust the timeout before sending the final message
761
 * if it is time so send the final message, the callback will notify the application, else the receiver is
762
 * automatically re-enabled
763
 *
764
 * this function is only used for anchors (having a role of ANCHOR_RNG) when ranging to other anchors
765
 */
766
uint8_t ancsendfinalorrxreenable(uint16_t sourceAddress){
767
  uint8_t type_pend = DWT_SIG_DW_IDLE;
768
  uint8_t anc = sourceAddress & 0x3;
769
  int instance = 0;
770

    
771
  if(instance_data[instance].instanceAddress16 == GATEWAY_ANCHOR_ADDR) {
772
    switch(anc) {
773
    //if we got Response from anchor 1 - go back to wait for next anchor's response
774
    case 1:
775
      dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTime_sy); //reconfigure the timeout
776
      dwt_rxenable(DWT_START_RX_IMMEDIATE) ;
777
      type_pend = DWT_SIG_RX_PENDING ;
778
      break;
779

    
780
      //if we got Response from anchor 2 - this is the last expected response - send the final
781
    case 2:
782
    default:
783
      type_pend = DWT_SIG_DW_IDLE;
784
      break;
785
    }
786
  }
787

    
788
  if(instance_data[instance].instanceAddress16 == A1_ANCHOR_ADDR){
789
    switch(anc)
790
    {
791
    //if we got Response from anchor 2 - this is the last expected response - send the final
792
    case 2:
793
    default:
794
      type_pend = DWT_SIG_DW_IDLE;
795
      break;
796
    }
797
  }
798
  return type_pend;
799
}
800

    
801
/**
802
 * @brief this function either enables the receiver (delayed)
803
 *
804
 **/
805
void ancenablerx(void){
806
  int instance = 0;
807
  //subtract preamble length
808
  dwt_setdelayedtrxtime(instance_data[instance].delayedReplyTime - instance_data[instance].fixedReplyDelayAncP) ;
809
  if(dwt_rxenable(DWT_START_RX_DELAYED)) {  //delayed rx
810
    //if the delayed RX failed - time has passed - do immediate enable
811
    //led_on(LED_PC9);
812
    dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTimeAnc_sy*2); //reconfigure the timeout before enable
813
    //longer timeout as we cannot do delayed receive... so receiver needs to stay on for longer
814
    dwt_rxenable(DWT_START_RX_IMMEDIATE);
815
    dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTimeAnc_sy); //restore the timeout for next RX enable
816
    instance_data[instance].lateRX++;
817
    //led_off(LED_PC9);
818
  }
819

    
820
}
821

    
822
/**
823
 * @brief this function either re-enables the receiver (delayed or immediate) or transmits the response frame
824
 *
825
 * @param the sourceAddress is the address of the sender of the current received frame
826
 * @param ancToAncTWR == 1 means that the anchor is ranging to another anchor, if == 0 then ranging to a tag
827
 *
828
 */
829
#pragma GCC optimize ("O0")
830
uint8_t anctxorrxreenable(uint16_t sourceAddress, int ancToAncTWR){
831
  uint8_t type_pend = DWT_SIG_DW_IDLE;
832
  int sendResp = 0;
833
  int instance = 0;
834

    
835
  if(instance_data[instance].responseTO == 0) {  //go back to RX without TO - ranging has finished. (wait for Final but no TO)
836
    dwt_setrxtimeout(0); //reconfigure the timeout
837
    dwt_setpreambledetecttimeout(0);
838
  }
839

    
840
  if((ancToAncTWR & 1) == 1)  {
841
    if(instance_data[instance].responseTO == 1) {  //if one response left to receive (send a response now)
842
      sendResp = 1;
843
    }
844
    //if A0 or A3 go back to RX as they do not send any responses when Anchor to Anchor ranging
845
    if((instance_data[instance].gatewayAnchor)
846
        || (instance_data[instance].shortAdd_idx == 3)) { //if this is anchor ID 3 do not reply to anchor poll
847
      dwt_setrxtimeout(0);
848
      dwt_rxenable(DWT_START_RX_IMMEDIATE);
849
      return DWT_SIG_RX_PENDING ;
850
    }
851
  }
852

    
853
  //configure delayed reply time (this is incremented for each received frame) it is timed from Poll rx time
854
  instance_data[instance].delayedReplyTime += (instance_data[instance].fixedReplyDelayAnc >> 8);
855

    
856
  //this checks if to send a frame
857
  if((((ancToAncTWR & 1) == 0) && ((instance_data[instance].responseTO + instance_data[instance].shortAdd_idx) == NUM_EXPECTED_RESPONSES)) //it's our turn to tx
858
      || (sendResp == 1)) {
859
    //led_on(LED_PC9);
860
    //response is expected
861
    instance_data[instance].wait4ack = DWT_RESPONSE_EXPECTED; //re has/will be re-enabled
862

    
863
    dwt_setdelayedtrxtime(instance_data[instance].delayedReplyTime) ;
864
    if(dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED)) {
865
      //if TX has failed - we need to re-enable RX for the next response or final reception...
866
      dwt_setrxaftertxdelay(0);
867
      instance_data[instance].wait4ack = 0; //clear the flag as the TX has failed the TRX is off
868
      instance_data[instance].lateTX++;
869
      instance_data[instance].delayedReplyTime += 2*(instance_data[instance].fixedReplyDelayAnc >> 8); //to take into account W4R
870
      ancenablerx();
871
      type_pend = DWT_SIG_RX_PENDING ;
872
    }
873
    else {
874
      instance_data[instance].delayedReplyTime += (instance_data[instance].fixedReplyDelayAnc >> 8); //to take into account W4R
875
      type_pend = DWT_SIG_TX_PENDING ; // exit this interrupt and notify the application/instance that TX is in progress.
876
      instance_data[instance].timeofTx = portGetTickCnt();
877
      instance_data[instance].monitor = 1;
878
    }
879
    //led_off(LED_PC9);
880
  }
881
  else {  //stay in receive
882
    if(sourceAddress == 0) { //we got here after RX error, as we don't need to TX, we just enable RX
883
      dwt_setrxtimeout(0);
884
      dwt_rxenable(DWT_START_RX_IMMEDIATE);
885
    }
886
    else{
887
      //led_on(LED_PC9);
888
      ancenablerx();
889
      //led_off(LED_PC9);
890
    }
891

    
892
    type_pend = DWT_SIG_RX_PENDING ;
893
  }
894
  //if time to send a response
895

    
896
  return type_pend;
897
}
898

    
899
/**
900
 * @brief this function handles frame error event, it will either signal TO or re-enable the receiver
901
 */
902
void handle_error_unknownframe(event_data_t dw_event){
903
  int instance = 0;
904
  //re-enable the receiver (after error frames as we are not using auto re-enable
905
  //for ranging application rx error frame is same as TO - as we are not going to get the expected frame
906
  if(instance_data[instance].mode == ANCHOR){
907
    //if we are participating in the ranging (i.e. Poll was received)
908
    //and we get an rx error (in one of the responses)
909
    //need to consider this as a timeout as we could be sending our response next and
910
    //the applications needs to know to change the state
911
    //
912
    if(instance_data[instance].responseTO > 0){
913
      instance_data[instance].responseTO--;
914

    
915
      //send a response or re-enable rx
916
      dw_event.type_pend = anctxorrxreenable(0, 0);
917
      dw_event.type = 0;
918
      dw_event.type_save = 0x40 | DWT_SIG_RX_TIMEOUT;
919
      dw_event.rxLength = 0;
920

    
921
      instance_putevent(dw_event, DWT_SIG_RX_TIMEOUT);
922
    }
923
    else{
924
      dwt_setrxtimeout(0); //reconfigure the timeout
925
      dwt_rxenable(DWT_START_RX_IMMEDIATE) ;
926
    }
927
  }
928
  else if(instance_data[instance].mode == LISTENER){
929
    dwt_rxenable(DWT_START_RX_IMMEDIATE) ;
930
  }
931
  else{
932
    instance_data[instance].responseTO--; //got something (need to reduce timeout (for remaining responses))
933

    
934
    dw_event.type_pend = tagrxreenable(0); //check if receiver will be re-enabled or it's time to send the final
935
    dw_event.type = 0;
936
    dw_event.type_save = 0x40 | DWT_SIG_RX_TIMEOUT;
937
    dw_event.rxLength = 0;
938

    
939
    instance_putevent(dw_event, DWT_SIG_RX_TIMEOUT);
940
  }
941
}
942

    
943

    
944
/**
945
 * @brief this function prepares and writes the anchor to anchor response frame into the TX buffer
946
 * it is called after anchor receives a Poll from an anchor
947
 */
948
void ancprepareresponse2(uint16_t sourceAddress, uint8_t srcAddr_index, uint8_t fcode_index, uint8_t *frame){
949
  uint16_t frameLength = 0;
950
  uint8_t tof_idx = (sourceAddress) & 0x3 ;
951
  int instance = 0;
952

    
953
  instance_data[instance].psduLength = frameLength = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
954
  //set the destination address (copy source as this is a reply)
955
  memcpy(&instance_data[instance].msg_f.destAddr[0], &frame[srcAddr_index], ADDR_BYTE_SIZE_S); //remember who to send the reply to (set destination address)
956
  instance_data[instance].msg_f.sourceAddr[0] = instance_data[instance].eui64[0];
957
  instance_data[instance].msg_f.sourceAddr[1] = instance_data[instance].eui64[1];
958
  // Write calculated TOF into response message (get the previous ToF+range number from that anchor)
959
  memcpy(&(instance_data[instance].msg_f.messageData[TOFR]), &instance_data[instance].tofAnc[tof_idx], 4);
960
  instance_data[instance].msg_f.messageData[TOFRN] = instance_data[instance].rangeNumAAnc[tof_idx]; //get the previous range number
961

    
962
  instance_data[instance].rangeNumAAnc[tof_idx] = 0; //clear the entry
963
  instance_data[instance].rangeNumAnc = frame[POLL_RNUM + fcode_index] ;
964
  instance_data[instance].msg_f.seqNum = instance_data[instance].frameSN++;
965

    
966
  //set the delayed rx on time (the final message will be sent after this delay)
967
  dwt_setrxaftertxdelay(instance_data[instance].ancRespRxDelay);  //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
968

    
969
  instance_data[instance].tagSleepCorrection = 0;
970
  instance_data[instance].msg_f.messageData[RES_TAG_SLP0] = 0 ;
971
  instance_data[instance].msg_f.messageData[RES_TAG_SLP1] = 0 ;
972

    
973
  instance_data[instance].msg_f.messageData[FCODE] = RTLS_DEMO_MSG_ANCH_RESP2; //message function code (specifies if message is a poll, response or other...)
974

    
975
  //write the TX data
976
  dwt_writetxfctrl(frameLength, 0, 1);
977
  dwt_writetxdata(frameLength, (uint8_t *)  &instance_data[instance].msg_f, 0) ;        // write the frame data
978

    
979
}
980

    
981
/**
982
 * @brief this function prepares and writes the anchor to tag response frame into the TX buffer
983
 * it is called after anchor receives a Poll from a tag
984
 */
985
void ancprepareresponse(uint16_t sourceAddress, uint8_t srcAddr_index, uint8_t fcode_index, uint8_t *frame, uint32_t uTimeStamp){
986
  uint16_t frameLength = 0;
987
  uint8_t tof_idx = (sourceAddress) & 0x7 ;
988
  int instance = 0;
989

    
990
  instance_data[instance].psduLength = frameLength = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
991
  memcpy(&instance_data[instance].msg_f.destAddr[0], &frame[srcAddr_index], ADDR_BYTE_SIZE_S); //remember who to send the reply to (set destination address)
992
  instance_data[instance].msg_f.sourceAddr[0] = instance_data[instance].eui64[0];
993
  instance_data[instance].msg_f.sourceAddr[1] = instance_data[instance].eui64[1];
994
  // Write calculated TOF into response message (get the previous ToF+range number from that tag)
995
  memcpy(&(instance_data[instance].msg_f.messageData[TOFR]), &instance_data[instance].tof[tof_idx], 4);
996
  instance_data[instance].msg_f.messageData[TOFRN] = instance_data[instance].rangeNumA[tof_idx]; //get the previous range number
997

    
998
  instance_data[instance].rangeNumA[tof_idx] = 0; //clear after copy above...
999
  instance_data[instance].rangeNum = frame[POLL_RNUM+fcode_index] ;
1000
  instance_data[instance].msg_f.seqNum = instance_data[instance].frameSN++;
1001

    
1002
  //we have our range - update the own mask entry...
1003
  if(instance_data[instance].tof[tof_idx] != INVALID_TOF) { //check the last ToF entry is valid and copy into the current array
1004
    instance_data[instance].rxResponseMask = (0x1 << instance_data[instance].shortAdd_idx);
1005
    instance_data[instance].tofArray[instance_data[instance].shortAdd_idx] = instance_data[instance].tof[tof_idx];
1006
  }
1007
  else        {  //reset response mask
1008
    instance_data[instance].tofArray[instance_data[instance].shortAdd_idx] = INVALID_TOF ;
1009
    instance_data[instance].rxResponseMask = 0;        //reset the mask of received responses when rx poll
1010
  }
1011
  //set the delayed rx on time (the final message will be sent after this delay)
1012
  dwt_setrxaftertxdelay(instance_data[instance].ancRespRxDelay);  //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
1013

    
1014
  //if this is gateway anchor - calculate the slot period correction
1015
  if(instance_data[instance].gatewayAnchor)  {
1016
    int error = 0;
1017
    int currentSlotTime = 0;
1018
    int expectedSlotTime = 0;
1019
    //find the time in the current superframe
1020
    currentSlotTime = uTimeStamp % instance_data[instance].sframePeriod;
1021

    
1022
    //this is the slot time the poll should be received in (Mask 0x07 for the 8 MAX tags we support in TREK)
1023
    expectedSlotTime = (sourceAddress&0xFF) * instance_data[instance].slotPeriod; //
1024

    
1025
    //error = expectedSlotTime - currentSlotTime
1026
    error = expectedSlotTime - currentSlotTime;
1027

    
1028
    if(error < (-(instance_data[instance].sframePeriod>>1))) { //if error is more negative than 0.5 period, add whole period to give up to 1.5 period sleep
1029
      instance_data[instance].tagSleepCorrection = (instance_data[instance].sframePeriod + error);
1030
    }
1031
    else { //the minimum Sleep time will be 0.5 period
1032
      instance_data[instance].tagSleepCorrection = error;
1033
    }
1034
    instance_data[instance].msg_f.messageData[RES_TAG_SLP0] = instance_data[instance].tagSleepCorrection & 0xFF ;
1035
    instance_data[instance].msg_f.messageData[RES_TAG_SLP1] = (instance_data[instance].tagSleepCorrection >> 8) & 0xFF;
1036
  }
1037
  else {
1038
    instance_data[instance].tagSleepCorrection = 0;
1039
    instance_data[instance].msg_f.messageData[RES_TAG_SLP0] = 0 ;
1040
    instance_data[instance].msg_f.messageData[RES_TAG_SLP1] = 0 ;
1041
  }
1042
  instance_data[instance].msg_f.messageData[FCODE] = RTLS_DEMO_MSG_ANCH_RESP; //message function code (specifies if message is a poll, response or other...)
1043

    
1044
  //write the TX data
1045
  dwt_writetxfctrl(frameLength, 0, 1);
1046
  dwt_writetxdata(frameLength, (uint8_t *)  &instance_data[instance].msg_f, 0) ;        // write the frame data
1047
}
1048

    
1049
/**
1050
 * @brief this is the receive event callback handler, the received event is processed and the instance either
1051
 * responds by sending a response frame or re-enables the receiver to await the next frame
1052
 * once the immediate action is taken care of the event is queued up for application to process
1053
 */
1054
#pragma GCC optimize ("O3")
1055
void instance_rxcallback(const dwt_cb_data_t *rxd){
1056
  int instance = 0;
1057
  uint8_t rxTimeStamp[5]  = {0, 0, 0, 0, 0};
1058

    
1059
  uint8_t rxd_event = 0;
1060
  uint8_t fcode_index  = 0;
1061
  uint8_t srcAddr_index = 0;
1062
  event_data_t dw_event;
1063

    
1064
  //microcontroller time at which we received the frame
1065
  dw_event.uTimeStamp = portGetTickCnt();
1066

    
1067
  //if we got a frame with a good CRC - RX OK
1068
  //  if(rxd->event == DWT_SIG_RX_OKAY) { // Timeout and error are handle separately in the driver itself
1069
  dw_event.rxLength = rxd->datalength;
1070

    
1071
  //need to process the frame control bytes to figure out what type of frame we have received
1072
  if(((rxd->fctrl[0] == 0x41) || (rxd->fctrl[0] == 0x61))
1073
      &&
1074
      ((rxd->fctrl[1] & 0xCC) == 0x88)) {  //short address
1075
    fcode_index = FRAME_CRTL_AND_ADDRESS_S; //function code is in first byte after source address
1076
    srcAddr_index = FRAME_CTRLP + ADDR_BYTE_SIZE_S;
1077
    rxd_event = DWT_SIG_RX_OKAY;
1078
  }
1079
  else {
1080
    rxd_event = SIG_RX_UNKNOWN; //not supported - all TREK1000 frames are short addressed
1081
  }
1082

    
1083
  //read RX timestamp
1084
  dwt_readrxtimestamp(rxTimeStamp) ;
1085
  dwt_readrxdata((uint8_t *)&dw_event.msgu.frame[0], rxd->datalength, 0);  // Read Data Frame
1086
  dw_event.timeStamp32l =  (uint32_t)rxTimeStamp[0] + ((uint32_t)rxTimeStamp[1] << 8) + ((uint32_t)rxTimeStamp[2] << 16) + ((uint32_t)rxTimeStamp[3] << 24);
1087
  dw_event.timeStamp = rxTimeStamp[4];
1088
  dw_event.timeStamp <<= 32;
1089
  dw_event.timeStamp += dw_event.timeStamp32l;
1090
  dw_event.timeStamp32h = ((uint32_t)rxTimeStamp[4] << 24) + (dw_event.timeStamp32l >> 8);
1091

    
1092
  dw_event.type = 0; //type will be added as part of adding to event queue
1093
  dw_event.type_save = rxd_event;
1094
  dw_event.type_pend = DWT_SIG_DW_IDLE;
1095

    
1096
  //if Listener then just report the received frame to the instance (application)
1097
  if(rxd_event == DWT_SIG_RX_OKAY) { //Process good/known frame types
1098
    uint16_t sourceAddress = (((uint16_t)dw_event.msgu.frame[srcAddr_index+1]) << 8) + dw_event.msgu.frame[srcAddr_index];
1099

    
1100
    if(instance_data[instance].mode != LISTENER)      {
1101
      if(instance_data[instance].mode == TAG) //if tag got a good frame - this is probably a response, but could also be some other non-ranging frame
1102
        //(although due to frame filtering this is limited as non-addressed frames are filtered out)
1103
      {
1104
        instance_data[instance].responseTO--; //got 1 more response or other RX frame - need to reduce timeout (for next response)
1105
      }
1106

    
1107
      //check if this is a TWR message (and also which one)
1108
      switch(dw_event.msgu.frame[fcode_index]){
1109
      //poll message from an anchor
1110
      case RTLS_DEMO_MSG_ANCH_POLL:{
1111
        //the anchor to anchor ranging poll frames are ignored by A0 and A3
1112
        if(instance_data[instance].gatewayAnchor || (instance_data[instance].instanceAddress16 > A2_ANCHOR_ADDR)){
1113
          //ignore poll from anchor 1 if gateway or anchor 3
1114
          //anchors 2 and 3 will never send polls
1115
          dw_event.type_pend = DWT_SIG_DW_IDLE ;
1116
          break;
1117
        }
1118

    
1119
        if(instance_data[instance].mode == TAG)  {  //tag should ignore any other Polls from anchors
1120
          instance_data[instance].responseTO++; //as will be decremented in the function and was also decremented above
1121
          handle_error_unknownframe(dw_event);
1122
          instance_data[instance].stopTimer = 1;
1123
          instance_data[instance].rxMsgCount++;
1124
          return;
1125
        }
1126

    
1127
        //update the response index and number of responses received tables
1128
        instance_data[instance].rxRespsIdx = (uint8_t) ((dw_event.msgu.frame[POLL_RNUM+fcode_index] & 0xf)
1129
            + (((sourceAddress&0x3) + 8) << 4));
1130
        instance_data[instance].rxResps[instance_data[instance].rxRespsIdx] = 0;
1131
        //debug LED on
1132
        //                                                led_on(LED_PC9);
1133

    
1134
        //prepare the response and write it to the tx buffer
1135
        ancprepareresponse2(sourceAddress, srcAddr_index, fcode_index, &dw_event.msgu.frame[0]);
1136

    
1137
        //A2 does not need timeout if ranging to A1
1138
        if(sourceAddress != A1_ANCHOR_ADDR){
1139
          dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTimeAnc_sy); //reconfigure the timeout for response
1140
        }
1141

    
1142
        //set the bast reply time (the actual will be Poll rx time + instance_data[0].fixedReplyDelayAnc)
1143
        instance_data[instance].delayedReplyTime = dw_event.timeStamp32h ;
1144
        instance_data[instance].responseTO = (instance_data[instance].instanceAddress16 - sourceAddress) & 0x3; //set number of expected responses
1145

    
1146
        dw_event.type_pend = anctxorrxreenable(instance_data[instance].instanceAddress16, 2+1);
1147

    
1148
        instance_data[instance].tofAnc[sourceAddress & 0x3] = INVALID_TOF; //clear ToF ..
1149
        //debug LED off
1150
        //                                                led_off(LED_PC9);
1151
        break;
1152
      }
1153

    
1154
      case RTLS_DEMO_MSG_TAG_POLL:{
1155
        if(instance_data[instance].mode == TAG) {  //tag should ignore any other Polls from tags
1156
          instance_data[instance].responseTO++; //as will be decremented in the function and was also decremented above
1157
          handle_error_unknownframe(dw_event);
1158
          instance_data[instance].stopTimer = 1;
1159
          instance_data[instance].rxMsgCount++;
1160
          return;
1161
        }
1162
        instance_data[instance].rxRespsIdx = (int8_t) ((dw_event.msgu.frame[POLL_RNUM+fcode_index] & 0xf)
1163
            + ((sourceAddress&0x7) << 4));
1164
        instance_data[instance].rxResps[instance_data[instance].rxRespsIdx] = 0;
1165

    
1166
        //prepare the response and write it to the tx buffer
1167
        ancprepareresponse(sourceAddress, srcAddr_index, fcode_index, &dw_event.msgu.frame[0], dw_event.uTimeStamp);
1168

    
1169
        dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTimeAnc_sy); //reconfigure the timeout for response
1170

    
1171
        instance_data[0].delayedReplyTime = dw_event.timeStamp32h /*+ (instance_data[0].fixedReplyDelayAnc >> 8)*/ ;
1172
        instance_data[instance].responseTO = NUM_EXPECTED_RESPONSES; //set number of expected responses to 3 (from other anchors)
1173

    
1174
        dw_event.type_pend = anctxorrxreenable(instance_data[instance].instanceAddress16, 2+0);
1175

    
1176
        instance_data[instance].tof[sourceAddress & 0x7] = INVALID_TOF; //clear ToF ..
1177
      }
1178
      break;
1179

    
1180
      //we got a response from a "responder" (anchor)
1181
      case RTLS_DEMO_MSG_ANCH_RESP:
1182
      case RTLS_DEMO_MSG_ANCH_RESP2:{
1183

    
1184
        //we are a tag
1185
        if(instance_data[instance].mode == TAG){
1186
          uint8_t index ;
1187
          instance_data[instance].rxResps[instance_data[instance].rangeNum]++;
1188
          dw_event.type_pend = tagrxreenable(sourceAddress); //responseTO decremented above...
1189
          index = RRXT0 + 5*(sourceAddress & 0x3);
1190

    
1191
          instance_data[instance].rxResponseMask |= (0x1 << (sourceAddress & 0x3)); //add anchor ID to the mask
1192
          // Write Response RX time field of Final message
1193
          memcpy(&(instance_data[instance].msg_f.messageData[index]), rxTimeStamp, 5);
1194

    
1195
        }
1196
        else if (instance_data[instance].mode == ANCHOR_RNG) {  //A0 and A1 only when ranging to other anchors
1197
          uint8_t index ;
1198
          instance_data[instance].rxResps[instance_data[instance].rangeNumAnc]++;
1199
          dw_event.type_pend = ancsendfinalorrxreenable(sourceAddress);
1200
          index = RRXT0 + 5*(sourceAddress & 0x3);
1201

    
1202
          instance_data[instance].rxResponseMaskAnc |= (0x1 << (sourceAddress & 0x3)); //add anchor ID to the mask
1203
          // Write Response RX time field of Final message
1204
          memcpy(&(instance_data[instance].msg_f.messageData[index]), rxTimeStamp, 5);
1205
        }
1206
        else { //normal anchor mode
1207
          //got a response... (check if we got a Poll with the same range number as in this response)
1208
          if(RTLS_DEMO_MSG_ANCH_RESP == dw_event.msgu.frame[fcode_index]){
1209
            if((instance_data[instance].rxResps[instance_data[instance].rxRespsIdx] >= 0) //we got the poll else ignore this response
1210
                && (instance_data[instance].responseTO > 0)        ) {  //if responseTO == 0 we have already received all of the responses - meaning should not be here => error
1211
              instance_data[instance].rxResps[instance_data[instance].rxRespsIdx]++; //increment the number of responses received
1212
              instance_data[instance].responseTO--;
1213

    
1214
              //send a response or re-enable rx
1215
              dw_event.type_pend = anctxorrxreenable(sourceAddress, 4+0);
1216
            }
1217
            else {  //like a timeout (error) ...
1218

    
1219
              //send a response or re-enable rx
1220
              dwt_setrxtimeout(0); //reconfigure the timeout
1221
              dwt_rxenable(DWT_START_RX_IMMEDIATE) ;
1222
              dw_event.type_pend = DWT_SIG_RX_PENDING ;
1223
            }
1224
          }
1225
          else {  //in anchor mode and got RTLS_DEMO_MSG_ANCH_RESP2
1226
            if((instance_data[instance].gatewayAnchor) &&
1227
                (instance_data[instance].rxResps[instance_data[instance].rangeNumAnc]) == 2){ //got two responses A1 and A2 this is third (A2's to A1)
1228
              instance_data[instance].rxResps[instance_data[instance].rangeNumAnc]++;
1229
              instance_data[instance].rxResponseMaskAnc |= 0x8 ;
1230

    
1231
              dw_event.type_pend = anctxorrxreenable(sourceAddress, 4+1); //re-enable the RX
1232
            }
1233
            //A2 got A1's response to A0 - send A2 response (but only if we got the Poll from A0)
1234
            else if((instance_data[instance].instanceAddress16 == A2_ANCHOR_ADDR) &&
1235
                (instance_data[instance].rxResps[instance_data[instance].rxRespsIdx] >= 0) ){
1236
              instance_data[instance].rxResps[instance_data[instance].rxRespsIdx]++;
1237
              instance_data[instance].responseTO--;
1238

    
1239
              dwt_setrxtimeout(0);
1240
              dwt_setrxaftertxdelay(0); //clear rx on delay as Final will come sooner than if we were waiting for next Response
1241
              dw_event.type_pend = anctxorrxreenable(sourceAddress, 1);
1242
            }
1243
            else {  // if other anchor A1, A2, A3 .. ignore these responses when in ANCHOR mode
1244
              dwt_setrxtimeout(0); //reconfigure the timeout
1245
              dwt_rxenable(DWT_START_RX_IMMEDIATE) ;
1246
              dw_event.type_pend = DWT_SIG_RX_PENDING ;
1247
            }
1248
          }
1249
        }
1250

    
1251
      }
1252
      break;
1253

    
1254
      case RTLS_DEMO_MSG_TAG_FINAL:
1255
      case RTLS_DEMO_MSG_ANCH_FINAL:
1256
        if(instance_data[instance].mode == TAG) {  //tag should ignore any other Final from anchors
1257
          instance_data[instance].responseTO++; //as will be decremented in the function and was also decremented above
1258
          handle_error_unknownframe(dw_event);
1259
          instance_data[instance].stopTimer = 1;
1260
          instance_data[instance].rxMsgCount++;
1261
          return;
1262
        }
1263
      break;  // TODO: fallthourgh for anchor
1264
//        __attribute__ ((fallthrough));
1265

    
1266
        //if anchor fall into case below and process the frame
1267
      default: {  //process rx frame
1268
        dw_event.type_pend = DWT_SIG_DW_IDLE;
1269
      }
1270
      break;
1271

    
1272
      }
1273
    }//end of if not Listener mode
1274
    instance_data[instance].stopTimer = 1;
1275

    
1276
    instance_putevent(dw_event, rxd_event);
1277

    
1278
    instance_data[instance].rxMsgCount++;
1279
  }
1280
  else { //if (rxd_event == SIG_RX_UNKNOWN) //need to re-enable the rx (got unknown frame type)
1281
    handle_error_unknownframe(dw_event);
1282
  }
1283
  //  }
1284
}
1285

    
1286

    
1287
/*TODO: Callback funtion for RX timeout (available from newer driver version) */
1288
#pragma GCC optimize ("O3")
1289
void instance_rxtimeoutcallback(const dwt_cb_data_t *rxd){
1290

    
1291
    (void) rxd;
1292
    event_data_t dw_event;
1293

    
1294
    int instance = 0;
1295

    
1296
    dw_event.type_pend = DWT_SIG_DW_IDLE;
1297

    
1298
    if(instance_data[instance].mode == ANCHOR) {
1299
        //check if anchor has received all of the responses from other anchors (it could have received only 1 or 2)
1300
        //it's timed out (re-enable rx or tx response)
1301
        if(instance_data[instance].responseTO > 0) {
1302
            instance_data[instance].responseTO--;
1303
            //send a response or re-enable rx
1304
            dw_event.type_pend = anctxorrxreenable(instance_data[instance].instanceAddress16, 6+0);
1305
//          Print_On_Uart("Re-enable RX in rxtimeout callback\r\n");
1306
        }
1307
    }
1308
    dw_event.type = 0;
1309
    dw_event.type_save = DWT_SIG_RX_TIMEOUT;
1310
    dw_event.rxLength = 0;
1311
    dw_event.timeStamp = 0;
1312
    dw_event.timeStamp32l = 0;
1313
    dw_event.timeStamp32h = 0;
1314

    
1315
    instance_putevent(dw_event, DWT_SIG_RX_TIMEOUT);
1316
//  printf("RX timeout while in %d\n", instance_data[instance].testAppState);
1317

    
1318
}
1319

    
1320

    
1321
/*TODO: Callback funtion for RX error (available from newer driver version) */
1322
#pragma GCC optimize ("O3")
1323
void instance_rxerrorcallback(const dwt_cb_data_t *rxd) {
1324

    
1325
    (void) rxd;
1326
    event_data_t dw_event;
1327

    
1328
    dw_event.uTimeStamp = portGetTickCnt();
1329

    
1330
    handle_error_unknownframe(dw_event);
1331
}
1332

    
1333

    
1334

    
1335
#pragma GCC optimize ("O3")
1336
int instance_peekevent(void){
1337
  int instance = 0;
1338
  return instance_data[instance].dwevent[instance_data[instance].dweventPeek].type; //return the type of event that is in front of the queue
1339
}
1340

    
1341
#pragma GCC optimize ("O3")
1342
void instance_saveevent(event_data_t newevent, uint8_t etype){
1343
  int instance = 0;
1344

    
1345
  instance_data[instance].saved_dwevent = newevent;
1346
  instance_data[instance].saved_dwevent.type = etype;
1347
}
1348

    
1349
#pragma GCC optimize ("O3")
1350
event_data_t instance_getsavedevent(void){
1351
  int instance = 0;
1352

    
1353
  return instance_data[instance].saved_dwevent;
1354
}
1355

    
1356
#pragma GCC optimize ("O3")
1357
void instance_putevent(event_data_t newevent, uint8_t etype){
1358
  int instance = 0;
1359
  //newevent.eventtime = portGetTickCnt();
1360
  newevent.gotit = 0 ; //newevent.eventtimeclr = 0;
1361

    
1362
  //copy event
1363
  instance_data[instance].dwevent[instance_data[instance].dweventIdxIn] = newevent;
1364

    
1365
  //set type - this makes it a new event (making sure the event data is copied before event is set as new)
1366
  //to make sure that the get event function does not get an incomplete event
1367
  instance_data[instance].dwevent[instance_data[instance].dweventIdxIn].type = etype;
1368

    
1369
  instance_data[instance].dweventIdxIn++;
1370

    
1371
  if(MAX_EVENT_NUMBER == instance_data[instance].dweventIdxIn)
1372
  {
1373
    instance_data[instance].dweventIdxIn = 0;
1374
  }
1375
  //eventIncount++;
1376

    
1377
  //printf("put %d - in %d out %d @ %d\n", newevent.type, instance_data[instance].dweventCntIn, instance_data[instance].dweventCntOut, ptime);
1378
}
1379

    
1380
event_data_t dw_event_g;
1381

    
1382
#pragma GCC optimize ("O3")
1383
event_data_t* instance_getevent(int x){
1384
  int instance = 0;
1385
  int indexOut = instance_data[instance].dweventIdxOut;
1386

    
1387
  //dw_event_g = instance_data[instance].dwevent[instance_data[instance].dweventCntOut]; //this holds any TX/RX events
1388

    
1389
  //memcpy(&dw_event_g, &instance_data[instance].dwevent[instance_data[instance].dweventCntOut], sizeof(event_data_t));
1390

    
1391
  if(instance_data[instance].dwevent[indexOut].type == 0) {  //exit with "no event"
1392
    dw_event_g.type = 0;
1393
    dw_event_g.type_save = 0;
1394
    return &dw_event_g;
1395
  }
1396

    
1397
  //copy the event
1398
  dw_event_g.type_save = instance_data[instance].dwevent[indexOut].type_save ;
1399
  dw_event_g.type_pend = instance_data[instance].dwevent[indexOut].type_pend ;
1400
  dw_event_g.rxLength = instance_data[instance].dwevent[indexOut].rxLength ;
1401
  dw_event_g.timeStamp = instance_data[instance].dwevent[indexOut].timeStamp ;
1402
  dw_event_g.timeStamp32l = instance_data[instance].dwevent[indexOut].timeStamp32l ;
1403
  dw_event_g.timeStamp32h = instance_data[instance].dwevent[indexOut].timeStamp32h ;
1404
  dw_event_g.uTimeStamp = instance_data[instance].dwevent[indexOut].uTimeStamp ;
1405
  //dw_event_g.eventtime = instance_data[instance].dwevent[indexOut].eventtime ;
1406
  //dw_event_g.eventtimeclr = instance_data[instance].dwevent[indexOut].eventtimeclr ;
1407
  //dw_event_g.gotit = instance_data[instance].dwevent[indexOut].gotit ;
1408

    
1409
  memcpy(&dw_event_g.msgu, &instance_data[instance].dwevent[indexOut].msgu, sizeof(instance_data[instance].dwevent[indexOut].msgu));
1410

    
1411
  dw_event_g.type = instance_data[instance].dwevent[indexOut].type ;
1412

    
1413

    
1414
  instance_data[instance].dwevent[indexOut].gotit = x;
1415

    
1416
  //instance_data[instance].dwevent[indexOut].eventtimeclr = portGetTickCnt();
1417

    
1418
  instance_data[instance].dwevent[indexOut].type = 0; //clear the event
1419

    
1420
  instance_data[instance].dweventIdxOut++;
1421
  if(MAX_EVENT_NUMBER == instance_data[instance].dweventIdxOut){   //wrap the counter
1422
    instance_data[instance].dweventIdxOut = 0;
1423
  }
1424
  instance_data[instance].dweventPeek = instance_data[instance].dweventIdxOut; //set the new peek value
1425

    
1426
  //if(dw_event.type) printf("get %d - in %d out %d @ %d\n", dw_event.type, instance_data[instance].dweventCntIn, instance_data[instance].dweventCntOut, ptime);
1427

    
1428
  //eventOutcount++;
1429

    
1430

    
1431
  return &dw_event_g;
1432
}
1433

    
1434
void instance_clearevents(void){
1435
  int i = 0;
1436
  int instance = 0;
1437

    
1438
  for(i=0; i<MAX_EVENT_NUMBER; i++) {
1439
    memset(&instance_data[instance].dwevent[i], 0, sizeof(event_data_t));
1440
  }
1441

    
1442
  instance_data[instance].dweventIdxIn = 0;
1443
  instance_data[instance].dweventIdxOut = 0;
1444
  instance_data[instance].dweventPeek = 0;
1445

    
1446
  //eventOutcount = 0;
1447
  //eventIncount = 0;
1448
}
1449

    
1450
// -------------------------------------------------------------------------------------------------------------------
1451
#pragma GCC optimize ("O3")
1452
int instance_run(void){
1453
  int instance = 0 ;
1454
  int done = INST_NOT_DONE_YET;
1455
  int message = instance_peekevent(); //get any of the received events from ISR
1456

    
1457

    
1458
  while(done == INST_NOT_DONE_YET){
1459
    //int state = instance_data[instance].testAppState;
1460
    done = testapprun(&instance_data[instance], message) ;                                               // run the communications application
1461

    
1462
    //we've processed message
1463
    message = 0;
1464
  }
1465

    
1466

    
1467

    
1468
  if(done == INST_DONE_WAIT_FOR_NEXT_EVENT_TO) { //we are in RX and need to timeout (Tag needs to send another poll if no Rx frame)
1469
    if(instance_data[instance].mode == TAG) { //Tag (is either in RX or sleeping)
1470
      int32_t nextPeriod ;
1471

    
1472
      // next period will be a positive number because correction is -0.5 to +1.5 periods, (and tagSleepTime_ms is the period)
1473
      nextPeriod = instance_data[instance].tagSleepRnd + instance_data[instance].tagSleepTime_ms + instance_data[instance].tagSleepCorrection;
1474

    
1475
      instance_data[instance].nextSleepPeriod = (uint32_t) nextPeriod ; //set timeout time, CAST the positive period to UINT for correct wrapping.
1476
      instance_data[instance].tagSleepCorrection2 = instance_data[instance].tagSleepCorrection;
1477
      instance_data[instance].tagSleepCorrection = 0; //clear the correction
1478
      instance_data[instance].instanceTimerEn = 1; //start timer
1479
    }
1480
    instance_data[instance].stopTimer = 0 ; //clear the flag - timer can run if instancetimer_en set (set above)
1481
    instance_data[instance].done = INST_NOT_DONE_YET;
1482
  }
1483

    
1484
  //check if timer has expired
1485
  if((instance_data[instance].instanceTimerEn == 1) && (instance_data[instance].stopTimer == 0))  {
1486
    if(instance_data[instance].mode == TAG)    {
1487
      if((portGetTickCnt() - instance_data[instance].instanceWakeTime) > instance_data[instance].nextSleepPeriod)      {
1488
        event_data_t dw_event;
1489
        instance_data[instance].instanceTimerEn = 0;
1490
        dw_event.rxLength = 0;
1491
        dw_event.type = 0;
1492
        dw_event.type_save = 0x80 | DWT_SIG_RX_TIMEOUT;
1493
        //printf("PC timeout DWT_SIG_RX_TIMEOUT\n");
1494
        instance_putevent(dw_event, DWT_SIG_RX_TIMEOUT);
1495
      }
1496
    }
1497
#if (ANCTOANCTWR == 1) //allow anchor to anchor ranging
1498
    else if(instance_data[instance].mode == ANCHOR)    {
1499
      uint32_t slotTime = portGetTickCnt() % instance_data[instance].sframePeriod;
1500

    
1501
      if(instance_data[instance].gatewayAnchor) {
1502
        //if we are in the last slot - then A0 ranges to A1 and A2
1503
        if( slotTime >= instance_data[instance].a0SlotTime)        {
1504
          port_DisableEXT_IRQ(); //enable ScenSor IRQ before starting
1505
          //anchor0 sends poll to anchor1
1506
          instance_data[instance].mode = ANCHOR_RNG; //change to ranging initiator
1507
          dwt_forcetrxoff(); //disable DW1000
1508
          instance_clearevents(); //clear any events
1509
          //change state to send a Poll
1510
          instance_data[instance].testAppState = TA_TXPOLL_WAIT_SEND ;
1511
          instance_data[instance].msg_f.destAddr[0] = 0x1 ;
1512
          instance_data[instance].msg_f.destAddr[1] = (GATEWAY_ANCHOR_ADDR >> 8);
1513
          instance_data[instance].instanceTimerEn = 0;
1514
          instance_data[instance].rangeNumAnc++;
1515
          port_EnableEXT_IRQ(); //enable ScenSor IRQ before starting
1516
        }
1517
      }
1518
      else if (instance_data[instance].instanceAddress16 == A1_ANCHOR_ADDR) { //A1 ranges to A2 in the 2nd half of last slot
1519
        if(portGetTickCnt() >= instance_data[instance].a1SlotTime) {
1520
          port_DisableEXT_IRQ(); //enable ScenSor IRQ before starting
1521
          //anchor1 sends poll to anchor2
1522
          instance_data[instance].mode = ANCHOR_RNG; //change to ranging initiator
1523
          dwt_forcetrxoff(); //disable DW1000
1524
          instance_clearevents(); //clear any events
1525
          //change state to send a Poll
1526
          instance_data[instance].testAppState = TA_TXPOLL_WAIT_SEND ;
1527
          instance_data[instance].msg_f.destAddr[0] = 0x2 ;
1528
          instance_data[instance].msg_f.destAddr[1] = (GATEWAY_ANCHOR_ADDR >> 8);
1529

    
1530
          instance_data[instance].instanceTimerEn = 0;
1531
          //instance_data[instance].a1SlotTime = 0;
1532
          port_EnableEXT_IRQ(); //enable ScenSor IRQ before starting
1533
        }
1534
      }
1535
    }
1536
#endif
1537
  }
1538

    
1539
#if (ANCTOANCTWR == 1) //allow anchor to anchor ranging
1540
  else if (instance_data[instance].instanceTimerEn == 0){
1541
    if((instance_data[instance].mode == ANCHOR) && (instance_data[instance].gatewayAnchor)){
1542
      uint32_t slotTime = portGetTickCnt() % instance_data[instance].sframePeriod;
1543
      //enable the timer in 1st slot
1544
      if(slotTime < instance_data[instance].slotPeriod){
1545
        instance_data[instance].instanceTimerEn = 1;
1546
      }
1547
    }
1548
  }
1549
#endif
1550
  return 0 ;
1551
}
1552

    
1553

    
1554
void instance_close(void){
1555
  //wake up device from low power mode
1556
  //NOTE - in the ARM  code just drop chip select for 200us
1557
  port_SPIx_clear_chip_select();  //CS low
1558
  Sleep(1);   //200 us to wake up then waits 5ms for DW1000 XTAL to stabilise
1559
  port_SPIx_set_chip_select();  //CS high
1560
  Sleep(5);
1561
  dwt_entersleepaftertx(0); // clear the "enter deep sleep after tx" bit
1562
  dwt_setinterrupt(0xFFFFFFFF, 0); //don't allow any interrupts
1563

    
1564
}
1565

    
1566

    
1567
void instance_notify_DW1000_inIDLE(int idle){
1568
  instance_data[0].dwIDLE = idle;
1569
}
1570

    
1571
void instanceconfigtxpower(uint32_t txpower){
1572
  instance_data[0].txPower = txpower ;
1573

    
1574
  instance_data[0].txPowerChanged = 1;
1575

    
1576
}
1577

    
1578
void instancesettxpower(void){
1579
  if(instance_data[0].txPowerChanged == 1){
1580
    //Configure TX power
1581
    dwt_write32bitreg(0x1E, instance_data[0].txPower);
1582

    
1583
    instance_data[0].txPowerChanged = 0;
1584
  }
1585
}
1586

    
1587
void instanceconfigantennadelays(uint16_t tx, uint16_t rx){
1588
  instance_data[0].txAntennaDelay = tx ;
1589
  instance_data[0].rxAntennaDelay = rx ;
1590

    
1591
  instance_data[0].antennaDelayChanged = 1;
1592
}
1593

    
1594
void instancesetantennadelays(void){
1595
  if(instance_data[0].antennaDelayChanged == 1){
1596
    dwt_setrxantennadelay(instance_data[0].rxAntennaDelay);
1597
    dwt_settxantennadelay(instance_data[0].txAntennaDelay);
1598

    
1599
    instance_data[0].antennaDelayChanged = 0;
1600
  }
1601
}
1602

    
1603

    
1604
uint16_t instancetxantdly(void){
1605
  return instance_data[0].txAntennaDelay;
1606
}
1607

    
1608
uint16_t instancerxantdly(void){
1609
  return instance_data[0].rxAntennaDelay;
1610
}
1611

    
1612
uint8_t instancevalidranges(void){
1613
  uint8_t x = instance_data[0].rxResponseMaskReport;
1614
  instance_data[0].rxResponseMaskReport = 0; //reset mask as we have printed out the ToFs
1615
  return x;
1616
}
1617
#endif
1618

    
1619

    
1620
/*! TODO: the following Functions are added for user application (previously on the API )*/
1621

    
1622
/*! ------------------------------------------------------------------------------------------------------------------
1623
 * @fn dwt_getotptxpower()
1624
 *
1625
 * @brief This API function returns the tx power value read from OTP memory as part of initialisation
1626
 *
1627
 * input parameters
1628
 * @param prf   -   this is the PRF e.g. DWT_PRF_16M or DWT_PRF_64M
1629
 * @param chan  -   this is the channel e.g. 1 to 7
1630
 *
1631
 * output parameters
1632
 *
1633
 * returns tx power value for a given PRF and channel
1634
 */
1635
uint32_t dwt_getotptxpower(uint8_t prf, uint8_t chan){
1636
    return platformLocalData.txPowCfg[(prf - DWT_PRF_16M) + (chan_idx[chan] * 2)];
1637
}
1638

    
1639
/*! ------------------------------------------------------------------------------------------------------------------
1640
 * @fn dwt_getTREKOTPantennadelay()
1641
 *
1642
 * @brief This API function returns the antenna delay read from the OTP memory as part of device initialisation
1643
 * Note: the antenna delay will only be read if dwt_initialise is called with DWT_LOADANTDLY bit set in the config parameter
1644
 * The values of antenna delay are only valid for TREK use case modes.
1645
 *
1646
 * input parameters:
1647
 * @param anchor   - this is the mode (Tag or Anchor) if Tag set to 0, if Anchor set to 1
1648
 * @param chan   - this is the channel (1, 2, 3, 4, 5, 7)
1649
 * @param datarate   - this is the datarate DWT_BR_6M8, DWT_BR_110K or DWT_BR_850K
1650
 *
1651
 */
1652
uint16_t dwt_getTREKOTPantennadelay(uint8_t anchor, uint8_t chan, uint8_t datarate){
1653
    uint32_t dly = 0;
1654

    
1655
    // 32-bit antenna delay value previously read from OTP, high 16 bits is value for Anchor mode, low 16-bits for Tag mode
1656
    switch(chan){
1657
        case 2:
1658
            if(datarate == DWT_BR_6M8)
1659
                dly = platformLocalData.antennaCals[0];
1660
            else if(datarate == DWT_BR_110K)
1661
                dly = platformLocalData.antennaCals[1];
1662
            break;
1663
        case 5:
1664
            if(datarate == DWT_BR_6M8)
1665
                dly = platformLocalData.antennaCals[2];
1666
            else if(datarate == DWT_BR_110K)
1667
                dly = platformLocalData.antennaCals[3];
1668
            break;
1669
        default:
1670
            dly = 0;
1671
            break;
1672
    }
1673

    
1674
    return (dly >> (16*(anchor & 0x1))) & 0xFFFF;
1675
}
1676

    
1677
/*! ------------------------------------------------------------------------------------------------------------------
1678
 * @fn dwt_readantennadelay()
1679
 *
1680
 * @brief This API function returns the antenna delay read from the OTP memory as part of device initialisation
1681
 * Note: the antenna delay will only be read if dwt_initialise is called with DWT_LOADANTDLY bit set in the config parameter
1682
 *
1683
 * input parameters:
1684
 * @param prf   -   this is the PRF e.g. DWT_PRF_16M or DWT_PRF_64M
1685
 *
1686
 */
1687
uint16_t dwt_readantennadelay(uint8_t prf){
1688
    // 32-bit antenna delay value previously read from OTP, high 16 bits is value for 64 MHz PRF, low 16-bits for 16 MHz PRF
1689
    return (platformLocalData.antennaDly >> (16*(prf-DWT_PRF_16M))) & 0xFFFF;
1690
}
1691

    
1692

    
1693
/* ==========================================================
1694

1695
Notes:
1696

1697
Previously code handled multiple instances in a single console application
1698

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

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

1703
*/
1704

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