Statistics
| Branch: | Tag: | Revision:

amiro-lld / source / DW1000 / v0 / deca_instance_common.c @ 1d5bcc82

History | View | Annotate | Download (34.1 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 == 0)) || defined(__DOXYGEN__)
17

    
18
#include <string.h>
19
#include <math.h>
20

    
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
static 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
// -------------------------------------------------------------------------------------------------------------------
164
// Functions
165
// -------------------------------------------------------------------------------------------------------------------
166

    
167

    
168
/* @fn           instance_get_local_structure_ptr
169
 * @brief function to return the pointer to local instance data structure
170
 * */
171
instance_data_t* instance_get_local_structure_ptr(unsigned int x)
172
{
173
        if (x >= NUM_INST)
174
        {
175
                return NULL;
176
        }
177

    
178
        return &instance_data[x];
179
}
180

    
181

    
182
// -------------------------------------------------------------------------------------------------------------------
183
/* @fn           instance_convert_usec_to_devtimeu
184
 * @brief function to convert microseconds to device time
185
 * */
186
uint64_t instance_convert_usec_to_devtimeu (double microsecu)
187
{
188
    uint64_t dt;
189
    long double dtime;
190

    
191
    dtime = (microsecu / (double) DWT_TIME_UNITS) / 1e6 ;
192

    
193
    dt =  (uint64_t) (dtime) ;
194

    
195
    return dt;
196
}
197

    
198
/* @fn           instance_calculate_rangefromTOF
199
 * @brief function to calculate and the range from given Time of Flight
200
 * */
201
int instance_calculate_rangefromTOF(int idx, uint32_t tofx)
202
{
203
                instance_data_t* inst = instance_get_local_structure_ptr(0);
204
        double distance ;
205
        double distance_to_correct;
206
        double tof ;
207
        int32_t tofi ;
208

    
209
        // check for negative results and accept them making them proper negative integers
210
        tofi = (int32_t) tofx ; // make it signed
211
        if (tofi > 0x7FFFFFFF)  // close up TOF may be negative
212
        {
213
            tofi -= 0x80000000 ;  //
214
        }
215

    
216
        // convert device time units to seconds (as floating point)
217
        tof = tofi * DWT_TIME_UNITS ;
218
        inst_idistraw[idx] = distance = tof * SPEED_OF_LIGHT;
219

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

    
241
        distance = distance - dwt_getrangebias(inst->configData.chan, (float) distance_to_correct, inst->configData.prf);
242
#endif
243

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

    
247
        inst_idist[idx] = distance;
248

    
249
        inst->longTermRangeCount++ ;                          // for computing a long term average
250

    
251
    return 1;
252
}// end of calculateRangeFromTOF
253

    
254
void instance_set_tagdist(int tidx, int aidx)
255
{
256
        inst_tdist[tidx] = inst_idist[aidx];
257
}
258

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

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

    
270
void instance_cleardisttableall(void)
271
{
272
        int i;
273

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

    
281
// -------------------------------------------------------------------------------------------------------------------
282
// Set this instance role as the Tag, Anchor
283
/*void instance_set_role(int inst_mode)
284
{
285
    // assume instance 0, for this
286
    inst->mode =  inst_mode;                   // set the role
287
}
288
*/
289
int instance_get_role(void)
290
{
291
        instance_data_t* inst = instance_get_local_structure_ptr(0);
292

    
293
    return inst->mode;
294
}
295

    
296
int instance_newrange(void)
297
{
298
        instance_data_t* inst = instance_get_local_structure_ptr(0);
299
        int x = inst->newRange;
300
    inst->newRange = TOF_REPORT_NUL;
301
    return x;
302
}
303

    
304
int instance_newrangeancadd(void)
305
{
306
        instance_data_t* inst = instance_get_local_structure_ptr(0);
307
    return inst->newRangeAncAddress;
308
}
309

    
310
int instance_newrangetagadd(void)
311
{
312
        instance_data_t* inst = instance_get_local_structure_ptr(0);
313
    return inst->newRangeTagAddress;
314
}
315

    
316
int instance_newrangetim(void)
317
{
318
        instance_data_t* inst = instance_get_local_structure_ptr(0);
319
    return inst->newRangeTime;
320
}
321

    
322
// -------------------------------------------------------------------------------------------------------------------
323
// function to clear counts/averages/range values
324
//
325
void instance_clearcounts(void)
326
{
327
        instance_data_t* inst = instance_get_local_structure_ptr(0);
328
    int i= 0 ;
329

    
330
    //inst->rxTimeouts = 0 ;
331
    //inst->txMsgCount = 0 ;
332
    //inst->rxMsgCount = 0 ;
333

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

    
336
    inst->frameSN = 0;
337

    
338
    inst->longTermRangeCount  = 0;
339

    
340

    
341
    for(i=0; i<MAX_ANCHOR_LIST_SIZE; i++)
342
        {
343
            inst->tofArray[i] = INVALID_TOF;
344
        }
345

    
346
    for(i=0; i<MAX_TAG_LIST_SIZE; i++)
347
        {
348
                inst->tof[i] = INVALID_TOF;
349
        }
350

    
351
} // end instanceclearcounts()
352

    
353

    
354
// -------------------------------------------------------------------------------------------------------------------
355
// function to initialise instance structures
356
//
357
// Returns 0 on success and -1 on error
358
int instance_init(int inst_mode, DW1000Driver* drv)
359
{
360
        instance_data_t* inst = instance_get_local_structure_ptr(0);
361
    int result;
362

    
363
    inst->mode =  inst_mode;                                // assume listener,
364
    inst->twrMode = LISTENER;
365
    inst->testAppState = TA_INIT ;
366
    inst->instToSleep = FALSE;
367

    
368

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

    
373
        //this initialises DW1000 and uses specified configurations from OTP/ROM
374
    result = dwt_initialise(DWT_LOADUCODE, drv) ;
375

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

    
379
    if (DWT_SUCCESS != result)
380
    {
381
        return (-1) ;   // device initialise has failed
382
    }
383

    
384
    instance_clearcounts() ;
385

    
386
    inst->wait4ack = 0;
387
    inst->instanceTimerEn = 0;
388

    
389
    instance_clearevents();
390

    
391
#if (DISCOVERY == 1)
392
    dwt_geteui(inst->eui64);
393
    inst->panID = 0xdada ;
394
#else
395
    memset(inst->eui64, 0, ADDR_BYTE_SIZE_L);
396
    inst->panID = 0xdeca ;
397
#endif
398
    inst->tagSleepCorrection_ms = 0;
399

    
400
    dwt_setdblrxbuffmode(0); //disable double RX buffer
401

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

    
407
    if(inst_mode == ANCHOR)
408
    {
409
            dwt_setcallbacks(tx_conf_cb, rx_ok_cb_anch, rx_to_cb_anch, rx_err_cb_anch);
410
    }
411
    else
412
    {
413
            dwt_setcallbacks(tx_conf_cb, rx_ok_cb_tag, rx_to_cb_tag, rx_err_cb_tag);
414
    }
415

    
416
    inst->monitor = 0;
417

    
418
    //inst->lateTX = 0;
419
    //inst->lateRX = 0;
420

    
421
    inst->remainingRespToRx = -1; //initialise
422

    
423
    inst->rxResps = 0;
424

    
425
    dwt_setlnapamode(1, 1); //enable TX, RX state on GPIOs 6 and 5
426

    
427
    inst->delayedTRXTime32h = 0;
428

    
429
#if (READ_EVENT_COUNTERS == 1)
430
    dwt_configeventcounters(1);
431
#endif
432
    return 0 ;
433
}
434

    
435
// -------------------------------------------------------------------------------------------------------------------
436
//
437
// Return the Device ID register value, enables higher level validation of physical device presence
438
//
439

    
440
uint32_t instance_readdeviceid(void)
441
{
442
    return dwt_readdevid() ;
443
}
444

    
445

    
446
//OTP memory addresses for TREK calibration data
447
#define TXCFG_ADDRESS  (0x10)
448
#define ANTDLY_ADDRESS (0x1C)
449
#define TREK_ANTDLY_1  (0xD)
450
#define TREK_ANTDLY_2  (0xE)
451
#define TREK_ANTDLY_3  (0xF)
452
#define TREK_ANTDLY_4  (0x1D)
453

    
454
extern uint8_t chan_idx[];
455
// -------------------------------------------------------------------------------------------------------------------
456
//
457
// function to allow application configuration be passed into instance and affect underlying device operation
458
//
459
void instance_config(instanceConfig_t *config, sfConfig_t *sfConfig)
460
{
461
        instance_data_t* inst = instance_get_local_structure_ptr(0);
462
    uint32_t power = 0;
463
    uint8_t otprev ;
464

    
465
    inst->configData.chan = config->channelNumber ;
466
    inst->configData.rxCode =  config->preambleCode ;
467
    inst->configData.txCode = config->preambleCode ;
468
    inst->configData.prf = config->pulseRepFreq ;
469
    inst->configData.dataRate = config->dataRate ;
470
    inst->configData.txPreambLength = config->preambleLen ;
471
    inst->configData.rxPAC = config->pacSize ;
472
    inst->configData.nsSFD = config->nsSFD ;
473
    inst->configData.phrMode = DWT_PHRMODE_STD ;
474
    inst->configData.sfdTO = config->sfdTO;
475

    
476
    //the DW1000 will automatically use gating gain for frames < 1ms duration (i.e. 6.81Mbps data rate)
477
    //smartPowerEn should be set based on the frame length, but we can also use dtaa rate.
478
    if(inst->configData.dataRate == DWT_BR_6M8)
479
    {
480
        inst->smartPowerEn = 1;
481
    }
482
    else
483
    {
484
        inst->smartPowerEn = 0;
485
    }
486

    
487
    //configure the channel parameters
488
    dwt_configure(&inst->configData) ;
489

    
490
    port_set_dw1000_slowrate(); //reduce SPI to < 3MHz
491
    //load TX values from OTP
492
        dwt_otpread(TXCFG_ADDRESS+(config->pulseRepFreq - DWT_PRF_16M) + (chan_idx[inst->configData.chan] * 2), &power, 1);
493
    port_set_dw1000_fastrate(); //increase SPI
494
        //check if there are calibrated TX power value in the DW1000 OTP
495
    if((power == 0x0) || (power == 0xFFFFFFFF)) //if there are no calibrated values... need to use defaults
496
    {
497
        power = txSpectrumConfig[config->channelNumber].txPwr[config->pulseRepFreq- DWT_PRF_16M];
498
    }
499

    
500
    //Configure TX power and PG delay
501
    inst->configTX.power = power;
502
    inst->configTX.PGdly = txSpectrumConfig[config->channelNumber].pgDelay ;
503
    //configure the tx spectrum parameters (power and PG delay)
504
    dwt_configuretxrf(&inst->configTX);
505

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

    
508
        if ((2 == otprev) || (3 == otprev))  // board is calibrated with TREK1000 with antenna delays set for each use case)
509
        {
510
        uint8_t mode = (inst->mode == ANCHOR ? 1 : 0);
511
        uint8_t chanindex = 0;
512
        uint32_t dly = 0;
513

    
514
        port_set_dw1000_slowrate(); //reduce SPI to < 3MHz
515

    
516
                //read 32-bit antenna delay value from OTP, high 16 bits is value for Anchor mode, low 16-bits for Tag mode
517
                switch(inst->configData.chan)
518
                {
519
                        case 2:
520
                                if(inst->configData.dataRate == DWT_BR_6M8)
521
                                        dwt_otpread(TREK_ANTDLY_1, &dly, 1);
522
                                else if(inst->configData.dataRate == DWT_BR_110K)
523
                                        dwt_otpread(TREK_ANTDLY_2, &dly, 1);
524
                                break;
525
                        case 5:
526
                                if(inst->configData.dataRate == DWT_BR_6M8)
527
                                        dwt_otpread(TREK_ANTDLY_3, &dly, 1);
528
                                else if(inst->configData.dataRate == DWT_BR_110K)
529
                                        dwt_otpread(TREK_ANTDLY_4, &dly, 1);
530
                                break;
531
                        default:
532
                                dly = 0;
533
                                break;
534
                }
535

    
536
        port_set_dw1000_fastrate(); //increase SPI to max
537

    
538
                // if nothing was actually programmed then set a reasonable value anyway
539
                if ((dly == 0)
540
                                || (dly == 0xffffffff))
541
                {
542
                        if(inst->configData.chan == 5)
543
                        {
544
                                chanindex = 1;
545
                        }
546

    
547
                        inst->txAntennaDelay = rfDelaysTREK[chanindex];
548
                }
549
                else
550
                {
551
                        inst->txAntennaDelay = (dly >> (16*(mode & 0x1))) & 0xFFFF;
552
                }
553

    
554
        }
555
        else // assume it is older EVK1000 programming.
556
        {
557
        uint32_t antennaDly;
558
        port_set_dw1000_slowrate(); //reduce SPI to < 3MHz
559
                //read the antenna delay that was programmed in the OTP calibration area
560
            dwt_otpread(ANTDLY_ADDRESS, &antennaDly, 1) ;
561
        port_set_dw1000_fastrate(); //increase SPI to max
562

    
563
                // if nothing was actually programmed then set a reasonable value anyway
564
                if ((antennaDly == 0)
565
                                || (antennaDly == 0xffffffff))
566
                {
567
                        inst->txAntennaDelay = rfDelays[config->pulseRepFreq - DWT_PRF_16M];
568
                }
569
                else
570
                {
571
                        // 32-bit antenna delay value read from OTP, high 16 bits is value for 64 MHz PRF, low 16-bits for 16 MHz PRF
572
                        inst->txAntennaDelay = ((antennaDly >> (16*(inst->configData.prf-DWT_PRF_16M))) & 0xFFFF) >> 1;
573
                }
574
        }
575

    
576
        // -------------------------------------------------------------------------------------------------------------------
577
        // set the antenna delay, we assume that the RX is the same as TX.
578
        dwt_setrxantennadelay(inst->txAntennaDelay);
579
        dwt_settxantennadelay(inst->txAntennaDelay);
580

    
581
    inst->rxAntennaDelay = inst->txAntennaDelay;
582

    
583
    if(config->preambleLen == DWT_PLEN_64) //if preamble length is 64
584
        {
585
            port_set_dw1000_slowrate(); //reduce SPI to < 3MHz
586

    
587
                dwt_loadopsettabfromotp(0);
588

    
589
                port_set_dw1000_fastrate(); //increase SPI to max
590
    }
591

    
592

    
593
    inst->tagPeriod_ms = sfConfig->tagPeriod_ms; //set the Tag sleep time
594
        inst->sframePeriod_ms = sfConfig->sfPeriod_ms;
595
        inst->slotDuration_ms = sfConfig->slotDuration_ms;
596
        inst->tagSleepRnd_ms = sfConfig->slotDuration_ms;
597
        inst->numSlots = sfConfig->numSlots;
598

    
599
        //last two slots are used for anchor to anchor ranging
600
        inst->a0SlotTime_ms = (inst->numSlots-2) * inst->slotDuration_ms;
601

    
602
        //set the default response delays
603
        instance_set_replydelay(sfConfig->pollTxToFinalTxDly_us);
604

    
605
}
606

    
607
int instance_get_rnum(void) //get ranging number
608
{
609
        instance_data_t* inst = instance_get_local_structure_ptr(0);
610
        return inst->rangeNum;
611
}
612

    
613
int instance_get_rnuma(int idx) //get ranging number
614
{
615
        instance_data_t* inst = instance_get_local_structure_ptr(0);
616
        return inst->rangeNumA[idx];
617
}
618

    
619
int instance_get_rnumanc(int idx) //get ranging number
620
{
621
        instance_data_t* inst = instance_get_local_structure_ptr(0);
622
        return inst->rangeNumAAnc[idx];
623
}
624

    
625
int instance_get_lcount(void) //get count of ranges used for calculation of lt avg
626
{
627
        instance_data_t* inst = instance_get_local_structure_ptr(0);
628
    int x = inst->longTermRangeCount;
629

    
630
    return (x);
631
}
632

    
633
double instance_get_idist(int idx) //get instantaneous range
634
{
635
    double x ;
636

    
637
    idx &= (MAX_ANCHOR_LIST_SIZE - 1);
638

    
639
    x = inst_idist[idx];
640

    
641
    return (x);
642
}
643

    
644
double instance_get_idistraw(int idx) //get instantaneous range (uncorrected)
645
{
646
    double x ;
647

    
648
    idx &= (MAX_ANCHOR_LIST_SIZE - 1);
649

    
650
    x = inst_idistraw[idx];
651

    
652
    return (x);
653
}
654

    
655
int instance_get_idist_mm(int idx) //get instantaneous range
656
{
657
    int x ;
658

    
659
    idx &= (MAX_ANCHOR_LIST_SIZE - 1);
660

    
661
    x = (int)(inst_idist[idx]*1000);
662

    
663
    return (x);
664
}
665

    
666
int instance_get_idistraw_mm(int idx) //get instantaneous range (uncorrected)
667
{
668
    int x ;
669

    
670
    idx &= (MAX_ANCHOR_LIST_SIZE - 1);
671

    
672
    x = (int)(inst_idistraw[idx]*1000);
673

    
674
    return (x);
675
}
676

    
677
/* @fn           instanceSet16BitAddress
678
 * @brief set the 16-bit MAC address
679
 *
680
 */
681
void instance_set_16bit_address(uint16_t address)
682
{
683
        instance_data_t* inst = instance_get_local_structure_ptr(0);
684
    inst->instanceAddress16 = address ;       // copy configurations
685
}
686

    
687
/**
688
 * @brief this function configures the Frame Control and PAN ID bits
689
 */
690
void instance_config_frameheader_16bit(instance_data_t *inst)
691
{
692
    //set frame type (0-2), SEC (3), Pending (4), ACK (5), PanIDcomp(6)
693
    inst->msg_f.frameCtrl[0] = 0x1 /*frame type 0x1 == data*/ | 0x40 /*PID comp*/;
694

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

    
698
        inst->msg_f.panID[0] = (inst->panID) & 0xff;
699
        inst->msg_f.panID[1] = inst->panID >> 8;
700

    
701
    inst->msg_f.seqNum = 0;
702
}
703

    
704
/**
705
 * @brief this function writes DW TX Frame Control, Delay TX Time and Starts Transmission
706
 */
707
int instance_send_delayed_frame(instance_data_t *inst, int delayedTx)
708
{
709
    int result = 0;
710

    
711
    dwt_writetxfctrl(inst->psduLength, 0, 1);
712
    if(delayedTx == DWT_START_TX_DELAYED)
713
    {
714
        dwt_setdelayedtrxtime(inst->delayedTRXTime32h) ; //should be high 32-bits of delayed TX TS
715
    }
716

    
717
    //begin delayed TX of frame
718
    if (dwt_starttx(delayedTx | inst->wait4ack))  // delayed start was too late
719
    {
720
        result = 1; //late/error
721
        //inst->lateTX++;
722
    }
723
    else
724
    {
725
        inst->timeofTx = chVTGetSystemTimeX();
726
        inst->monitor = 1;
727
    }
728
    return result;                                              // state changes
729
}
730

    
731
//
732
// NB: This function is called from the (TX) interrupt handler
733
//
734
void tx_conf_cb(const dwt_cb_data_t *txd)
735
{
736
    (void) txd;
737
        instance_data_t* inst = instance_get_local_structure_ptr(0);
738
    uint8_t txTimeStamp[5] = {0, 0, 0, 0, 0};
739
        event_data_t dw_event;
740

    
741
    dw_event.uTimeStamp = chVTGetSystemTimeX();
742

    
743
        if(inst->twrMode == RESPONDER_B) //anchor has responded to a blink - don't report this event
744
        {
745
                inst->twrMode = LISTENER ;
746
        }
747
#if(DISCOVERY == 1)
748
        else if (inst->twrMode == GREETER)
749
        {
750
                //don't report TX event ...
751
        }
752
#endif
753
        else
754
        {
755
        //uint64_t txtimestamp = 0;
756

    
757
                //NOTE - we can only get TX good (done) while here
758
                //dwt_readtxtimestamp((uint8*) &inst->txu.txTimeStamp);
759

    
760
                dwt_readtxtimestamp(txTimeStamp) ;
761
                instance_seteventtime(&dw_event, txTimeStamp);
762

    
763
                dw_event.rxLength = inst->psduLength;
764
                dw_event.type =  0;
765
                dw_event.typePend =  0;
766
                //dw_event.typeSave = DWT_SIG_TX_DONE;
767

    
768
        memcpy((uint8_t *)&dw_event.msgu.frame[0], (uint8_t *)&inst->msg_f, inst->psduLength);
769

    
770
                instance_putevent(dw_event, DWT_SIG_TX_DONE);
771

    
772
                //inst->txMsgCount++;
773
        }
774

    
775
        inst->monitor = 0;
776
}
777

    
778

    
779
void instance_seteventtime(event_data_t *dw_event, uint8_t* timeStamp)
780
{
781
    dw_event->timeStamp32l =  (uint32_t)timeStamp[0] + ((uint32_t)timeStamp[1] << 8) + ((uint32_t)timeStamp[2] << 16) + ((uint32_t)timeStamp[3] << 24);
782
        dw_event->timeStamp = timeStamp[4];
783
        dw_event->timeStamp <<= 32;
784
        dw_event->timeStamp += dw_event->timeStamp32l;
785
    dw_event->timeStamp32h = ((uint32_t)timeStamp[4] << 24) + (dw_event->timeStamp32l >> 8);
786
}
787

    
788

    
789
int instance_peekevent(void)
790
{
791
        instance_data_t* inst = instance_get_local_structure_ptr(0);
792
    return inst->dwevent[inst->dweventPeek].type; //return the type of event that is in front of the queue
793
}
794

    
795
void instance_putevent(event_data_t newevent, uint8_t etype)
796
{
797
        instance_data_t* inst = instance_get_local_structure_ptr(0);
798
        //newevent.gotit = 0 ; //newevent.eventtimeclr = 0;
799

    
800
        //copy event
801
        inst->dwevent[inst->dweventIdxIn] = newevent;
802

    
803
        //set type - this makes it a new event (making sure the event data is copied before event is set as new)
804
        //to make sure that the get event function does not get an incomplete event
805
        inst->dwevent[inst->dweventIdxIn].type = etype;
806

    
807
        inst->dweventIdxIn++;
808

    
809
        if(MAX_EVENT_NUMBER == inst->dweventIdxIn)
810
        {
811
                inst->dweventIdxIn = 0;
812
        }
813
}
814

    
815
event_data_t dw_event_g;
816

    
817
event_data_t* instance_getevent(int x)
818
{
819
    (void) x;
820
        instance_data_t* inst = instance_get_local_structure_ptr(0);
821
        int indexOut = inst->dweventIdxOut;
822
        if(inst->dwevent[indexOut].type == 0) //exit with "no event"
823
        {
824
                dw_event_g.type = 0;
825
                //dw_event_g.typeSave = 0;
826
                return &dw_event_g;
827
        }
828

    
829
        //copy the event
830
        //dw_event_g.typeSave = inst->dwevent[indexOut].typeSave ;
831
        dw_event_g.typePend = inst->dwevent[indexOut].typePend ;
832
        dw_event_g.rxLength = inst->dwevent[indexOut].rxLength ;
833
        dw_event_g.timeStamp = inst->dwevent[indexOut].timeStamp ;
834
        dw_event_g.timeStamp32l = inst->dwevent[indexOut].timeStamp32l ;
835
        dw_event_g.timeStamp32h = inst->dwevent[indexOut].timeStamp32h ;
836
        dw_event_g.uTimeStamp = inst->dwevent[indexOut].uTimeStamp ;
837

    
838
        memcpy(&dw_event_g.msgu, &inst->dwevent[indexOut].msgu, sizeof(inst->dwevent[indexOut].msgu));
839

    
840
        dw_event_g.type = inst->dwevent[indexOut].type ;
841

    
842
        //inst->dwevent[indexOut].gotit = x;
843

    
844
        inst->dwevent[indexOut].type = 0; //clear the event
845

    
846
        inst->dweventIdxOut++;
847
        if(MAX_EVENT_NUMBER == inst->dweventIdxOut) //wrap the counter
848
        {
849
                inst->dweventIdxOut = 0;
850
        }
851
        inst->dweventPeek = inst->dweventIdxOut; //set the new peek value
852

    
853
        return &dw_event_g;
854
}
855

    
856
void instance_clearevents(void)
857
{
858
        int i = 0;
859
        instance_data_t* inst = instance_get_local_structure_ptr(0);
860

    
861
        for(i=0; i<MAX_EVENT_NUMBER; i++)
862
        {
863
        memset(&inst->dwevent[i], 0, sizeof(event_data_t));
864
        }
865

    
866
        inst->dweventIdxIn = 0;
867
        inst->dweventIdxOut = 0;
868
        inst->dweventPeek = 0;
869

    
870
}
871

    
872

    
873
void instance_config_txpower(uint32_t txpower)
874
{
875
        instance_data_t* inst = instance_get_local_structure_ptr(0);
876
        inst->txPower = txpower ;
877

    
878
        inst->txPowerChanged = 1;
879

    
880
}
881

    
882
void instance_set_txpower(void)
883
{
884
        instance_data_t* inst = instance_get_local_structure_ptr(0);
885
        if(inst->txPowerChanged == 1)
886
        {
887
            //Configure TX power
888
            dwt_write32bitreg(0x1E, inst->txPower);
889

    
890
                inst->txPowerChanged = 0;
891
        }
892
}
893

    
894
void instance_config_antennadelays(uint16_t tx, uint16_t rx)
895
{
896
        instance_data_t* inst = instance_get_local_structure_ptr(0);
897
        inst->txAntennaDelay = tx ;
898
        inst->rxAntennaDelay = rx ;
899

    
900
        inst->antennaDelayChanged = 1;
901
}
902

    
903
void instance_set_antennadelays(void)
904
{
905
        instance_data_t* inst = instance_get_local_structure_ptr(0);
906
        if(inst->antennaDelayChanged == 1)
907
        {
908
                dwt_setrxantennadelay(inst->rxAntennaDelay);
909
                dwt_settxantennadelay(inst->txAntennaDelay);
910

    
911
                inst->antennaDelayChanged = 0;
912
        }
913
}
914

    
915

    
916
uint16_t instance_get_txantdly(void)
917
{
918
        instance_data_t* inst = instance_get_local_structure_ptr(0);
919
        return inst->txAntennaDelay;
920
}
921

    
922
uint16_t instance_get_rxantdly(void)
923
{
924
        instance_data_t* inst = instance_get_local_structure_ptr(0);
925
        return inst->rxAntennaDelay;
926
}
927

    
928
uint8_t instance_validranges(void)
929
{
930
        instance_data_t* inst = instance_get_local_structure_ptr(0);
931
    uint8_t x = inst->rxResponseMaskReport;
932
        inst->rxResponseMaskReport = 0; //reset mask as we have printed out the ToFs
933
        return x;
934
}
935

    
936

    
937

    
938
// -------------------------------------------------------------------------------------------------------------------
939
// function to set the fixed reply delay time (in us)
940
//
941
// This sets delay for RX to TX - Delayed Send, and for TX to RX delayed receive (wait for response) functionality,
942
// and the frame wait timeout value to use.  This is a function of data rate, preamble length, and PRF
943

    
944
extern uint8_t dwnsSFDlen[];
945

    
946
float calc_length_data(float msgdatalen)
947
{
948
        instance_data_t* inst = instance_get_local_structure_ptr(0);
949

    
950
        int x = 0;
951

    
952
        x = (int) ceil(msgdatalen*8/330.0f);
953

    
954
        msgdatalen = msgdatalen*8 + x*48;
955

    
956
        //assume PHR length is 172308ns for 110k and 21539ns for 850k/6.81M
957
        if(inst->configData.dataRate == DWT_BR_110K)
958
        {
959
                msgdatalen *= 8205.13f;
960
                msgdatalen += 172308; // PHR length in nanoseconds
961

    
962
        }
963
        else if(inst->configData.dataRate == DWT_BR_850K)
964
        {
965
                msgdatalen *= 1025.64f;
966
                msgdatalen += 21539; // PHR length in nanoseconds
967
        }
968
        else
969
        {
970
                msgdatalen *= 128.21f;
971
                msgdatalen += 21539; // PHR length in nanoseconds
972
        }
973

    
974
        return msgdatalen ;
975
}
976
void instance_set_replydelay(int delayus) //delay in us
977
{
978
        instance_data_t *inst = &instance_data[0];
979

    
980
    int margin = 3000; //2000 symbols
981
    int respframe = 0;
982
    int respframe_sy = 0;
983
    int pollframe_sy = 0;
984
    //int finalframeT_sy = 0;
985
    int finalframeA_sy = 0;
986

    
987
        //configure the rx delay receive delay time, it is dependent on the message length
988
        float msgdatalen_resp = 0;
989
        float msgdatalen_poll = 0;
990
        float msgdatalen_finalA = 0;
991
        //float msgdatalen_finalT = 0;
992
        float preamblelen = 0;
993
        int sfdlen = 0;
994

    
995
        //Set the RX timeouts based on the longest expected message - the Final message
996
        //Poll = 13, Response = 20, Final = 44 bytes
997
        //msgdatalen = TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
998
        msgdatalen_resp = calc_length_data(ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC);
999
        msgdatalen_poll = calc_length_data(TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC);
1000

    
1001
        msgdatalen_finalA = calc_length_data(ANCH_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC);
1002
        //msgdatalen_finalT = calc_length_data(TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC);
1003

    
1004
        //SFD length is 64 for 110k (always)
1005
        //SFD length is 8 for 6.81M, and 16 for 850k, but can vary between 8 and 16 bytes
1006
        sfdlen = dwnsSFDlen[inst->configData.dataRate];
1007

    
1008
        switch (inst->configData.txPreambLength)
1009
    {
1010
    case DWT_PLEN_4096 : preamblelen = 4096.0f; break;
1011
    case DWT_PLEN_2048 : preamblelen = 2048.0f; break;
1012
    case DWT_PLEN_1536 : preamblelen = 1536.0f; break;
1013
    case DWT_PLEN_1024 : preamblelen = 1024.0f; break;
1014
    case DWT_PLEN_512  : preamblelen = 512.0f; break;
1015
    case DWT_PLEN_256  : preamblelen = 256.0f; break;
1016
    case DWT_PLEN_128  : preamblelen = 128.0f; break;
1017
    case DWT_PLEN_64   : preamblelen = 64.0f; break;
1018
            }
1019

    
1020
        //preamble  = plen * (994 or 1018) depending on 16 or 64 PRF
1021
        if(inst->configData.prf == DWT_PRF_16M)
1022
        {
1023
                preamblelen = (sfdlen + preamblelen) * 0.99359f;
1024
        }
1025
        else
1026
        {
1027
                preamblelen = (sfdlen + preamblelen) * 1.01763f;
1028
        }
1029

    
1030
        respframe_sy = (DW_RX_ON_DELAY + (int)((preamblelen + ((msgdatalen_resp + margin)/1000.0))/ 1.0256)) ;
1031
        pollframe_sy = (DW_RX_ON_DELAY + (int)((preamblelen + ((msgdatalen_poll + margin)/1000.0))/ 1.0256)) ;
1032

    
1033
        finalframeA_sy = (DW_RX_ON_DELAY + (int)((preamblelen + ((msgdatalen_finalA + margin)/1000.0))/ 1.0256)) ;
1034
        //finalframeT_sy = (DW_RX_ON_DELAY + (int)((preamblelen + ((msgdatalen_finalT + margin)/1000.0))/ 1.0256)) ;
1035

    
1036
        //tag to anchor ranging consists of poll, 4xresponse and final
1037
        //pollTx2FinalTxDelay delay is the time from start of sending of the poll to the start of sending of the final message
1038
        //this is configured by the user with pollTxToFinalTxDly in sfConfig_t
1039
        inst->pollTx2FinalTxDelay = instance_convert_usec_to_devtimeu (delayus);
1040

    
1041
        //the anchor to anchor ranging consist of A0 ranging to A1 and A2 and A1 ranging to A2
1042
        //so there are a maximum of two responses, thus the poll to final delay can be shorter
1043
        inst->pollTx2FinalTxDelayAnc = instance_convert_usec_to_devtimeu ((delayus >> 1) + RX_RESPONSE_TURNAROUND);
1044

    
1045
        //this is the delay the anchors 1, 2, etc.. will send the response back at...
1046
        //anchor 2 will have the delay set to 2 * fixedReplyDelayAnc
1047
        //andhor 3 will have the delay set to 3 * fixedReplyDelayAnc and so on...
1048
        //this delay depends on how quickly the tag can receive and process the message from previous anchor
1049
        //(and also the frame length of course)
1050
        respframe = (int)(preamblelen + (msgdatalen_resp/1000.0)); //length of response frame (micro seconds)
1051
        if(inst->configData.dataRate == DWT_BR_110K)
1052
        {
1053
                //set the frame wait timeout time - total time the frame takes in symbols
1054
                inst->fwtoTime_sy = respframe_sy + RX_RESPONSE_TURNAROUND + 400; //add some margin because of the resp to resp RX turn on time
1055
        inst->preambleDuration32h = (uint32_t) (((uint64_t) instance_convert_usec_to_devtimeu (preamblelen)) >> 8) + DW_RX_ON_DELAY; //preamble duration + 16 us for RX on
1056
        }
1057
        else
1058
        {
1059
                //set the frame wait timeout time - total time the frame takes in symbols
1060
                inst->fwtoTime_sy = respframe_sy + RX_RESPONSE_TURNAROUND; //add some margin because of the resp to resp RX turn on time
1061
        inst->preambleDuration32h = (uint32_t) (((uint64_t) instance_convert_usec_to_devtimeu (preamblelen)) >> 8) + DW_RX_ON_DELAY; //preamble duration + 16 us for RX on
1062
        }
1063

    
1064
        inst->tagRespRxDelay_sy = RX_RESPONSE_TURNAROUND + respframe_sy - pollframe_sy;
1065

    
1066
        //anchors will reply after RX_RESPONSE_TURNAROUND time, also subtract 16 us for RX on delay
1067
        inst->ancRespRxDelay_sy = RX_RESPONSE_TURNAROUND - DW_RX_ON_DELAY;
1068

    
1069
    inst->fixedReplyDelayAnc32h = ((uint64_t)instance_convert_usec_to_devtimeu (respframe + RX_RESPONSE_TURNAROUND) >> 8);
1070

    
1071
        inst->fwto4RespFrame_sy = respframe_sy;
1072
        inst->fwto4FinalFrame_sy = finalframeA_sy + 200; //add some margin so we don't timeout too soon
1073

    
1074
        //RX Poll (from A0), the Final will come
1075
        inst->anc1RespTx2FinalRxDelay_sy = ((delayus >> 1) + RX_RESPONSE_TURNAROUND) - (respframe_sy+RX_RESPONSE_TURNAROUND) - DW_RX_ON_DELAY - respframe;
1076
    inst->anc2RespTx2FinalRxDelay_sy = (delayus >> 1) + RX_RESPONSE_TURNAROUND - 2*(respframe_sy+RX_RESPONSE_TURNAROUND) - DW_RX_ON_DELAY - respframe;
1077

    
1078
}
1079

    
1080
/* @fn           instance_calc_ranges
1081
 * @brief calculate range for each ToF in the array, and return a mask of valid ranges
1082
 * */
1083
int instance_calc_ranges(uint32_t *array, uint16_t size, int reportRange, uint8_t* mask)
1084
{
1085
        int i;
1086
        int newRange = TOF_REPORT_NUL;
1087
        int distance = 0;
1088

    
1089
        for(i=0; i<size; i++)
1090
        {
1091
        uint32_t tofx = array[i];
1092
                if(tofx != INVALID_TOF) //if ToF == 0 - then no new range to report
1093
                {
1094
                        distance = instance_calculate_rangefromTOF(i, tofx);
1095
                }
1096

    
1097
                if(distance == 1)
1098
                {
1099
                        newRange = reportRange;
1100
                }
1101
                else
1102
                {
1103
                        //clear mask
1104
                        *mask &= ~(0x1 << i) ;
1105
                        instance_cleardisttable(i);
1106
                }
1107
                array[i] = INVALID_TOF;
1108

    
1109
                distance = 0;
1110
        }
1111

    
1112
        return newRange;
1113
}
1114

    
1115

    
1116
/* ==========================================================
1117

1118
Notes:
1119

1120
Previously code handled multiple instances in a single console application
1121

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

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

1126
*/
1127

    
1128
#endif /* defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 0) */
1129