amiro-lld / source / deca_instance_common.c @ 8c47f14b
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) */ |