Statistics
| Branch: | Tag: | Revision:

amiro-lld / source / deca_instance_common.c @ fce9feec

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

    
15

    
16
#include <deca_instance.h>
17
#if defined(AMIROLLD_CFG_USE_DW1000) || defined(__DOXYGEN__)
18

    
19

    
20
#include <alld_dw1000.h>
21
#include <string.h>
22
#include <math.h>
23

    
24

    
25

    
26
extern double dwt_getrangebias(uint8_t chan, float range, uint8_t prf);
27

    
28
extern const uint16_t rfDelays[2];
29
extern const uint16_t rfDelaysTREK[2];
30
extern const tx_struct txSpectrumConfig[8];
31

    
32

    
33

    
34
// -------------------------------------------------------------------------------------------------------------------
35
// Deca Calibration Values
36
// -------------------------------------------------------------------------------------------------------------------
37

    
38
#define DWT_PRF_64M_RFDLY   (514.462f)
39
#define DWT_PRF_16M_RFDLY   (513.9067f)
40

    
41
// -------------------------------------------------------------------------------------------------------------------
42

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

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

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

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

    
127
//int instance_starttxtest(int framePeriod)
128
//{
129
//    //define some test data for the tx buffer
130
//    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";
131

    
132
//    //NOTE: SPI frequency must be < 3MHz
133
//    port_set_dw1000_slowrate();  //max SPI before PLLs configured is ~4M
134

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

    
139
//    dwt_writetxdata(127, (uint8 *)  msg, 0) ;
140
//    dwt_writetxfctrl(127, 0, 0);
141

    
142
//    //to start the first frame - set TXSTRT
143
//    dwt_starttx(DWT_START_TX_IMMEDIATE);
144

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

    
153
//    return DWT_SUCCESS ;
154
//}
155

    
156
// -------------------------------------------------------------------------------------------------------------------
157
//      Data Definitions
158
// -------------------------------------------------------------------------------------------------------------------
159

    
160
static instance_data_t instance_data[NUM_INST] ;
161

    
162
static double inst_tdist[MAX_TAG_LIST_SIZE] ;
163
static double inst_idist[MAX_ANCHOR_LIST_SIZE] ;
164
static double inst_idistraw[MAX_ANCHOR_LIST_SIZE] ;
165

    
166
// -------------------------------------------------------------------------------------------------------------------
167
// Functions
168
// -------------------------------------------------------------------------------------------------------------------
169

    
170

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

    
181
        return &instance_data[x];
182
}
183

    
184

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

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

    
196
    dt =  (uint64_t) (dtime) ;
197

    
198
    return dt;
199
}
200

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

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

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

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

    
244
        distance = distance - dwt_getrangebias(inst->configData.chan, (float) distance_to_correct, inst->configData.prf);
245
#endif
246

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

    
250
        inst_idist[idx] = distance;
251

    
252
        inst->longTermRangeCount++ ;                          // for computing a long term average
253

    
254
    return 1;
255
}// end of calculateRangeFromTOF
256

    
257
void instance_set_tagdist(int tidx, int aidx)
258
{
259
        inst_tdist[tidx] = inst_idist[aidx];
260
}
261

    
262
double instance_get_tagdist(int idx)
263
{
264
        return inst_tdist[idx];
265
}
266

    
267
void instance_cleardisttable(int idx)
268
{
269
        inst_idistraw[idx] = 0;
270
        inst_idist[idx] = 0;
271
}
272

    
273
void instance_cleardisttableall(void)
274
{
275
        int i;
276

    
277
        for(i=0; i<MAX_ANCHOR_LIST_SIZE; i++)
278
        {
279
                inst_idistraw[i] = 0xffff;
280
                inst_idist[i] = 0xffff;
281
        }
282
}
283

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

    
296
    return inst->mode;
297
}
298

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

    
307
int instance_newrangeancadd(void)
308
{
309
        instance_data_t* inst = instance_get_local_structure_ptr(0);
310
    return inst->newRangeAncAddress;
311
}
312

    
313
int instance_newrangetagadd(void)
314
{
315
        instance_data_t* inst = instance_get_local_structure_ptr(0);
316
    return inst->newRangeTagAddress;
317
}
318

    
319
int instance_newrangetim(void)
320
{
321
        instance_data_t* inst = instance_get_local_structure_ptr(0);
322
    return inst->newRangeTime;
323
}
324

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

    
333
    //inst->rxTimeouts = 0 ;
334
    //inst->txMsgCount = 0 ;
335
    //inst->rxMsgCount = 0 ;
336

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

    
339
    inst->frameSN = 0;
340

    
341
    inst->longTermRangeCount  = 0;
342

    
343

    
344
    for(i=0; i<MAX_ANCHOR_LIST_SIZE; i++)
345
        {
346
            inst->tofArray[i] = INVALID_TOF;
347
        }
348

    
349
    for(i=0; i<MAX_TAG_LIST_SIZE; i++)
350
        {
351
                inst->tof[i] = INVALID_TOF;
352
        }
353

    
354
} // end instanceclearcounts()
355

    
356

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

    
366
    inst->mode =  inst_mode;                                // assume listener,
367
    inst->twrMode = LISTENER;
368
    inst->testAppState = TA_INIT ;
369
    inst->instToSleep = FALSE;
370

    
371

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

    
376
        //this initialises DW1000 and uses specified configurations from OTP/ROM
377
    result = dwt_initialise(DWT_LOADUCODE, drv) ;
378

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

    
382
    if (DWT_SUCCESS != result)
383
    {
384
        return (-1) ;   // device initialise has failed
385
    }
386

    
387
    instance_clearcounts() ;
388

    
389
    inst->wait4ack = 0;
390
    inst->instanceTimerEn = 0;
391

    
392
    instance_clearevents();
393

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

    
403
    dwt_setdblrxbuffmode(0); //disable double RX buffer
404

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

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

    
419
    inst->monitor = 0;
420

    
421
    //inst->lateTX = 0;
422
    //inst->lateRX = 0;
423

    
424
    inst->remainingRespToRx = -1; //initialise
425

    
426
    inst->rxResps = 0;
427

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

    
430
    inst->delayedTRXTime32h = 0;
431

    
432
#if (READ_EVENT_COUNTERS == 1)
433
    dwt_configeventcounters(1);
434
#endif
435
    return 0 ;
436
}
437

    
438
// -------------------------------------------------------------------------------------------------------------------
439
//
440
// Return the Device ID register value, enables higher level validation of physical device presence
441
//
442

    
443
uint32_t instance_readdeviceid(void)
444
{
445
    return dwt_readdevid() ;
446
}
447

    
448

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

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

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

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

    
490
    //configure the channel parameters
491
    dwt_configure(&inst->configData) ;
492

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

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

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

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

    
517
        port_set_dw1000_slowrate(); //reduce SPI to < 3MHz
518

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

    
539
        port_set_dw1000_fastrate(); //increase SPI to max
540

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

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

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

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

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

    
584
    inst->rxAntennaDelay = inst->txAntennaDelay;
585

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

    
590
                dwt_loadopsettabfromotp(0);
591

    
592
                port_set_dw1000_fastrate(); //increase SPI to max
593
    }
594

    
595

    
596
    inst->tagPeriod_ms = sfConfig->tagPeriod_ms; //set the Tag sleep time
597
        inst->sframePeriod_ms = sfConfig->sfPeriod_ms;
598
        inst->slotDuration_ms = sfConfig->slotDuration_ms;
599
        inst->tagSleepRnd_ms = sfConfig->slotDuration_ms;
600
        inst->numSlots = sfConfig->numSlots;
601

    
602
        //last two slots are used for anchor to anchor ranging
603
        inst->a0SlotTime_ms = (inst->numSlots-2) * inst->slotDuration_ms;
604

    
605
        //set the default response delays
606
        instance_set_replydelay(sfConfig->pollTxToFinalTxDly_us);
607

    
608
}
609

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

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

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

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

    
633
    return (x);
634
}
635

    
636
double instance_get_idist(int idx) //get instantaneous range
637
{
638
    double x ;
639

    
640
    idx &= (MAX_ANCHOR_LIST_SIZE - 1);
641

    
642
    x = inst_idist[idx];
643

    
644
    return (x);
645
}
646

    
647
double instance_get_idistraw(int idx) //get instantaneous range (uncorrected)
648
{
649
    double x ;
650

    
651
    idx &= (MAX_ANCHOR_LIST_SIZE - 1);
652

    
653
    x = inst_idistraw[idx];
654

    
655
    return (x);
656
}
657

    
658
int instance_get_idist_mm(int idx) //get instantaneous range
659
{
660
    int x ;
661

    
662
    idx &= (MAX_ANCHOR_LIST_SIZE - 1);
663

    
664
    x = (int)(inst_idist[idx]*1000);
665

    
666
    return (x);
667
}
668

    
669
int instance_get_idistraw_mm(int idx) //get instantaneous range (uncorrected)
670
{
671
    int x ;
672

    
673
    idx &= (MAX_ANCHOR_LIST_SIZE - 1);
674

    
675
    x = (int)(inst_idistraw[idx]*1000);
676

    
677
    return (x);
678
}
679

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

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

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

    
701
        inst->msg_f.panID[0] = (inst->panID) & 0xff;
702
        inst->msg_f.panID[1] = inst->panID >> 8;
703

    
704
    inst->msg_f.seqNum = 0;
705
}
706

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

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

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

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

    
744
    dw_event.uTimeStamp = chVTGetSystemTimeX();
745

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

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

    
763
                dwt_readtxtimestamp(txTimeStamp) ;
764
                instance_seteventtime(&dw_event, txTimeStamp);
765

    
766
                dw_event.rxLength = inst->psduLength;
767
                dw_event.type =  0;
768
                dw_event.typePend =  0;
769
                //dw_event.typeSave = DWT_SIG_TX_DONE;
770

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

    
773
                instance_putevent(dw_event, DWT_SIG_TX_DONE);
774

    
775
                //inst->txMsgCount++;
776
        }
777

    
778
        inst->monitor = 0;
779
}
780

    
781

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

    
791

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

    
798
void instance_putevent(event_data_t newevent, uint8_t etype)
799
{
800
        instance_data_t* inst = instance_get_local_structure_ptr(0);
801
        //newevent.gotit = 0 ; //newevent.eventtimeclr = 0;
802

    
803
        //copy event
804
        inst->dwevent[inst->dweventIdxIn] = newevent;
805

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

    
810
        inst->dweventIdxIn++;
811

    
812
        if(MAX_EVENT_NUMBER == inst->dweventIdxIn)
813
        {
814
                inst->dweventIdxIn = 0;
815
        }
816
}
817

    
818
event_data_t dw_event_g;
819

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

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

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

    
843
        dw_event_g.type = inst->dwevent[indexOut].type ;
844

    
845
        //inst->dwevent[indexOut].gotit = x;
846

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

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

    
856
        return &dw_event_g;
857
}
858

    
859
void instance_clearevents(void)
860
{
861
        int i = 0;
862
        instance_data_t* inst = instance_get_local_structure_ptr(0);
863

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

    
869
        inst->dweventIdxIn = 0;
870
        inst->dweventIdxOut = 0;
871
        inst->dweventPeek = 0;
872

    
873
}
874

    
875

    
876
void instance_config_txpower(uint32_t txpower)
877
{
878
        instance_data_t* inst = instance_get_local_structure_ptr(0);
879
        inst->txPower = txpower ;
880

    
881
        inst->txPowerChanged = 1;
882

    
883
}
884

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

    
893
                inst->txPowerChanged = 0;
894
        }
895
}
896

    
897
void instance_config_antennadelays(uint16_t tx, uint16_t rx)
898
{
899
        instance_data_t* inst = instance_get_local_structure_ptr(0);
900
        inst->txAntennaDelay = tx ;
901
        inst->rxAntennaDelay = rx ;
902

    
903
        inst->antennaDelayChanged = 1;
904
}
905

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

    
914
                inst->antennaDelayChanged = 0;
915
        }
916
}
917

    
918

    
919
uint16_t instance_get_txantdly(void)
920
{
921
        instance_data_t* inst = instance_get_local_structure_ptr(0);
922
        return inst->txAntennaDelay;
923
}
924

    
925
uint16_t instance_get_rxantdly(void)
926
{
927
        instance_data_t* inst = instance_get_local_structure_ptr(0);
928
        return inst->rxAntennaDelay;
929
}
930

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

    
939

    
940

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

    
947
extern uint8_t dwnsSFDlen[];
948

    
949
float calc_length_data(float msgdatalen)
950
{
951
        instance_data_t* inst = instance_get_local_structure_ptr(0);
952

    
953
        int x = 0;
954

    
955
        x = (int) ceil(msgdatalen*8/330.0f);
956

    
957
        msgdatalen = msgdatalen*8 + x*48;
958

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

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

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

    
983
    int margin = 3000; //2000 symbols
984
    int respframe = 0;
985
    int respframe_sy = 0;
986
    int pollframe_sy = 0;
987
    //int finalframeT_sy = 0;
988
    int finalframeA_sy = 0;
989

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

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

    
1004
        msgdatalen_finalA = calc_length_data(ANCH_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC);
1005
        //msgdatalen_finalT = calc_length_data(TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC);
1006

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

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

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

    
1033
        respframe_sy = (DW_RX_ON_DELAY + (int)((preamblelen + ((msgdatalen_resp + margin)/1000.0))/ 1.0256)) ;
1034
        pollframe_sy = (DW_RX_ON_DELAY + (int)((preamblelen + ((msgdatalen_poll + margin)/1000.0))/ 1.0256)) ;
1035

    
1036
        finalframeA_sy = (DW_RX_ON_DELAY + (int)((preamblelen + ((msgdatalen_finalA + margin)/1000.0))/ 1.0256)) ;
1037
        //finalframeT_sy = (DW_RX_ON_DELAY + (int)((preamblelen + ((msgdatalen_finalT + margin)/1000.0))/ 1.0256)) ;
1038

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

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

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

    
1067
        inst->tagRespRxDelay_sy = RX_RESPONSE_TURNAROUND + respframe_sy - pollframe_sy;
1068

    
1069
        //anchors will reply after RX_RESPONSE_TURNAROUND time, also subtract 16 us for RX on delay
1070
        inst->ancRespRxDelay_sy = RX_RESPONSE_TURNAROUND - DW_RX_ON_DELAY;
1071

    
1072
    inst->fixedReplyDelayAnc32h = ((uint64_t)instance_convert_usec_to_devtimeu (respframe + RX_RESPONSE_TURNAROUND) >> 8);
1073

    
1074
        inst->fwto4RespFrame_sy = respframe_sy;
1075
        inst->fwto4FinalFrame_sy = finalframeA_sy + 200; //add some margin so we don't timeout too soon
1076

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

    
1081
}
1082

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

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

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

    
1112
                distance = 0;
1113
        }
1114

    
1115
        return newRange;
1116
}
1117

    
1118

    
1119
/* ==========================================================
1120

1121
Notes:
1122

1123
Previously code handled multiple instances in a single console application
1124

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

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

1129
*/
1130

    
1131
#endif /* defined(AMIROLLD_CFG_USE_DW1000) */