amiro-lld / source / deca_instance_common.c @ 8c47f14b
History | View | Annotate | Download (34.1 KB)
1 | fce9feec | Robin Ewers | /*! ----------------------------------------------------------------------------
|
---|---|---|---|
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) */ |