amiro-lld / source / DW1000 / v0 / deca_instance_common.c @ c3c5444e
History | View | Annotate | Download (34.107 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 |
|