Statistics
| Branch: | Tag: | Revision:

amiro-lld / source / DW1000 / v1 / deca_instance_common_v1.c @ 33f54213

History | View | Annotate | Download (73.2 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
#include <v1/deca_instance_v1.h>
22

    
23

    
24
/*! @brief Software defined configuration settings for RTLS applications */
25
/*! Configuration for DecaRangeRTLS TREK Modes (4 default use cases selected by the switch S1 [2,3] on EVB1000, indexed 0 to 3 )*/
26
instanceConfig_t chConfig[4] ={
27
                               //mode 1 - S1: 2 off, 3 off
28
                               {
29
                                .channelNumber = 2,             // channel
30
                                .preambleCode = 4,              // preambleCode
31
                                .pulseRepFreq = DWT_PRF_16M,    // prf
32
                                .dataRate = DWT_BR_110K,        // datarate
33
                                .preambleLen = DWT_PLEN_1024,   // preambleLength
34
                                .pacSize = DWT_PAC32,           // pacSize
35
                                .nsSFD = 1,                     // non-standard SFD
36
                                .sfdTO = (1025 + 64 - 32)       // SFD timeout
37
                               },
38
                               //mode 2 - S1: 2 on, 3 off
39
                               {
40
                                .channelNumber = 2,            // channel
41
                                .preambleCode = 4,             // preambleCode
42
                                .pulseRepFreq = DWT_PRF_16M,   // prf
43
                                .dataRate = DWT_BR_6M8,        // datarate
44
                                .preambleLen = DWT_PLEN_128,   // preambleLength
45
                                .pacSize = DWT_PAC8,           // pacSize
46
                                .nsSFD = 0,                    // non-standard SFD
47
                                .sfdTO = (129 + 8 - 8)        // SFD timeout
48
                               },
49
                               //mode 3 - S1: 2 off, 3 on
50
                               {
51
                                .channelNumber = 5,             // channel
52
                                .preambleCode = 3,              // preambleCode
53
                                .pulseRepFreq = DWT_PRF_16M,    // prf
54
                                .dataRate = DWT_BR_110K,        // datarate
55
                                .preambleLen = DWT_PLEN_1024,   // preambleLength
56
                                .pacSize = DWT_PAC32,           // pacSize
57
                                .nsSFD = 1,                     // non-standard SFD
58
                                .sfdTO = (1025 + 64 - 32)       // SFD timeout
59
                               },
60
                               //mode 4 - S1: 2 on, 3 on
61
                               {
62
                                .channelNumber = 5,            // channel
63
                                .preambleCode = 3,             // preambleCode
64
                                .pulseRepFreq = DWT_PRF_16M,   // prf
65
                                .dataRate = DWT_BR_6M8,        // datarate
66
                                .preambleLen = DWT_PLEN_128,   // preambleLength
67
                                .pacSize = DWT_PAC8,           // pacSize
68
                                .nsSFD = 0,                    // non-standard SFD
69
                                .sfdTO = (129 + 8 - 8)        // SFD timeout
70
                               }
71
};
72

    
73

    
74
/*! Slot and Superframe Configuration for DecaRangeRTLS TREK Modes (4 default use cases selected by the switch S1 [2,3] on EVB1000, indexed 0 to 3 )*/
75
sfConfig_t sfConfig[4] ={
76
                         //mode 1 - S1: 2 off, 3 off
77
                         {
78
                          .slotPeriod = (28), //slot duration in milliseconds (NOTE: the ranging exchange must be able to complete in this time
79
                          //e.g. tag sends a poll, 4 anchors send responses and tag sends the final + processing time
80
                          .numSlots = (10),        //number of slots in the superframe (8 tag slots and 2 used for anchor to anchor ranging),
81
                          .sfPeriod = (10*28),  //in ms => 280ms frame means 3.57 Hz location rate
82
                          .pollSleepDly = (10*28), //tag period in ms (sleep time + ranging time)
83
                          .replyDly = (25000) //poll to final delay in microseconds (needs to be adjusted according to lengths of ranging frames)
84
                         },
85
#if (DISCOVERY == 1)
86
                         //mode 2 - S1: 2 on, 3 off
87
                         {
88
                          .slotPeriod = (10), //slot duration in milliseconds (NOTE: the ranging exchange must be able to complete in this time
89
                          //e.g. tag sends a poll, 4 anchors send responses and tag sends the final + processing time
90
                          .numSlots = (100),        //number of slots in the superframe (98 tag slots and 2 used for anchor to anchor ranging),
91
                          .sfPeriod = (10*100),  //in ms => 1000 ms frame means 1 Hz location rate
92
                          .pollSleepDly = (10*100), //tag period in ms (sleep time + ranging time)
93
                          .replyDly = (2500) //poll to final delay in microseconds (needs to be adjusted according to lengths of ranging frames)
94

    
95
                         },
96
#else
97
                         //mode 2 - S1: 2 on, 3 off
98
                         {
99
                          .slotPeriod = (10), //slot duration in milliseconds (NOTE: the ranging exchange must be able to complete in this time
100
                          //e.g. tag sends a poll, 4 anchors send responses and tag sends the final + processing time
101
                          .numSlots = (10),        //number of slots in the superframe (8 tag slots and 2 used for anchor to anchor ranging),
102
                          .sfPeriod = (10*10),  //in ms => 100 ms frame means 10 Hz location rate
103
                          .pollSleepDly = (10*10), //tag period in ms (sleep time + ranging time)
104
                          .replyDly = (2500) //poll to final delay in microseconds (needs to be adjusted according to lengths of ranging frames)
105
                         },
106
#endif
107
                         //mode 3 - S1: 2 off, 3 on
108
                         {
109
                          .slotPeriod = (28), //slot duration in milliseconds (NOTE: the ranging exchange must be able to complete in this time
110
                          //e.g. tag sends a poll, 4 anchors send responses and tag sends the final + processing time
111
                          .numSlots = (10),        //number of slots in the superframe (8 tag slots and 2 used for anchor to anchor ranging),
112
                          .sfPeriod = (10*28),  //in ms => 280ms frame means 3.57 Hz location rate
113
                          .pollSleepDly = (10*28), //tag period in ms (sleep time + ranging time)
114
                          .replyDly = (20000) //poll to final delay in microseconds (needs to be adjusted according to lengths of ranging frames)
115

    
116
                         },
117
                         //mode 4 - S1: 2 on, 3 on
118
                         {
119
                          .slotPeriod = (10), //slot duration in milliseconds (NOTE: the ranging exchange must be able to complete in this time
120
                          //e.g. tag sends a poll, 4 anchors send responses and tag sends the final + processing time
121
                          .numSlots = (10),        //number of slots in the superframe (8 tag slots and 2 used for anchor to anchor ranging),
122
                          .sfPeriod = (10*10),  //in ms => 100 ms frame means 10 Hz location rate
123
                          .pollSleepDly = (10*10), //tag period in ms (sleep time + ranging time)
124
                          .replyDly = (2500) //poll to final delay in microseconds (needs to be adjusted according to lengths of ranging frames)
125
                         }
126
};
127

    
128

    
129
// -------------------------------------------------------------------------------------------------------------------
130

    
131
//The table below specifies the default TX spectrum configuration parameters... this has been tuned for DW EVK hardware units
132
//the table is set for smart power - see below in the instance_config function how this is used when not using smart power
133
const tx_struct txSpectrumConfig[8] =
134
{
135
    //Channel 0 ----- this is just a place holder so the next array element is channel 1
136
    {
137
            0x0,   //0
138
            {
139
                    0x0, //0
140
                    0x0 //0
141
            }
142
    },
143
    //Channel 1
144
    {
145
            0xc9,   //PG_DELAY
146
            {
147
                    0x15355575, //16M prf power
148
                    0x07274767 //64M prf power
149
            }
150

    
151
    },
152
    //Channel 2
153
    {
154
            0xc2,   //PG_DELAY
155
            {
156
                    0x15355575, //16M prf power
157
                    0x07274767 //64M prf power
158
            }
159
    },
160
    //Channel 3
161
    {
162
            0xc5,   //PG_DELAY
163
            {
164
                    0x0f2f4f6f, //16M prf power
165
                    0x2b4b6b8b //64M prf power
166
            }
167
    },
168
    //Channel 4
169
    {
170
            0x95,   //PG_DELAY
171
            {
172
                    0x1f1f3f5f, //16M prf power
173
                    0x3a5a7a9a //64M prf power
174
            }
175
    },
176
    //Channel 5
177
    {
178
            0xc0,   //PG_DELAY
179
            {
180
                    0x0E082848, //16M prf power
181
                    0x25456585 //64M prf power
182
            }
183
    },
184
    //Channel 6 ----- this is just a place holder so the next array element is channel 7
185
    {
186
            0x0,   //0
187
            {
188
                    0x0, //0
189
                    0x0 //0
190
            }
191
    },
192
    //Channel 7
193
    {
194
            0x93,   //PG_DELAY
195
            {
196
                    0x32527292, //16M prf power
197
                    0x5171B1d1 //64M prf power
198
            }
199
    }
200
};
201

    
202
//these are default antenna delays for EVB1000, these can be used if there is no calibration data in the DW1000,
203
//or instead of the calibration data
204
const uint16_t rfDelays[2] = {
205
        (uint16_t) ((DWT_PRF_16M_RFDLY/ 2.0) * 1e-9 / DWT_TIME_UNITS),//PRF 16
206
        (uint16_t) ((DWT_PRF_64M_RFDLY/ 2.0) * 1e-9 / DWT_TIME_UNITS)
207
};
208

    
209
//these are default TREK Tag/Anchor antenna delays
210
const uint16_t rfDelaysTREK[2] = {
211
        (uint16_t) ((514.83f/ 2.0) * 1e-9 / DWT_TIME_UNITS),//channel 2
212
        (uint16_t) ((514.65f/ 2.0) * 1e-9 / DWT_TIME_UNITS) //channel 5
213
};
214

    
215
//int instance_starttxtest(int framePeriod)
216
//{
217
//    //define some test data for the tx buffer
218
//    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";
219

    
220
//    //NOTE: SPI frequency must be < 3MHz
221
//    port_set_dw1000_slowrate();  //max SPI before PLLs configured is ~4M
222

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

    
227
//    dwt_writetxdata(127, (uint8 *)  msg, 0) ;
228
//    dwt_writetxfctrl(127, 0, 0);
229

    
230
//    //to start the first frame - set TXSTRT
231
//    dwt_starttx(DWT_START_TX_IMMEDIATE);
232

    
233
//    //measure the power
234
//    //Spectrum Analyser set:
235
//    //FREQ to be channel default e.g. 3.9936 GHz for channel 2
236
//    //SPAN to 1GHz
237
//    //SWEEP TIME 1s
238
//    //RBW and VBW 1MHz
239
//    //measure channel power
240

    
241
//    return DWT_SUCCESS ;
242
//}
243

    
244
// -------------------------------------------------------------------------------------------------------------------
245
//      Data Definitions
246
// -------------------------------------------------------------------------------------------------------------------
247

    
248
instance_data_t instance_data[NUM_INST] ;
249

    
250
static double inst_tdist[MAX_TAG_LIST_SIZE] ;
251
static double inst_idist[MAX_ANCHOR_LIST_SIZE] ;
252
static double inst_idistraw[MAX_ANCHOR_LIST_SIZE] ;
253

    
254
// TODO: Instance_data declaration in .c. Not on .h (extern only)
255
instance_data_t instance_data[NUM_INST] ;
256

    
257
/* TODO: Custom data declarations */
258
typedef struct
259
{
260
    uint32_t      deviceID ;
261
    uint8_t       chan;               // added chan here - used in the reading of acc
262
    uint16_t      rfrxDly;            // rf delay (delay though the RF blocks before the signal comes out of the antenna i.e. "antenna delay")
263
    uint16_t      rftxDly;            // rf delay (delay though the RF blocks before the signal comes out of the antenna i.e. "antenna delay")
264
    uint32_t      antennaDly;         // antenna delay read from OTP 64 PRF value is in high 16 bits and 16M PRF in low 16 bits
265
    uint32_t      antennaCals[4];     // antenna delays for the TREKs (Anchor high 16-bits, Tag low 16-bits)
266
    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)
267
    uint32_t      states[3] ;         //MP workaround debug states register
268
    uint8_t       statescount ;
269
    int           prfIndex ;
270
    uint32_t      ldoTune ;           //low 32 bits of LDO tune value
271
} platform_local_data_t ;
272

    
273
static platform_local_data_t platformLocalData ; // Static local device data
274

    
275
// -------------------------------------------------------------------------------------------------------------------
276
// Functions
277
// -------------------------------------------------------------------------------------------------------------------
278

    
279
// -------------------------------------------------------------------------------------------------------------------
280
// convert microseconds to device time
281
uint64_t convertmicrosectodevicetimeu (double microsecu){
282
  uint64_t dt;
283
  long double dtime;
284

    
285
  dtime = (microsecu / (double) DWT_TIME_UNITS) / 1e6 ;
286

    
287
  dt =  (uint64_t) (dtime) ;
288

    
289
  return dt;
290
}
291

    
292
double convertdevicetimetosec(int32_t dt){
293
  double f = 0;
294

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

    
297
  return f ;
298
}
299

    
300
#pragma GCC optimize ("O3")
301
int reportTOF(int idx, uint32_t tofx){
302
  double distance ;
303
  double distance_to_correct;
304
  double tof ;
305
  int32_t tofi ;
306

    
307
  // check for negative results and accept them making them proper negative integers
308
  tofi = (int32_t) tofx ; // make it signed
309
  if (tofi > 0x7FFFFFFF){  // close up TOF may be negative
310
    tofi -= 0x80000000 ;  //
311
  }
312

    
313
  // convert to seconds (as floating point)
314
  tof = convertdevicetimetosec(tofi) ;          //this is divided by 4 to get single time of flight
315
  inst_idistraw[idx] = distance = tof * SPEED_OF_LIGHT;
316

    
317
#if (CORRECT_RANGE_BIAS == 1)
318
  //for the 6.81Mb data rate we assume gating gain of 6dB is used,
319
  //thus a different range bias needs to be applied
320
  //if(inst->configData.dataRate == DWT_BR_6M8)
321
  if(instance_data[0].configData.smartPowerEn){
322
    //1.31 for channel 2 and 1.51 for channel 5
323
    if(instance_data[0].configData.chan == 5){
324
      distance_to_correct = distance/1.51;
325
    }
326
    else{   //channel 2
327
      distance_to_correct = distance/1.31;
328
    }
329
  }
330
  else{
331
    distance_to_correct = distance;
332
  }
333

    
334
  distance = distance - dwt_getrangebias(instance_data[0].configData.chan, (float) distance_to_correct, instance_data[0].configData.prf);
335
#endif
336

    
337
  if ((distance < 0) || (distance > 20000.000))    // discard any results less than <0 cm or >20 km
338
    return 0;
339

    
340
  inst_idist[idx] = distance;
341

    
342
  instance_data[0].longTermRangeCount++ ;                          // for computing a long term average
343

    
344
  return 1;
345
}// end of reportTOF
346

    
347
void setTagDist(int tidx, int aidx){
348
  inst_tdist[tidx] = inst_idist[aidx];
349
}
350

    
351
double getTagDist(int idx){
352
  return inst_tdist[idx];
353
}
354

    
355
void clearDistTable(int idx){
356
  inst_idistraw[idx] = 0;
357
  inst_idist[idx] = 0;
358
}
359

    
360
void instancecleardisttableall(void){
361
  int i;
362

    
363
  for(i=0; i<MAX_ANCHOR_LIST_SIZE; i++)  {
364
    inst_idistraw[i] = 0xffff;
365
    inst_idist[i] = 0xffff;
366
  }
367
}
368

    
369
// -------------------------------------------------------------------------------------------------------------------
370
#if NUM_INST != 1
371
#error These functions assume one instance only
372
#else
373

    
374

    
375
// -------------------------------------------------------------------------------------------------------------------
376
// Set this instance role as the Tag, Anchor or Listener
377
void instancesetrole(int inst_mode){
378
  // assume instance 0, for this
379
  instance_data[0].mode =  inst_mode;                   // set the role
380
}
381

    
382
int instancegetrole(void){
383
  return instance_data[0].mode;
384
}
385

    
386
int instancenewrange(void){
387
  int x = instance_data[0].newRange;
388
  instance_data[0].newRange = TOF_REPORT_NUL;
389
  return x;
390
}
391

    
392
int instancenewrangeancadd(void){
393
  return instance_data[0].newRangeAncAddress;
394
}
395

    
396
int instancenewrangetagadd(void){
397
  return instance_data[0].newRangeTagAddress;
398
}
399

    
400
int instancenewrangetim(void){
401
  return instance_data[0].newRangeTime;
402
}
403

    
404
// -------------------------------------------------------------------------------------------------------------------
405
// function to clear counts/averages/range values
406
//
407
void instanceclearcounts(void){
408
  int instance = 0 ;
409
  int i= 0 ;
410

    
411
  instance_data[instance].rxTimeouts = 0 ;
412

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

    
415
  instance_data[instance].frameSN = 0;
416

    
417
  instance_data[instance].longTermRangeCount  = 0;
418

    
419

    
420
  for(i=0; i<MAX_ANCHOR_LIST_SIZE; i++){
421
    instance_data[instance].tofArray[i] = INVALID_TOF;
422
  }
423

    
424
  for(i=0; i<MAX_TAG_LIST_SIZE; i++){
425
    instance_data[instance].tof[i] = INVALID_TOF;
426
  }
427

    
428
} // end instanceclearcounts()
429

    
430

    
431
// -------------------------------------------------------------------------------------------------------------------
432
// function to initialise instance structures
433
//
434
// Returns 0 on success and -1 on error
435
//int instance_init(void){   // TODO
436
int instance_init(DW1000Driver* drv){
437
  int instance = 0 ;
438
  int i;
439
  int result;
440

    
441
  instance_data[instance].mode =  ANCHOR;                                // assume listener,
442
  instance_data[instance].testAppState = TA_INIT ;
443
  instance_data[instance].instToSleep = FALSE;
444

    
445

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

    
450
  //this initialises DW1000 and uses specified configurations from OTP/ROM
451
  result = dwt_initialise(DWT_LOADUCODE, drv) ;    // TODO
452
//  result = dwt_initialise(DWT_LOADUCODE) ;
453

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

    
457
  if (DWT_SUCCESS != result){
458
    return (-1) ;   // device initialise has failed
459
  }
460

    
461

    
462
  instanceclearcounts() ;
463

    
464
  instance_data[instance].panID = 0xdeca ;
465

    
466
  instance_data[instance].wait4ack = 0;
467
  instance_data[instance].stopTimer = 0;
468
  instance_data[instance].instanceTimerEn = 0;
469

    
470
  instance_clearevents();
471

    
472
  //dwt_geteui(instance_data[instance].eui64);
473
  memset(instance_data[instance].eui64, 0, ADDR_BYTE_SIZE_L);
474

    
475
  instance_data[instance].tagSleepCorrection = 0;
476

    
477
//  dwt_setautorxreenable(0); //disable auto RX re-enable
478
  dwt_setdblrxbuffmode(0); //disable double RX buffer
479

    
480
  // if using auto CRC check (DWT_INT_RFCG and DWT_INT_RFCE) are used instead of DWT_INT_RDFR flag
481
  // other errors which need to be checked (as they disable receiver) are
482
  //dwt_setinterrupt(DWT_INT_TFRS | DWT_INT_RFCG | (DWT_INT_SFDT | DWT_INT_RFTO /*| DWT_INT_RXPTO*/), 1);
483
  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);
484

    
485
  dwt_setcallbacks(instance_txcallback, instance_rxcallback, instance_rxtimeoutcallback,instance_rxerrorcallback );
486

    
487
  instance_data[instance].monitor = 0;
488

    
489
  instance_data[instance].lateTX = 0;
490
  instance_data[instance].lateRX = 0;
491

    
492
  instance_data[instance].responseTO = -1; //initialise
493
  for(i=0; i<256; i++)  {
494
    instance_data[instance].rxResps[i] = -10;
495
  }
496

    
497
  instance_data[instance].delayedReplyTime = 0;
498

    
499
  return 0 ;
500
}
501

    
502
// -------------------------------------------------------------------------------------------------------------------
503
//
504
// Return the Device ID register value, enables higher level validation of physical device presence
505
//
506

    
507
uint32_t instancereaddeviceid(void){
508
  return dwt_readdevid() ;
509
}
510

    
511

    
512
// -------------------------------------------------------------------------------------------------------------------
513
//
514
// function to allow application configuration be passed into instance and affect underlying device operation
515
//
516
void instance_config(instanceConfig_t *config, sfConfig_t *sfConfig){
517
  int instance = 0 ;
518
  uint32_t power = 0;
519
  uint8_t otprev ;
520

    
521
  instance_data[instance].configData.chan = config->channelNumber ;
522
  instance_data[instance].configData.rxCode =  config->preambleCode ;
523
  instance_data[instance].configData.txCode = config->preambleCode ;
524
  instance_data[instance].configData.prf = config->pulseRepFreq ;
525
  instance_data[instance].configData.dataRate = config->dataRate ;
526
  instance_data[instance].configData.txPreambLength = config->preambleLen ;
527
  instance_data[instance].configData.rxPAC = config->pacSize ;
528
  instance_data[instance].configData.nsSFD = config->nsSFD ;
529
  instance_data[instance].configData.phrMode = DWT_PHRMODE_STD ;
530
  instance_data[instance].configData.sfdTO = config->sfdTO;
531

    
532
  //the DW1000 will automatically use gating gain for frames < 1ms duration (i.e. 6.81Mbps data rate)
533
  //smartPowerEn should be set based on the frame length, but we can also use dtaa rate.
534
  if(instance_data[instance].configData.dataRate == DWT_BR_6M8)  {
535
    instance_data[instance].configData.smartPowerEn = 1;
536
  }
537
  else{
538
    instance_data[instance].configData.smartPowerEn = 0;
539
  }
540

    
541
  //configure the channel parameters
542
  dwt_configure(&instance_data[instance].configData) ;
543

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

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

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

    
553
  //Configure TX power
554
  instance_data[instance].configTX.power = power;
555

    
556
  //configure the tx spectrum parameters (power and PG delay)
557
  dwt_configuretxrf(&instance_data[instance].configTX);
558

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

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

    
565
    instance_data[instance].txAntennaDelay
566
    = dwt_getTREKOTPantennadelay(mode,
567
                                 instance_data[instance].configData.chan,
568
                                 instance_data[instance].configData.dataRate) ;
569

    
570
    // if nothing was actually programmed then set a reasonable value anyway
571
    if ((instance_data[instance].txAntennaDelay == 0)
572
        || (instance_data[instance].txAntennaDelay == 0xffff)){
573
      if(instance_data[instance].configData.chan == 5){
574
        chanindex = 1;
575
      }
576

    
577
      instance_data[instance].txAntennaDelay = rfDelaysTREK[chanindex];
578
    }
579

    
580
  }
581
  else {  // assume it is older EVK1000 programming.
582
    //get the antenna delay that was read from the OTP calibration area
583
    instance_data[instance].txAntennaDelay = dwt_readantennadelay(config->pulseRepFreq) >> 1;
584

    
585
    // if nothing was actually programmed then set a reasonable value anyway
586
    if ((instance_data[instance].txAntennaDelay == 0)
587
        || (instance_data[instance].txAntennaDelay == 0xffff)){
588
      instance_data[instance].txAntennaDelay = rfDelays[config->pulseRepFreq - DWT_PRF_16M];
589
    }
590
  }
591

    
592
  // -------------------------------------------------------------------------------------------------------------------
593
  // set the antenna delay, we assume that the RX is the same as TX.
594
  dwt_setrxantennadelay(instance_data[instance].txAntennaDelay);
595
  dwt_settxantennadelay(instance_data[instance].txAntennaDelay);
596

    
597
  instance_data[instance].rxAntennaDelay = instance_data[instance].txAntennaDelay;
598

    
599
  if(config->preambleLen == DWT_PLEN_64)  {  //if preamble length is 64
600
    //reduce SPI to < 3MHz
601
    setHighSpeed_SPI(FALSE);  //TODO: workaround using flag to change speed
602
    dwt_loadopsettabfromotp(0);
603
    //increase SPI to max
604
    setHighSpeed_SPI(TRUE);  //TODO: workaround using flag to change speed
605
  }
606

    
607
  instancesettagsleepdelay(sfConfig->pollSleepDly); //set the Tag sleep time
608
  instance_data[instance].sframePeriod = sfConfig->sfPeriod;
609
  instance_data[instance].slotPeriod = sfConfig->slotPeriod;
610
  instance_data[instance].tagSleepRnd = sfConfig->slotPeriod;
611
  instance_data[instance].numSlots = sfConfig->numSlots;
612

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

    
616
  //set the default response delays
617
  instancesetreplydelay(sfConfig->replyDly);
618

    
619
}
620

    
621
// -------------------------------------------------------------------------------------------------------------------
622
// function to set the tag sleep time (in ms)
623
//
624
void instancesettagsleepdelay(int sleepdelay) {  //sleep in ms
625
  int instance = 0 ;
626
  instance_data[instance].tagSleepTime_ms = sleepdelay; //subtract the micro system delays (time it takes to switch states etc.)
627
}
628

    
629

    
630
int instancegetrnum(void) {  //get ranging number
631
  return instance_data[0].rangeNum;
632
}
633

    
634
int instancegetrnuma(int idx){  //get ranging number
635
  return instance_data[0].rangeNumA[idx];
636
}
637

    
638
int instancegetrnumanc(int idx){  //get ranging number
639
  return instance_data[0].rangeNumAAnc[idx];
640
}
641

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

    
645
  return (x);
646
}
647

    
648
double instancegetidist(int idx) {  //get instantaneous range
649
  double x ;
650

    
651
  idx &= (MAX_ANCHOR_LIST_SIZE - 1);
652

    
653
  x = inst_idist[idx];
654

    
655
  return (x);
656
}
657

    
658
double instancegetidistraw(int idx) { //get instantaneous range (uncorrected)
659
  double x ;
660

    
661
  idx &= (MAX_ANCHOR_LIST_SIZE - 1);
662

    
663
  x = inst_idistraw[idx];
664

    
665
  return (x);
666
}
667

    
668
int instancegetidist_mm(int idx) {  //get instantaneous range
669
  int x ;
670

    
671
  idx &= (MAX_ANCHOR_LIST_SIZE - 1);
672

    
673
  x = (int)(inst_idist[idx]*1000);
674

    
675
  return (x);
676
}
677

    
678
int instancegetidistraw_mm(int idx) {  //get instantaneous range (uncorrected)
679
  int x ;
680

    
681
  idx &= (MAX_ANCHOR_LIST_SIZE - 1);
682

    
683
  x = (int)(inst_idistraw[idx]*1000);
684

    
685
  return (x);
686
}
687

    
688
void instance_backtoanchor(instance_data_t *inst){
689
  //stay in RX and behave as anchor
690
  inst->testAppState = TA_RXE_WAIT ;
691
  inst->mode = ANCHOR ;
692
  dwt_setrxtimeout(0);
693
  dwt_setpreambledetecttimeout(0);
694
  dwt_setrxaftertxdelay(0);
695
}
696

    
697

    
698
#pragma GCC optimize ("O3")
699
void inst_processrxtimeout(instance_data_t *inst){
700

    
701
  //inst->responseTimeouts ++ ;
702
  inst->rxTimeouts ++ ;
703
  inst->done = INST_NOT_DONE_YET;
704

    
705
  if(inst->mode == ANCHOR) { //we did not receive the final - wait for next poll
706
    //only enable receiver when not using double buffering
707
    inst->testAppState = TA_RXE_WAIT ;              // wait for next frame
708
    dwt_setrxtimeout(0);
709
  }
710

    
711
  else if(inst->mode == TAG) {
712
    //if tag times out - no response (check if we are to send a final)
713
    //send the final only if it has received response from anchor 0
714
    if((inst->previousState == TA_TXPOLL_WAIT_SEND) && ((inst->rxResponseMask & 0x1) == 0)) {
715
      inst->instToSleep = TRUE ; //set sleep to TRUE so that tag will go to DEEP SLEEP before next ranging attempt
716
      inst->testAppState = TA_TXE_WAIT ;
717
      inst->nextState = TA_TXPOLL_WAIT_SEND ;
718
    }
719
    else if (inst->previousState == TA_TXFINAL_WAIT_SEND) {  //got here from main (error sending final - handle as timeout)
720
      dwt_forcetrxoff();        //this will clear all events
721
      inst->instToSleep = TRUE ;
722
      // initiate the re-transmission of the poll that was not responded to
723
      inst->testAppState = TA_TXE_WAIT ;
724
      inst->nextState = TA_TXPOLL_WAIT_SEND ;
725
    }
726
    else {  //send the final
727
      // initiate the re-transmission of the poll that was not responded to
728
      inst->testAppState = TA_TXE_WAIT ;
729
      inst->nextState = TA_TXFINAL_WAIT_SEND ;
730
    }
731

    
732
  }
733
  else {  //ANCHOR_RNG
734
    //no Response form the other anchor
735
    if(
736
        ((inst->previousState == TA_TXPOLL_WAIT_SEND)
737
            && ((A1_ANCHOR_ADDR == inst->instanceAddress16) && ((inst->rxResponseMaskAnc & 0x4) == 0)))
738
            ||
739
            ((inst->previousState == TA_TXPOLL_WAIT_SEND)
740
                && ((GATEWAY_ANCHOR_ADDR == inst->instanceAddress16) && ((inst->rxResponseMaskAnc & 0x2) == 0)))
741
    )  {
742
      instance_backtoanchor(inst);
743
    }
744
    else if (inst->previousState == TA_TXFINAL_WAIT_SEND) {  //got here from main (error ending final - handle as timeout)
745
      instance_backtoanchor(inst);
746
    }
747
    else {  //send the final
748
      // initiate the re-transmission of the poll that was not responded to
749
      inst->testAppState = TA_TXE_WAIT ;
750
      inst->nextState = TA_TXFINAL_WAIT_SEND ;
751
    }
752
  }
753

    
754
  //timeout - disable the radio (if using SW timeout the rx will not be off)
755
  //dwt_forcetrxoff() ;
756
}
757

    
758
//
759
// NB: This function is called from the (TX) interrupt handler
760
//
761
#pragma GCC optimize ("O3")
762
void instance_txcallback(const dwt_cb_data_t *txd){
763

    
764
  (void) txd;
765
  int instance = 0;
766
  uint8_t txTimeStamp[5] = {0, 0, 0, 0, 0};
767
//  uint8 txevent = txd->event;
768
  event_data_t dw_event;
769

    
770
  dw_event.uTimeStamp = portGetTickCnt();
771

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

    
774
//  if(txevent == DWT_SIG_TX_DONE) {
775
    //uint32_t txtimestamp = 0;
776

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

    
780
    dwt_readtxtimestamp(txTimeStamp) ;
781
    dw_event.timeStamp32l = (uint32_t)txTimeStamp[0] + ((uint32_t)txTimeStamp[1] << 8) + ((uint32_t)txTimeStamp[2] << 16) + ((uint32_t)txTimeStamp[3] << 24);
782
    dw_event.timeStamp = txTimeStamp[4];
783
    dw_event.timeStamp <<= 32;
784
    dw_event.timeStamp += dw_event.timeStamp32l;
785
    dw_event.timeStamp32h = ((uint32_t)txTimeStamp[4] << 24) + (dw_event.timeStamp32l >> 8);
786

    
787
    instance_data[instance].stopTimer = 0;
788

    
789
    dw_event.rxLength = instance_data[instance].psduLength;
790
    dw_event.type =  0;
791
    dw_event.type_pend =  0;
792
    dw_event.type_save = DWT_SIG_TX_DONE;
793

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

    
796
    instance_putevent(dw_event, DWT_SIG_TX_DONE);
797

    
798
    instance_data[instance].txMsgCount++;
799
//  }
800
//  else if(txevent == DWT_SIG_TX_AA_DONE)  {
801
//    //auto ACK confirmation
802
//    dw_event.rxLength = 0;
803
//    dw_event.type =  0;
804
//    dw_event.type_save = DWT_SIG_TX_AA_DONE;
805
//
806
//    instance_putevent(dw_event, DWT_SIG_TX_AA_DONE);
807
//
808
//    //printf("TX AA time %f ecount %d\n",convertdevicetimetosecu(instance_data[instance].txu.txTimeStamp), instance_data[instance].dweventCnt);
809
//  }
810

    
811
  instance_data[instance].monitor = 0;
812
}
813

    
814
/**
815
 * @brief function to re-enable the receiver and also adjust the timeout before sending the final message
816
 * if it is time so send the final message, the callback will notify the application, else the receiver is
817
 * automatically re-enabled
818
 *
819
 * this function is only used for tag when ranging to other anchors
820
 */
821
uint8_t tagrxreenable(uint16_t sourceAddress){
822
  uint8_t type_pend = DWT_SIG_DW_IDLE;
823
  uint8_t anc = sourceAddress & 0x3;
824
  int instance = 0;
825

    
826
  switch(anc){
827
  //if we got Response from anchor 3 - this is the last expected response - send the final
828
  case 3:
829
    type_pend = DWT_SIG_DW_IDLE;
830
    break;
831

    
832
    //if we got Response from anchor 0, 1, or 2 - go back to wait for next anchor's response
833
  case 0:
834
  case 1:
835
  case 2:
836
  default:
837
    if(instance_data[instance].responseTO > 0) {  //can get here as result of error frame so need to check
838
      dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTime_sy * instance_data[instance].responseTO); //reconfigure the timeout
839
      dwt_rxenable(DWT_START_RX_IMMEDIATE) ;
840
      type_pend = DWT_SIG_RX_PENDING ;
841
    }
842
    else //last response was not received (got error/frame was corrupt)
843
    {
844
      type_pend = DWT_SIG_DW_IDLE; //report timeout - send the final
845
    }
846
    break;
847
  }
848

    
849
  return type_pend;
850
}
851

    
852
/**
853
 * @brief function to re-enable the receiver and also adjust the timeout before sending the final message
854
 * if it is time so send the final message, the callback will notify the application, else the receiver is
855
 * automatically re-enabled
856
 *
857
 * this function is only used for anchors (having a role of ANCHOR_RNG) when ranging to other anchors
858
 */
859
uint8_t ancsendfinalorrxreenable(uint16_t sourceAddress){
860
  uint8_t type_pend = DWT_SIG_DW_IDLE;
861
  uint8_t anc = sourceAddress & 0x3;
862
  int instance = 0;
863

    
864
  if(instance_data[instance].instanceAddress16 == GATEWAY_ANCHOR_ADDR) {
865
    switch(anc) {
866
    //if we got Response from anchor 1 - go back to wait for next anchor's response
867
    case 1:
868
      dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTime_sy); //reconfigure the timeout
869
      dwt_rxenable(DWT_START_RX_IMMEDIATE) ;
870
      type_pend = DWT_SIG_RX_PENDING ;
871
      break;
872

    
873
      //if we got Response from anchor 2 - this is the last expected response - send the final
874
    case 2:
875
    default:
876
      type_pend = DWT_SIG_DW_IDLE;
877
      break;
878
    }
879
  }
880

    
881
  if(instance_data[instance].instanceAddress16 == A1_ANCHOR_ADDR){
882
    switch(anc)
883
    {
884
    //if we got Response from anchor 2 - this is the last expected response - send the final
885
    case 2:
886
    default:
887
      type_pend = DWT_SIG_DW_IDLE;
888
      break;
889
    }
890
  }
891
  return type_pend;
892
}
893

    
894
/**
895
 * @brief this function either enables the receiver (delayed)
896
 *
897
 **/
898
void ancenablerx(void){
899
  int instance = 0;
900
  //subtract preamble length
901
  dwt_setdelayedtrxtime(instance_data[instance].delayedReplyTime - instance_data[instance].fixedReplyDelayAncP) ;
902
  if(dwt_rxenable(DWT_START_RX_DELAYED)) {  //delayed rx
903
    //if the delayed RX failed - time has passed - do immediate enable
904
    //led_on(LED_PC9);
905
    dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTimeAnc_sy*2); //reconfigure the timeout before enable
906
    //longer timeout as we cannot do delayed receive... so receiver needs to stay on for longer
907
    dwt_rxenable(DWT_START_RX_IMMEDIATE);
908
    dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTimeAnc_sy); //restore the timeout for next RX enable
909
    instance_data[instance].lateRX++;
910
    //led_off(LED_PC9);
911
  }
912

    
913
}
914

    
915
/**
916
 * @brief this function either re-enables the receiver (delayed or immediate) or transmits the response frame
917
 *
918
 * @param the sourceAddress is the address of the sender of the current received frame
919
 * @param ancToAncTWR == 1 means that the anchor is ranging to another anchor, if == 0 then ranging to a tag
920
 *
921
 */
922
#pragma GCC optimize ("O0")
923
uint8_t anctxorrxreenable(uint16_t sourceAddress, int ancToAncTWR){
924
  uint8_t type_pend = DWT_SIG_DW_IDLE;
925
  int sendResp = 0;
926
  int instance = 0;
927

    
928
  if(instance_data[instance].responseTO == 0) {  //go back to RX without TO - ranging has finished. (wait for Final but no TO)
929
    dwt_setrxtimeout(0); //reconfigure the timeout
930
    dwt_setpreambledetecttimeout(0);
931
  }
932

    
933
  if((ancToAncTWR & 1) == 1)  {
934
    if(instance_data[instance].responseTO == 1) {  //if one response left to receive (send a response now)
935
      sendResp = 1;
936
    }
937
    //if A0 or A3 go back to RX as they do not send any responses when Anchor to Anchor ranging
938
    if((instance_data[instance].gatewayAnchor)
939
        || (instance_data[instance].shortAdd_idx == 3)) { //if this is anchor ID 3 do not reply to anchor poll
940
      dwt_setrxtimeout(0);
941
      dwt_rxenable(DWT_START_RX_IMMEDIATE);
942
      return DWT_SIG_RX_PENDING ;
943
    }
944
  }
945

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

    
949
  //this checks if to send a frame
950
  if((((ancToAncTWR & 1) == 0) && ((instance_data[instance].responseTO + instance_data[instance].shortAdd_idx) == NUM_EXPECTED_RESPONSES)) //it's our turn to tx
951
      || (sendResp == 1)) {
952
    //led_on(LED_PC9);
953
    //response is expected
954
    instance_data[instance].wait4ack = DWT_RESPONSE_EXPECTED; //re has/will be re-enabled
955

    
956
    dwt_setdelayedtrxtime(instance_data[instance].delayedReplyTime) ;
957
    if(dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED)) {
958
      //if TX has failed - we need to re-enable RX for the next response or final reception...
959
      dwt_setrxaftertxdelay(0);
960
      instance_data[instance].wait4ack = 0; //clear the flag as the TX has failed the TRX is off
961
      instance_data[instance].lateTX++;
962
      instance_data[instance].delayedReplyTime += 2*(instance_data[instance].fixedReplyDelayAnc >> 8); //to take into account W4R
963
      ancenablerx();
964
      type_pend = DWT_SIG_RX_PENDING ;
965
    }
966
    else {
967
      instance_data[instance].delayedReplyTime += (instance_data[instance].fixedReplyDelayAnc >> 8); //to take into account W4R
968
      type_pend = DWT_SIG_TX_PENDING ; // exit this interrupt and notify the application/instance that TX is in progress.
969
      instance_data[instance].timeofTx = portGetTickCnt();
970
      instance_data[instance].monitor = 1;
971
    }
972
    //led_off(LED_PC9);
973
  }
974
  else {  //stay in receive
975
    if(sourceAddress == 0) { //we got here after RX error, as we don't need to TX, we just enable RX
976
      dwt_setrxtimeout(0);
977
      dwt_rxenable(DWT_START_RX_IMMEDIATE);
978
    }
979
    else{
980
      //led_on(LED_PC9);
981
      ancenablerx();
982
      //led_off(LED_PC9);
983
    }
984

    
985
    type_pend = DWT_SIG_RX_PENDING ;
986
  }
987
  //if time to send a response
988

    
989
  return type_pend;
990
}
991

    
992
/**
993
 * @brief this function handles frame error event, it will either signal TO or re-enable the receiver
994
 */
995
void handle_error_unknownframe(event_data_t dw_event){
996
  int instance = 0;
997
  //re-enable the receiver (after error frames as we are not using auto re-enable
998
  //for ranging application rx error frame is same as TO - as we are not going to get the expected frame
999
  if(instance_data[instance].mode == ANCHOR){
1000
    //if we are participating in the ranging (i.e. Poll was received)
1001
    //and we get an rx error (in one of the responses)
1002
    //need to consider this as a timeout as we could be sending our response next and
1003
    //the applications needs to know to change the state
1004
    //
1005
    if(instance_data[instance].responseTO > 0){
1006
      instance_data[instance].responseTO--;
1007

    
1008
      //send a response or re-enable rx
1009
      dw_event.type_pend = anctxorrxreenable(0, 0);
1010
      dw_event.type = 0;
1011
      dw_event.type_save = 0x40 | DWT_SIG_RX_TIMEOUT;
1012
      dw_event.rxLength = 0;
1013

    
1014
      instance_putevent(dw_event, DWT_SIG_RX_TIMEOUT);
1015
    }
1016
    else{
1017
      dwt_setrxtimeout(0); //reconfigure the timeout
1018
      dwt_rxenable(DWT_START_RX_IMMEDIATE) ;
1019
    }
1020
  }
1021
  else if(instance_data[instance].mode == LISTENER){
1022
    dwt_rxenable(DWT_START_RX_IMMEDIATE) ;
1023
  }
1024
  else{
1025
    instance_data[instance].responseTO--; //got something (need to reduce timeout (for remaining responses))
1026

    
1027
    dw_event.type_pend = tagrxreenable(0); //check if receiver will be re-enabled or it's time to send the final
1028
    dw_event.type = 0;
1029
    dw_event.type_save = 0x40 | DWT_SIG_RX_TIMEOUT;
1030
    dw_event.rxLength = 0;
1031

    
1032
    instance_putevent(dw_event, DWT_SIG_RX_TIMEOUT);
1033
  }
1034
}
1035

    
1036

    
1037
/**
1038
 * @brief this function prepares and writes the anchor to anchor response frame into the TX buffer
1039
 * it is called after anchor receives a Poll from an anchor
1040
 */
1041
void ancprepareresponse2(uint16_t sourceAddress, uint8_t srcAddr_index, uint8_t fcode_index, uint8_t *frame){
1042
  uint16_t frameLength = 0;
1043
  uint8_t tof_idx = (sourceAddress) & 0x3 ;
1044
  int instance = 0;
1045

    
1046
  instance_data[instance].psduLength = frameLength = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
1047
  //set the destination address (copy source as this is a reply)
1048
  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)
1049
  instance_data[instance].msg_f.sourceAddr[0] = instance_data[instance].eui64[0];
1050
  instance_data[instance].msg_f.sourceAddr[1] = instance_data[instance].eui64[1];
1051
  // Write calculated TOF into response message (get the previous ToF+range number from that anchor)
1052
  memcpy(&(instance_data[instance].msg_f.messageData[TOFR]), &instance_data[instance].tofAnc[tof_idx], 4);
1053
  instance_data[instance].msg_f.messageData[TOFRN] = instance_data[instance].rangeNumAAnc[tof_idx]; //get the previous range number
1054

    
1055
  instance_data[instance].rangeNumAAnc[tof_idx] = 0; //clear the entry
1056
  instance_data[instance].rangeNumAnc = frame[POLL_RNUM + fcode_index] ;
1057
  instance_data[instance].msg_f.seqNum = instance_data[instance].frameSN++;
1058

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

    
1062
  instance_data[instance].tagSleepCorrection = 0;
1063
  instance_data[instance].msg_f.messageData[RES_TAG_SLP0] = 0 ;
1064
  instance_data[instance].msg_f.messageData[RES_TAG_SLP1] = 0 ;
1065

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

    
1068
  //write the TX data
1069
  dwt_writetxfctrl(frameLength, 0, 1);
1070
  dwt_writetxdata(frameLength, (uint8_t *)  &instance_data[instance].msg_f, 0) ;        // write the frame data
1071

    
1072
}
1073

    
1074
/**
1075
 * @brief this function prepares and writes the anchor to tag response frame into the TX buffer
1076
 * it is called after anchor receives a Poll from a tag
1077
 */
1078
void ancprepareresponse(uint16_t sourceAddress, uint8_t srcAddr_index, uint8_t fcode_index, uint8_t *frame, uint32_t uTimeStamp){
1079
  uint16_t frameLength = 0;
1080
  uint8_t tof_idx = (sourceAddress) & 0x7 ;
1081
  int instance = 0;
1082

    
1083
  instance_data[instance].psduLength = frameLength = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
1084
  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)
1085
  instance_data[instance].msg_f.sourceAddr[0] = instance_data[instance].eui64[0];
1086
  instance_data[instance].msg_f.sourceAddr[1] = instance_data[instance].eui64[1];
1087
  // Write calculated TOF into response message (get the previous ToF+range number from that tag)
1088
  memcpy(&(instance_data[instance].msg_f.messageData[TOFR]), &instance_data[instance].tof[tof_idx], 4);
1089
  instance_data[instance].msg_f.messageData[TOFRN] = instance_data[instance].rangeNumA[tof_idx]; //get the previous range number
1090

    
1091
  instance_data[instance].rangeNumA[tof_idx] = 0; //clear after copy above...
1092
  instance_data[instance].rangeNum = frame[POLL_RNUM+fcode_index] ;
1093
  instance_data[instance].msg_f.seqNum = instance_data[instance].frameSN++;
1094

    
1095
  //we have our range - update the own mask entry...
1096
  if(instance_data[instance].tof[tof_idx] != INVALID_TOF) { //check the last ToF entry is valid and copy into the current array
1097
    instance_data[instance].rxResponseMask = (0x1 << instance_data[instance].shortAdd_idx);
1098
    instance_data[instance].tofArray[instance_data[instance].shortAdd_idx] = instance_data[instance].tof[tof_idx];
1099
  }
1100
  else        {  //reset response mask
1101
    instance_data[instance].tofArray[instance_data[instance].shortAdd_idx] = INVALID_TOF ;
1102
    instance_data[instance].rxResponseMask = 0;        //reset the mask of received responses when rx poll
1103
  }
1104
  //set the delayed rx on time (the final message will be sent after this delay)
1105
  dwt_setrxaftertxdelay(instance_data[instance].ancRespRxDelay);  //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
1106

    
1107
  //if this is gateway anchor - calculate the slot period correction
1108
  if(instance_data[instance].gatewayAnchor)  {
1109
    int error = 0;
1110
    int currentSlotTime = 0;
1111
    int expectedSlotTime = 0;
1112
    //find the time in the current superframe
1113
    currentSlotTime = uTimeStamp % instance_data[instance].sframePeriod;
1114

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

    
1118
    //error = expectedSlotTime - currentSlotTime
1119
    error = expectedSlotTime - currentSlotTime;
1120

    
1121
    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
1122
      instance_data[instance].tagSleepCorrection = (instance_data[instance].sframePeriod + error);
1123
    }
1124
    else { //the minimum Sleep time will be 0.5 period
1125
      instance_data[instance].tagSleepCorrection = error;
1126
    }
1127
    instance_data[instance].msg_f.messageData[RES_TAG_SLP0] = instance_data[instance].tagSleepCorrection & 0xFF ;
1128
    instance_data[instance].msg_f.messageData[RES_TAG_SLP1] = (instance_data[instance].tagSleepCorrection >> 8) & 0xFF;
1129
  }
1130
  else {
1131
    instance_data[instance].tagSleepCorrection = 0;
1132
    instance_data[instance].msg_f.messageData[RES_TAG_SLP0] = 0 ;
1133
    instance_data[instance].msg_f.messageData[RES_TAG_SLP1] = 0 ;
1134
  }
1135
  instance_data[instance].msg_f.messageData[FCODE] = RTLS_DEMO_MSG_ANCH_RESP; //message function code (specifies if message is a poll, response or other...)
1136

    
1137
  //write the TX data
1138
  dwt_writetxfctrl(frameLength, 0, 1);
1139
  dwt_writetxdata(frameLength, (uint8_t *)  &instance_data[instance].msg_f, 0) ;        // write the frame data
1140
}
1141

    
1142
/**
1143
 * @brief this is the receive event callback handler, the received event is processed and the instance either
1144
 * responds by sending a response frame or re-enables the receiver to await the next frame
1145
 * once the immediate action is taken care of the event is queued up for application to process
1146
 */
1147
#pragma GCC optimize ("O3")
1148
void instance_rxcallback(const dwt_cb_data_t *rxd){
1149
  int instance = 0;
1150
  uint8_t rxTimeStamp[5]  = {0, 0, 0, 0, 0};
1151

    
1152
  uint8_t rxd_event = 0;
1153
  uint8_t fcode_index  = 0;
1154
  uint8_t srcAddr_index = 0;
1155
  event_data_t dw_event;
1156

    
1157
  //microcontroller time at which we received the frame
1158
  dw_event.uTimeStamp = portGetTickCnt();
1159

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

    
1164
  //need to process the frame control bytes to figure out what type of frame we have received
1165
  if(((rxd->fctrl[0] == 0x41) || (rxd->fctrl[0] == 0x61))
1166
      &&
1167
      ((rxd->fctrl[1] & 0xCC) == 0x88)) {  //short address
1168
    fcode_index = FRAME_CRTL_AND_ADDRESS_S; //function code is in first byte after source address
1169
    srcAddr_index = FRAME_CTRLP + ADDR_BYTE_SIZE_S;
1170
    rxd_event = DWT_SIG_RX_OKAY;
1171
  }
1172
  else {
1173
    rxd_event = SIG_RX_UNKNOWN; //not supported - all TREK1000 frames are short addressed
1174
  }
1175

    
1176
  //read RX timestamp
1177
  dwt_readrxtimestamp(rxTimeStamp) ;
1178
  dwt_readrxdata((uint8_t *)&dw_event.msgu.frame[0], rxd->datalength, 0);  // Read Data Frame
1179
  dw_event.timeStamp32l =  (uint32_t)rxTimeStamp[0] + ((uint32_t)rxTimeStamp[1] << 8) + ((uint32_t)rxTimeStamp[2] << 16) + ((uint32_t)rxTimeStamp[3] << 24);
1180
  dw_event.timeStamp = rxTimeStamp[4];
1181
  dw_event.timeStamp <<= 32;
1182
  dw_event.timeStamp += dw_event.timeStamp32l;
1183
  dw_event.timeStamp32h = ((uint32_t)rxTimeStamp[4] << 24) + (dw_event.timeStamp32l >> 8);
1184

    
1185
  dw_event.type = 0; //type will be added as part of adding to event queue
1186
  dw_event.type_save = rxd_event;
1187
  dw_event.type_pend = DWT_SIG_DW_IDLE;
1188

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

    
1193
    if(instance_data[instance].mode != LISTENER)      {
1194
      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
1195
        //(although due to frame filtering this is limited as non-addressed frames are filtered out)
1196
      {
1197
        instance_data[instance].responseTO--; //got 1 more response or other RX frame - need to reduce timeout (for next response)
1198
      }
1199

    
1200
      //check if this is a TWR message (and also which one)
1201
      switch(dw_event.msgu.frame[fcode_index]){
1202
      //poll message from an anchor
1203
      case RTLS_DEMO_MSG_ANCH_POLL:{
1204
        //the anchor to anchor ranging poll frames are ignored by A0 and A3
1205
        if(instance_data[instance].gatewayAnchor || (instance_data[instance].instanceAddress16 > A2_ANCHOR_ADDR)){
1206
          //ignore poll from anchor 1 if gateway or anchor 3
1207
          //anchors 2 and 3 will never send polls
1208
          dw_event.type_pend = DWT_SIG_DW_IDLE ;
1209
          break;
1210
        }
1211

    
1212
        if(instance_data[instance].mode == TAG)  {  //tag should ignore any other Polls from anchors
1213
          instance_data[instance].responseTO++; //as will be decremented in the function and was also decremented above
1214
          handle_error_unknownframe(dw_event);
1215
          instance_data[instance].stopTimer = 1;
1216
          instance_data[instance].rxMsgCount++;
1217
          return;
1218
        }
1219

    
1220
        //update the response index and number of responses received tables
1221
        instance_data[instance].rxRespsIdx = (uint8_t) ((dw_event.msgu.frame[POLL_RNUM+fcode_index] & 0xf)
1222
            + (((sourceAddress&0x3) + 8) << 4));
1223
        instance_data[instance].rxResps[instance_data[instance].rxRespsIdx] = 0;
1224
        //debug LED on
1225
        //                                                led_on(LED_PC9);
1226

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

    
1230
        //A2 does not need timeout if ranging to A1
1231
        if(sourceAddress != A1_ANCHOR_ADDR){
1232
          dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTimeAnc_sy); //reconfigure the timeout for response
1233
        }
1234

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

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

    
1241
        instance_data[instance].tofAnc[sourceAddress & 0x3] = INVALID_TOF; //clear ToF ..
1242
        //debug LED off
1243
        //                                                led_off(LED_PC9);
1244
        break;
1245
      }
1246

    
1247
      case RTLS_DEMO_MSG_TAG_POLL:{
1248
        if(instance_data[instance].mode == TAG) {  //tag should ignore any other Polls from tags
1249
          instance_data[instance].responseTO++; //as will be decremented in the function and was also decremented above
1250
          handle_error_unknownframe(dw_event);
1251
          instance_data[instance].stopTimer = 1;
1252
          instance_data[instance].rxMsgCount++;
1253
          return;
1254
        }
1255
        instance_data[instance].rxRespsIdx = (int8_t) ((dw_event.msgu.frame[POLL_RNUM+fcode_index] & 0xf)
1256
            + ((sourceAddress&0x7) << 4));
1257
        instance_data[instance].rxResps[instance_data[instance].rxRespsIdx] = 0;
1258

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

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

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

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

    
1269
        instance_data[instance].tof[sourceAddress & 0x7] = INVALID_TOF; //clear ToF ..
1270
      }
1271
      break;
1272

    
1273
      //we got a response from a "responder" (anchor)
1274
      case RTLS_DEMO_MSG_ANCH_RESP:
1275
      case RTLS_DEMO_MSG_ANCH_RESP2:{
1276

    
1277
        //we are a tag
1278
        if(instance_data[instance].mode == TAG){
1279
          uint8_t index ;
1280
          instance_data[instance].rxResps[instance_data[instance].rangeNum]++;
1281
          dw_event.type_pend = tagrxreenable(sourceAddress); //responseTO decremented above...
1282
          index = RRXT0 + 5*(sourceAddress & 0x3);
1283

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

    
1288
        }
1289
        else if (instance_data[instance].mode == ANCHOR_RNG) {  //A0 and A1 only when ranging to other anchors
1290
          uint8_t index ;
1291
          instance_data[instance].rxResps[instance_data[instance].rangeNumAnc]++;
1292
          dw_event.type_pend = ancsendfinalorrxreenable(sourceAddress);
1293
          index = RRXT0 + 5*(sourceAddress & 0x3);
1294

    
1295
          instance_data[instance].rxResponseMaskAnc |= (0x1 << (sourceAddress & 0x3)); //add anchor ID to the mask
1296
          // Write Response RX time field of Final message
1297
          memcpy(&(instance_data[instance].msg_f.messageData[index]), rxTimeStamp, 5);
1298
        }
1299
        else { //normal anchor mode
1300
          //got a response... (check if we got a Poll with the same range number as in this response)
1301
          if(RTLS_DEMO_MSG_ANCH_RESP == dw_event.msgu.frame[fcode_index]){
1302
            if((instance_data[instance].rxResps[instance_data[instance].rxRespsIdx] >= 0) //we got the poll else ignore this response
1303
                && (instance_data[instance].responseTO > 0)        ) {  //if responseTO == 0 we have already received all of the responses - meaning should not be here => error
1304
              instance_data[instance].rxResps[instance_data[instance].rxRespsIdx]++; //increment the number of responses received
1305
              instance_data[instance].responseTO--;
1306

    
1307
              //send a response or re-enable rx
1308
              dw_event.type_pend = anctxorrxreenable(sourceAddress, 4+0);
1309
            }
1310
            else {  //like a timeout (error) ...
1311

    
1312
              //send a response or re-enable rx
1313
              dwt_setrxtimeout(0); //reconfigure the timeout
1314
              dwt_rxenable(DWT_START_RX_IMMEDIATE) ;
1315
              dw_event.type_pend = DWT_SIG_RX_PENDING ;
1316
            }
1317
          }
1318
          else {  //in anchor mode and got RTLS_DEMO_MSG_ANCH_RESP2
1319
            if((instance_data[instance].gatewayAnchor) &&
1320
                (instance_data[instance].rxResps[instance_data[instance].rangeNumAnc]) == 2){ //got two responses A1 and A2 this is third (A2's to A1)
1321
              instance_data[instance].rxResps[instance_data[instance].rangeNumAnc]++;
1322
              instance_data[instance].rxResponseMaskAnc |= 0x8 ;
1323

    
1324
              dw_event.type_pend = anctxorrxreenable(sourceAddress, 4+1); //re-enable the RX
1325
            }
1326
            //A2 got A1's response to A0 - send A2 response (but only if we got the Poll from A0)
1327
            else if((instance_data[instance].instanceAddress16 == A2_ANCHOR_ADDR) &&
1328
                (instance_data[instance].rxResps[instance_data[instance].rxRespsIdx] >= 0) ){
1329
              instance_data[instance].rxResps[instance_data[instance].rxRespsIdx]++;
1330
              instance_data[instance].responseTO--;
1331

    
1332
              dwt_setrxtimeout(0);
1333
              dwt_setrxaftertxdelay(0); //clear rx on delay as Final will come sooner than if we were waiting for next Response
1334
              dw_event.type_pend = anctxorrxreenable(sourceAddress, 1);
1335
            }
1336
            else {  // if other anchor A1, A2, A3 .. ignore these responses when in ANCHOR mode
1337
              dwt_setrxtimeout(0); //reconfigure the timeout
1338
              dwt_rxenable(DWT_START_RX_IMMEDIATE) ;
1339
              dw_event.type_pend = DWT_SIG_RX_PENDING ;
1340
            }
1341
          }
1342
        }
1343

    
1344
      }
1345
      break;
1346

    
1347
      case RTLS_DEMO_MSG_TAG_FINAL:
1348
      case RTLS_DEMO_MSG_ANCH_FINAL:
1349
        if(instance_data[instance].mode == TAG) {  //tag should ignore any other Final from anchors
1350
          instance_data[instance].responseTO++; //as will be decremented in the function and was also decremented above
1351
          handle_error_unknownframe(dw_event);
1352
          instance_data[instance].stopTimer = 1;
1353
          instance_data[instance].rxMsgCount++;
1354
          return;
1355
        }
1356
      break;  // TODO: fallthourgh for anchor
1357
//        __attribute__ ((fallthrough));
1358

    
1359
        //if anchor fall into case below and process the frame
1360
      default: {  //process rx frame
1361
        dw_event.type_pend = DWT_SIG_DW_IDLE;
1362
      }
1363
      break;
1364

    
1365
      }
1366
    }//end of if not Listener mode
1367
    instance_data[instance].stopTimer = 1;
1368

    
1369
    instance_putevent(dw_event, rxd_event);
1370

    
1371
    instance_data[instance].rxMsgCount++;
1372
  }
1373
  else { //if (rxd_event == SIG_RX_UNKNOWN) //need to re-enable the rx (got unknown frame type)
1374
    handle_error_unknownframe(dw_event);
1375
  }
1376
  //  }
1377
}
1378

    
1379

    
1380
/*TODO: Callback funtion for RX timeout (available from newer driver version) */
1381
#pragma GCC optimize ("O3")
1382
void instance_rxtimeoutcallback(const dwt_cb_data_t *rxd){
1383

    
1384
    (void) rxd;
1385
    event_data_t dw_event;
1386

    
1387
    int instance = 0;
1388

    
1389
    dw_event.type_pend = DWT_SIG_DW_IDLE;
1390

    
1391
    if(instance_data[instance].mode == ANCHOR) {
1392
        //check if anchor has received all of the responses from other anchors (it could have received only 1 or 2)
1393
        //it's timed out (re-enable rx or tx response)
1394
        if(instance_data[instance].responseTO > 0) {
1395
            instance_data[instance].responseTO--;
1396
            //send a response or re-enable rx
1397
            dw_event.type_pend = anctxorrxreenable(instance_data[instance].instanceAddress16, 6+0);
1398
//          Print_On_Uart("Re-enable RX in rxtimeout callback\r\n");
1399
        }
1400
    }
1401
    dw_event.type = 0;
1402
    dw_event.type_save = DWT_SIG_RX_TIMEOUT;
1403
    dw_event.rxLength = 0;
1404
    dw_event.timeStamp = 0;
1405
    dw_event.timeStamp32l = 0;
1406
    dw_event.timeStamp32h = 0;
1407

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

    
1411
}
1412

    
1413

    
1414
/*TODO: Callback funtion for RX error (available from newer driver version) */
1415
#pragma GCC optimize ("O3")
1416
void instance_rxerrorcallback(const dwt_cb_data_t *rxd) {
1417

    
1418
    (void) rxd;
1419
    event_data_t dw_event;
1420

    
1421
    dw_event.uTimeStamp = portGetTickCnt();
1422

    
1423
    handle_error_unknownframe(dw_event);
1424
}
1425

    
1426

    
1427

    
1428
#pragma GCC optimize ("O3")
1429
int instance_peekevent(void){
1430
  int instance = 0;
1431
  return instance_data[instance].dwevent[instance_data[instance].dweventPeek].type; //return the type of event that is in front of the queue
1432
}
1433

    
1434
#pragma GCC optimize ("O3")
1435
void instance_saveevent(event_data_t newevent, uint8_t etype){
1436
  int instance = 0;
1437

    
1438
  instance_data[instance].saved_dwevent = newevent;
1439
  instance_data[instance].saved_dwevent.type = etype;
1440
}
1441

    
1442
#pragma GCC optimize ("O3")
1443
event_data_t instance_getsavedevent(void){
1444
  int instance = 0;
1445

    
1446
  return instance_data[instance].saved_dwevent;
1447
}
1448

    
1449
#pragma GCC optimize ("O3")
1450
void instance_putevent(event_data_t newevent, uint8_t etype){
1451
  int instance = 0;
1452
  //newevent.eventtime = portGetTickCnt();
1453
  newevent.gotit = 0 ; //newevent.eventtimeclr = 0;
1454

    
1455
  //copy event
1456
  instance_data[instance].dwevent[instance_data[instance].dweventIdxIn] = newevent;
1457

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

    
1462
  instance_data[instance].dweventIdxIn++;
1463

    
1464
  if(MAX_EVENT_NUMBER == instance_data[instance].dweventIdxIn)
1465
  {
1466
    instance_data[instance].dweventIdxIn = 0;
1467
  }
1468
  //eventIncount++;
1469

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

    
1473
event_data_t dw_event_g;
1474

    
1475
#pragma GCC optimize ("O3")
1476
event_data_t* instance_getevent(int x){
1477
  int instance = 0;
1478
  int indexOut = instance_data[instance].dweventIdxOut;
1479

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

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

    
1484
  if(instance_data[instance].dwevent[indexOut].type == 0) {  //exit with "no event"
1485
    dw_event_g.type = 0;
1486
    dw_event_g.type_save = 0;
1487
    return &dw_event_g;
1488
  }
1489

    
1490
  //copy the event
1491
  dw_event_g.type_save = instance_data[instance].dwevent[indexOut].type_save ;
1492
  dw_event_g.type_pend = instance_data[instance].dwevent[indexOut].type_pend ;
1493
  dw_event_g.rxLength = instance_data[instance].dwevent[indexOut].rxLength ;
1494
  dw_event_g.timeStamp = instance_data[instance].dwevent[indexOut].timeStamp ;
1495
  dw_event_g.timeStamp32l = instance_data[instance].dwevent[indexOut].timeStamp32l ;
1496
  dw_event_g.timeStamp32h = instance_data[instance].dwevent[indexOut].timeStamp32h ;
1497
  dw_event_g.uTimeStamp = instance_data[instance].dwevent[indexOut].uTimeStamp ;
1498
  //dw_event_g.eventtime = instance_data[instance].dwevent[indexOut].eventtime ;
1499
  //dw_event_g.eventtimeclr = instance_data[instance].dwevent[indexOut].eventtimeclr ;
1500
  //dw_event_g.gotit = instance_data[instance].dwevent[indexOut].gotit ;
1501

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

    
1504
  dw_event_g.type = instance_data[instance].dwevent[indexOut].type ;
1505

    
1506

    
1507
  instance_data[instance].dwevent[indexOut].gotit = x;
1508

    
1509
  //instance_data[instance].dwevent[indexOut].eventtimeclr = portGetTickCnt();
1510

    
1511
  instance_data[instance].dwevent[indexOut].type = 0; //clear the event
1512

    
1513
  instance_data[instance].dweventIdxOut++;
1514
  if(MAX_EVENT_NUMBER == instance_data[instance].dweventIdxOut){   //wrap the counter
1515
    instance_data[instance].dweventIdxOut = 0;
1516
  }
1517
  instance_data[instance].dweventPeek = instance_data[instance].dweventIdxOut; //set the new peek value
1518

    
1519
  //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);
1520

    
1521
  //eventOutcount++;
1522

    
1523

    
1524
  return &dw_event_g;
1525
}
1526

    
1527
void instance_clearevents(void){
1528
  int i = 0;
1529
  int instance = 0;
1530

    
1531
  for(i=0; i<MAX_EVENT_NUMBER; i++) {
1532
    memset(&instance_data[instance].dwevent[i], 0, sizeof(event_data_t));
1533
  }
1534

    
1535
  instance_data[instance].dweventIdxIn = 0;
1536
  instance_data[instance].dweventIdxOut = 0;
1537
  instance_data[instance].dweventPeek = 0;
1538

    
1539
  //eventOutcount = 0;
1540
  //eventIncount = 0;
1541
}
1542

    
1543
// -------------------------------------------------------------------------------------------------------------------
1544
#pragma GCC optimize ("O3")
1545
int instance_run(void){
1546
  int instance = 0 ;
1547
  int done = INST_NOT_DONE_YET;
1548
  int message = instance_peekevent(); //get any of the received events from ISR
1549

    
1550

    
1551
  while(done == INST_NOT_DONE_YET){
1552
    //int state = instance_data[instance].testAppState;
1553
    done = testapprun(&instance_data[instance], message) ;                                               // run the communications application
1554

    
1555
    //we've processed message
1556
    message = 0;
1557
  }
1558

    
1559

    
1560

    
1561
  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)
1562
    if(instance_data[instance].mode == TAG) { //Tag (is either in RX or sleeping)
1563
      int32_t nextPeriod ;
1564

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

    
1568
      instance_data[instance].nextSleepPeriod = (uint32_t) nextPeriod ; //set timeout time, CAST the positive period to UINT for correct wrapping.
1569
      instance_data[instance].tagSleepCorrection2 = instance_data[instance].tagSleepCorrection;
1570
      instance_data[instance].tagSleepCorrection = 0; //clear the correction
1571
      instance_data[instance].instanceTimerEn = 1; //start timer
1572
    }
1573
    instance_data[instance].stopTimer = 0 ; //clear the flag - timer can run if instancetimer_en set (set above)
1574
    instance_data[instance].done = INST_NOT_DONE_YET;
1575
  }
1576

    
1577
  //check if timer has expired
1578
  if((instance_data[instance].instanceTimerEn == 1) && (instance_data[instance].stopTimer == 0))  {
1579
    if(instance_data[instance].mode == TAG)    {
1580
      if((portGetTickCnt() - instance_data[instance].instanceWakeTime) > instance_data[instance].nextSleepPeriod)      {
1581
        event_data_t dw_event;
1582
        instance_data[instance].instanceTimerEn = 0;
1583
        dw_event.rxLength = 0;
1584
        dw_event.type = 0;
1585
        dw_event.type_save = 0x80 | DWT_SIG_RX_TIMEOUT;
1586
        //printf("PC timeout DWT_SIG_RX_TIMEOUT\n");
1587
        instance_putevent(dw_event, DWT_SIG_RX_TIMEOUT);
1588
      }
1589
    }
1590
#if (ANCTOANCTWR == 1) //allow anchor to anchor ranging
1591
    else if(instance_data[instance].mode == ANCHOR)    {
1592
      uint32_t slotTime = portGetTickCnt() % instance_data[instance].sframePeriod;
1593

    
1594
      if(instance_data[instance].gatewayAnchor) {
1595
        //if we are in the last slot - then A0 ranges to A1 and A2
1596
        if( slotTime >= instance_data[instance].a0SlotTime)        {
1597
          port_DisableEXT_IRQ(); //enable ScenSor IRQ before starting
1598
          //anchor0 sends poll to anchor1
1599
          instance_data[instance].mode = ANCHOR_RNG; //change to ranging initiator
1600
          dwt_forcetrxoff(); //disable DW1000
1601
          instance_clearevents(); //clear any events
1602
          //change state to send a Poll
1603
          instance_data[instance].testAppState = TA_TXPOLL_WAIT_SEND ;
1604
          instance_data[instance].msg_f.destAddr[0] = 0x1 ;
1605
          instance_data[instance].msg_f.destAddr[1] = (GATEWAY_ANCHOR_ADDR >> 8);
1606
          instance_data[instance].instanceTimerEn = 0;
1607
          instance_data[instance].rangeNumAnc++;
1608
          port_EnableEXT_IRQ(); //enable ScenSor IRQ before starting
1609
        }
1610
      }
1611
      else if (instance_data[instance].instanceAddress16 == A1_ANCHOR_ADDR) { //A1 ranges to A2 in the 2nd half of last slot
1612
        if(portGetTickCnt() >= instance_data[instance].a1SlotTime) {
1613
          port_DisableEXT_IRQ(); //enable ScenSor IRQ before starting
1614
          //anchor1 sends poll to anchor2
1615
          instance_data[instance].mode = ANCHOR_RNG; //change to ranging initiator
1616
          dwt_forcetrxoff(); //disable DW1000
1617
          instance_clearevents(); //clear any events
1618
          //change state to send a Poll
1619
          instance_data[instance].testAppState = TA_TXPOLL_WAIT_SEND ;
1620
          instance_data[instance].msg_f.destAddr[0] = 0x2 ;
1621
          instance_data[instance].msg_f.destAddr[1] = (GATEWAY_ANCHOR_ADDR >> 8);
1622

    
1623
          instance_data[instance].instanceTimerEn = 0;
1624
          //instance_data[instance].a1SlotTime = 0;
1625
          port_EnableEXT_IRQ(); //enable ScenSor IRQ before starting
1626
        }
1627
      }
1628
    }
1629
#endif
1630
  }
1631

    
1632
#if (ANCTOANCTWR == 1) //allow anchor to anchor ranging
1633
  else if (instance_data[instance].instanceTimerEn == 0){
1634
    if((instance_data[instance].mode == ANCHOR) && (instance_data[instance].gatewayAnchor)){
1635
      uint32_t slotTime = portGetTickCnt() % instance_data[instance].sframePeriod;
1636
      //enable the timer in 1st slot
1637
      if(slotTime < instance_data[instance].slotPeriod){
1638
        instance_data[instance].instanceTimerEn = 1;
1639
      }
1640
    }
1641
  }
1642
#endif
1643
  return 0 ;
1644
}
1645

    
1646

    
1647
void instance_close(void){
1648
  //wake up device from low power mode
1649
  //NOTE - in the ARM  code just drop chip select for 200us
1650
  port_SPIx_clear_chip_select();  //CS low
1651
  Sleep(1);   //200 us to wake up then waits 5ms for DW1000 XTAL to stabilise
1652
  port_SPIx_set_chip_select();  //CS high
1653
  Sleep(5);
1654
  dwt_entersleepaftertx(0); // clear the "enter deep sleep after tx" bit
1655
  dwt_setinterrupt(0xFFFFFFFF, 0); //don't allow any interrupts
1656

    
1657
}
1658

    
1659

    
1660
void instance_notify_DW1000_inIDLE(int idle){
1661
  instance_data[0].dwIDLE = idle;
1662
}
1663

    
1664
void instanceconfigtxpower(uint32_t txpower){
1665
  instance_data[0].txPower = txpower ;
1666

    
1667
  instance_data[0].txPowerChanged = 1;
1668

    
1669
}
1670

    
1671
void instancesettxpower(void){
1672
  if(instance_data[0].txPowerChanged == 1){
1673
    //Configure TX power
1674
    dwt_write32bitreg(0x1E, instance_data[0].txPower);
1675

    
1676
    instance_data[0].txPowerChanged = 0;
1677
  }
1678
}
1679

    
1680
void instanceconfigantennadelays(uint16_t tx, uint16_t rx){
1681
  instance_data[0].txAntennaDelay = tx ;
1682
  instance_data[0].rxAntennaDelay = rx ;
1683

    
1684
  instance_data[0].antennaDelayChanged = 1;
1685
}
1686

    
1687
void instancesetantennadelays(void){
1688
  if(instance_data[0].antennaDelayChanged == 1){
1689
    dwt_setrxantennadelay(instance_data[0].rxAntennaDelay);
1690
    dwt_settxantennadelay(instance_data[0].txAntennaDelay);
1691

    
1692
    instance_data[0].antennaDelayChanged = 0;
1693
  }
1694
}
1695

    
1696

    
1697
uint16_t instancetxantdly(void){
1698
  return instance_data[0].txAntennaDelay;
1699
}
1700

    
1701
uint16_t instancerxantdly(void){
1702
  return instance_data[0].rxAntennaDelay;
1703
}
1704

    
1705
uint8_t instancevalidranges(void){
1706
  uint8_t x = instance_data[0].rxResponseMaskReport;
1707
  instance_data[0].rxResponseMaskReport = 0; //reset mask as we have printed out the ToFs
1708
  return x;
1709
}
1710
#endif
1711

    
1712

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

    
1715
/*! ------------------------------------------------------------------------------------------------------------------
1716
 * @fn dwt_getotptxpower()
1717
 *
1718
 * @brief This API function returns the tx power value read from OTP memory as part of initialisation
1719
 *
1720
 * input parameters
1721
 * @param prf   -   this is the PRF e.g. DWT_PRF_16M or DWT_PRF_64M
1722
 * @param chan  -   this is the channel e.g. 1 to 7
1723
 *
1724
 * output parameters
1725
 *
1726
 * returns tx power value for a given PRF and channel
1727
 */
1728
uint32_t dwt_getotptxpower(uint8_t prf, uint8_t chan){
1729
    return platformLocalData.txPowCfg[(prf - DWT_PRF_16M) + (chan_idx[chan] * 2)];
1730
}
1731

    
1732
/*! ------------------------------------------------------------------------------------------------------------------
1733
 * @fn dwt_getTREKOTPantennadelay()
1734
 *
1735
 * @brief This API function returns the antenna delay read from the OTP memory as part of device initialisation
1736
 * Note: the antenna delay will only be read if dwt_initialise is called with DWT_LOADANTDLY bit set in the config parameter
1737
 * The values of antenna delay are only valid for TREK use case modes.
1738
 *
1739
 * input parameters:
1740
 * @param anchor   - this is the mode (Tag or Anchor) if Tag set to 0, if Anchor set to 1
1741
 * @param chan   - this is the channel (1, 2, 3, 4, 5, 7)
1742
 * @param datarate   - this is the datarate DWT_BR_6M8, DWT_BR_110K or DWT_BR_850K
1743
 *
1744
 */
1745
uint16_t dwt_getTREKOTPantennadelay(uint8_t anchor, uint8_t chan, uint8_t datarate){
1746
    uint32_t dly = 0;
1747

    
1748
    // 32-bit antenna delay value previously read from OTP, high 16 bits is value for Anchor mode, low 16-bits for Tag mode
1749
    switch(chan){
1750
        case 2:
1751
            if(datarate == DWT_BR_6M8)
1752
                dly = platformLocalData.antennaCals[0];
1753
            else if(datarate == DWT_BR_110K)
1754
                dly = platformLocalData.antennaCals[1];
1755
            break;
1756
        case 5:
1757
            if(datarate == DWT_BR_6M8)
1758
                dly = platformLocalData.antennaCals[2];
1759
            else if(datarate == DWT_BR_110K)
1760
                dly = platformLocalData.antennaCals[3];
1761
            break;
1762
        default:
1763
            dly = 0;
1764
            break;
1765
    }
1766

    
1767
    return (dly >> (16*(anchor & 0x1))) & 0xFFFF;
1768
}
1769

    
1770
/*! ------------------------------------------------------------------------------------------------------------------
1771
 * @fn dwt_readantennadelay()
1772
 *
1773
 * @brief This API function returns the antenna delay read from the OTP memory as part of device initialisation
1774
 * Note: the antenna delay will only be read if dwt_initialise is called with DWT_LOADANTDLY bit set in the config parameter
1775
 *
1776
 * input parameters:
1777
 * @param prf   -   this is the PRF e.g. DWT_PRF_16M or DWT_PRF_64M
1778
 *
1779
 */
1780
uint16_t dwt_readantennadelay(uint8_t prf){
1781
    // 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
1782
    return (platformLocalData.antennaDly >> (16*(prf-DWT_PRF_16M))) & 0xFFFF;
1783
}
1784

    
1785

    
1786
/* ==========================================================
1787

1788
Notes:
1789

1790
Previously code handled multiple instances in a single console application
1791

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

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

1796
*/
1797

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