amiro-lld / source / DW1000 / v1 / deca_instance_common_v1.c @ ef5bc242
History | View | Annotate | Download (71.35 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 == 1)) || defined(__DOXYGEN__) |
17 |
|
18 |
#include "module.h" |
19 |
#include <string.h> |
20 |
#include <math.h> |
21 |
#include <v1/deca_instance_v1.h> |
22 |
|
23 |
|
24 |
/*! @brief Software defined configuration settings for RTLS applications */
|
25 |
/*! Configuration for DecaRangeRTLS TREK Modes (4 default use cases selected by the switch S1 [2,3] on EVB1000, indexed 0 to 3 )*/
|
26 |
instanceConfig_t chConfig[4] ={
|
27 |
//mode 1 - S1: 2 off, 3 off
|
28 |
{ |
29 |
.channelNumber = 2, // channel |
30 |
.preambleCode = 4, // preambleCode |
31 |
.pulseRepFreq = DWT_PRF_16M, // prf
|
32 |
.dataRate = DWT_BR_110K, // datarate
|
33 |
.preambleLen = DWT_PLEN_1024, // preambleLength
|
34 |
.pacSize = DWT_PAC32, // pacSize
|
35 |
.nsSFD = 1, // non-standard SFD |
36 |
.sfdTO = (1025 + 64 - 32) // SFD timeout |
37 |
}, |
38 |
//mode 2 - S1: 2 on, 3 off
|
39 |
{ |
40 |
.channelNumber = 2, // channel |
41 |
.preambleCode = 4, // preambleCode |
42 |
.pulseRepFreq = DWT_PRF_16M, // prf
|
43 |
.dataRate = DWT_BR_6M8, // datarate
|
44 |
.preambleLen = DWT_PLEN_128, // preambleLength
|
45 |
.pacSize = DWT_PAC8, // pacSize
|
46 |
.nsSFD = 0, // non-standard SFD |
47 |
.sfdTO = (129 + 8 - 8) // SFD timeout |
48 |
}, |
49 |
//mode 3 - S1: 2 off, 3 on
|
50 |
{ |
51 |
.channelNumber = 5, // channel |
52 |
.preambleCode = 3, // preambleCode |
53 |
.pulseRepFreq = DWT_PRF_16M, // prf
|
54 |
.dataRate = DWT_BR_110K, // datarate
|
55 |
.preambleLen = DWT_PLEN_1024, // preambleLength
|
56 |
.pacSize = DWT_PAC32, // pacSize
|
57 |
.nsSFD = 1, // non-standard SFD |
58 |
.sfdTO = (1025 + 64 - 32) // SFD timeout |
59 |
}, |
60 |
//mode 4 - S1: 2 on, 3 on
|
61 |
{ |
62 |
.channelNumber = 5, // channel |
63 |
.preambleCode = 3, // preambleCode |
64 |
.pulseRepFreq = DWT_PRF_16M, // prf
|
65 |
.dataRate = DWT_BR_6M8, // datarate
|
66 |
.preambleLen = DWT_PLEN_128, // preambleLength
|
67 |
.pacSize = DWT_PAC8, // pacSize
|
68 |
.nsSFD = 0, // non-standard SFD |
69 |
.sfdTO = (129 + 8 - 8) // SFD timeout |
70 |
} |
71 |
}; |
72 |
|
73 |
|
74 |
/*! Slot and Superframe Configuration for DecaRangeRTLS TREK Modes (4 default use cases selected by the switch S1 [2,3] on EVB1000, indexed 0 to 3 )*/
|
75 |
sfConfig_t sfConfig[4] ={
|
76 |
//mode 1 - S1: 2 off (110 Kbps), 3 off (ch 2)
|
77 |
{ |
78 |
.slotPeriod = (28), //slot duration in milliseconds (NOTE: the ranging exchange must be able to complete in this time |
79 |
//e.g. tag sends a poll, 4 anchors send responses and tag sends the final + processing time
|
80 |
.numSlots = (10), //number of slots in the superframe (8 tag slots and 2 used for anchor to anchor ranging), |
81 |
.sfPeriod = (10*28), //in ms => 280ms frame means 3.57 Hz location rate |
82 |
.pollSleepDly = (10*28), //tag period in ms (sleep time + ranging time) |
83 |
.replyDly = (25000) //poll to final delay in microseconds (needs to be adjusted according to lengths of ranging frames) |
84 |
}, |
85 |
#if (DISCOVERY == 1) |
86 |
//mode 2 - S1: 2 on, 3 off
|
87 |
{ |
88 |
.slotPeriod = (10), //slot duration in milliseconds (NOTE: the ranging exchange must be able to complete in this time |
89 |
//e.g. tag sends a poll, 4 anchors send responses and tag sends the final + processing time
|
90 |
.numSlots = (100), //number of slots in the superframe (98 tag slots and 2 used for anchor to anchor ranging), |
91 |
.sfPeriod = (10*100), //in ms => 1000 ms frame means 1 Hz location rate |
92 |
.pollSleepDly = (10*100), //tag period in ms (sleep time + ranging time) |
93 |
.replyDly = (2500) //poll to final delay in microseconds (needs to be adjusted according to lengths of ranging frames) |
94 |
|
95 |
}, |
96 |
#else
|
97 |
//mode 2 - S1: 2 on (6.8 Mbps), 3 off (ch 2)
|
98 |
{ |
99 |
.slotPeriod = (10), //slot duration in milliseconds (NOTE: the ranging exchange must be able to complete in this time |
100 |
//e.g. tag sends a poll, 4 anchors send responses and tag sends the final + processing time
|
101 |
.numSlots = (10), //number of slots in the superframe (8 tag slots and 2 used for anchor to anchor ranging), |
102 |
.sfPeriod = (10*10), //in ms => 100 ms frame means 10 Hz location rate |
103 |
.pollSleepDly = (10*10), //tag period in ms (sleep time + ranging time) |
104 |
.replyDly = (3000) //poll to final delay in microseconds (needs to be adjusted according to lengths of ranging frames) |
105 |
}, |
106 |
#endif
|
107 |
//mode 3 - S1: 2 off (110 Kbps), 3 on (ch 5)
|
108 |
{ |
109 |
.slotPeriod = (28), //slot duration in milliseconds (NOTE: the ranging exchange must be able to complete in this time |
110 |
//e.g. tag sends a poll, 4 anchors send responses and tag sends the final + processing time
|
111 |
.numSlots = (10), //number of slots in the superframe (8 tag slots and 2 used for anchor to anchor ranging), |
112 |
.sfPeriod = (10*28), //in ms => 280ms frame means 3.57 Hz location rate |
113 |
.pollSleepDly = (10*28), //tag period in ms (sleep time + ranging time) |
114 |
.replyDly = (20000) //poll to final delay in microseconds (needs to be adjusted according to lengths of ranging frames) |
115 |
|
116 |
}, |
117 |
//mode 4 - S1: 2 on (6.8 Mbps), 3 on (ch 5)
|
118 |
{ |
119 |
.slotPeriod = (10), //slot duration in milliseconds (NOTE: the ranging exchange must be able to complete in this time |
120 |
//e.g. tag sends a poll, 4 anchors send responses and tag sends the final + processing time
|
121 |
.numSlots = (10), //number of slots in the superframe (8 tag slots and 2 used for anchor to anchor ranging), |
122 |
.sfPeriod = (10*10), //in ms => 100 ms frame means 10 Hz location rate |
123 |
.pollSleepDly = (10*10), //tag period in ms (sleep time + ranging time) |
124 |
.replyDly = (2500) //poll to final delay in microseconds (needs to be adjusted according to lengths of ranging frames) |
125 |
} |
126 |
}; |
127 |
|
128 |
|
129 |
// -------------------------------------------------------------------------------------------------------------------
|
130 |
|
131 |
//The table below specifies the default TX spectrum configuration parameters... this has been tuned for DW EVK hardware units
|
132 |
//the table is set for smart power - see below in the instance_config function how this is used when not using smart power
|
133 |
const tx_struct txSpectrumConfig[8] = |
134 |
{ |
135 |
//Channel 0 ----- this is just a place holder so the next array element is channel 1
|
136 |
{ |
137 |
0x0, //0 |
138 |
{ |
139 |
0x0, //0 |
140 |
0x0 //0 |
141 |
} |
142 |
}, |
143 |
//Channel 1
|
144 |
{ |
145 |
0xc9, //PG_DELAY |
146 |
{ |
147 |
0x15355575, //16M prf power |
148 |
0x07274767 //64M prf power |
149 |
} |
150 |
|
151 |
}, |
152 |
//Channel 2
|
153 |
{ |
154 |
0xc2, //PG_DELAY |
155 |
{ |
156 |
0x15355575, //16M prf power |
157 |
0x07274767 //64M prf power |
158 |
} |
159 |
}, |
160 |
//Channel 3
|
161 |
{ |
162 |
0xc5, //PG_DELAY |
163 |
{ |
164 |
0x0f2f4f6f, //16M prf power |
165 |
0x2b4b6b8b //64M prf power |
166 |
} |
167 |
}, |
168 |
//Channel 4
|
169 |
{ |
170 |
0x95, //PG_DELAY |
171 |
{ |
172 |
0x1f1f3f5f, //16M prf power |
173 |
0x3a5a7a9a //64M prf power |
174 |
} |
175 |
}, |
176 |
//Channel 5
|
177 |
{ |
178 |
0xc0, //PG_DELAY |
179 |
{ |
180 |
0x0E082848, //16M prf power |
181 |
0x25456585 //64M prf power |
182 |
} |
183 |
}, |
184 |
//Channel 6 ----- this is just a place holder so the next array element is channel 7
|
185 |
{ |
186 |
0x0, //0 |
187 |
{ |
188 |
0x0, //0 |
189 |
0x0 //0 |
190 |
} |
191 |
}, |
192 |
//Channel 7
|
193 |
{ |
194 |
0x93, //PG_DELAY |
195 |
{ |
196 |
0x32527292, //16M prf power |
197 |
0x5171B1d1 //64M prf power |
198 |
} |
199 |
} |
200 |
}; |
201 |
|
202 |
//these are default antenna delays for EVB1000, these can be used if there is no calibration data in the DW1000,
|
203 |
//or instead of the calibration data
|
204 |
const uint16_t rfDelays[2] = { |
205 |
(uint16_t) (((double)DWT_PRF_16M_RFDLY/ 2.0) * 1e-9 / DWT_TIME_UNITS),//PRF 16 |
206 |
(uint16_t) (((double)DWT_PRF_64M_RFDLY/ 2.0) * 1e-9 / DWT_TIME_UNITS) |
207 |
}; |
208 |
|
209 |
//these are default TREK Tag/Anchor antenna delays
|
210 |
const uint16_t rfDelaysTREK[2] = { |
211 |
(uint16_t) (((double)514.83f/ 2.0) * 1e-9 / DWT_TIME_UNITS),//channel 2 |
212 |
(uint16_t) (((double)514.65f/ 2.0) * 1e-9 / DWT_TIME_UNITS) //channel 5 |
213 |
}; |
214 |
|
215 |
|
216 |
// -------------------------------------------------------------------------------------------------------------------
|
217 |
// Data Definitions
|
218 |
// -------------------------------------------------------------------------------------------------------------------
|
219 |
|
220 |
instance_data_t instance_data[NUM_INST] ; |
221 |
|
222 |
static double inst_tdist[MAX_TAG_LIST_SIZE] ; |
223 |
static double inst_idist[MAX_ANCHOR_LIST_SIZE] ; |
224 |
static double inst_idistraw[MAX_ANCHOR_LIST_SIZE] ; |
225 |
|
226 |
instance_data_t instance_data[NUM_INST] ; |
227 |
|
228 |
typedef struct __attribute__((packed)) |
229 |
{ |
230 |
uint32_t deviceID ; |
231 |
uint8_t chan; // added chan here - used in the reading of acc
|
232 |
uint16_t rfrxDly; // rf delay (delay though the RF blocks before the signal comes out of the antenna i.e. "antenna delay")
|
233 |
uint16_t rftxDly; // rf delay (delay though the RF blocks before the signal comes out of the antenna i.e. "antenna delay")
|
234 |
uint32_t antennaDly; // antenna delay read from OTP 64 PRF value is in high 16 bits and 16M PRF in low 16 bits
|
235 |
uint32_t antennaCals[4]; // antenna delays for the TREKs (Anchor high 16-bits, Tag low 16-bits) |
236 |
uint32_t txPowCfg[12]; // stores the Tx power configuration read from OTP (6 channels consecutively with PRF16 then 64, e.g. Ch 1 PRF16 is index 0 and 64 index 1) |
237 |
uint32_t states[3] ; //MP workaround debug states register |
238 |
uint8_t statescount ; |
239 |
int prfIndex ;
|
240 |
uint32_t ldoTune ; //low 32 bits of LDO tune value
|
241 |
} platform_local_data_t ; |
242 |
|
243 |
static platform_local_data_t platformLocalData ; // Static local device data |
244 |
|
245 |
// -------------------------------------------------------------------------------------------------------------------
|
246 |
// Functions
|
247 |
// -------------------------------------------------------------------------------------------------------------------
|
248 |
|
249 |
// -------------------------------------------------------------------------------------------------------------------
|
250 |
// convert microseconds to device time
|
251 |
uint64_t convertmicrosectodevicetimeu (double microsecu){
|
252 |
uint64_t dt; |
253 |
long double dtime; |
254 |
|
255 |
dtime = (long double)((microsecu / (double) DWT_TIME_UNITS) / 1e6); |
256 |
|
257 |
dt = (uint64_t) (dtime) ; |
258 |
|
259 |
return dt;
|
260 |
} |
261 |
|
262 |
double convertdevicetimetosec(int32_t dt){
|
263 |
double f = 0; |
264 |
|
265 |
f = dt * DWT_TIME_UNITS ; // seconds #define TIME_UNITS (1.0/499.2e6/128.0) = 15.65e-12
|
266 |
|
267 |
return f ;
|
268 |
} |
269 |
|
270 |
#pragma GCC optimize ("O3") |
271 |
int reportTOF(int idx, uint32_t tofx){ |
272 |
double distance ;
|
273 |
double distance_to_correct;
|
274 |
double tof ;
|
275 |
int64_t tofi ; |
276 |
|
277 |
// check for negative results and accept them making them proper negative integers
|
278 |
tofi = (int32_t) tofx ; // make it signed
|
279 |
if (tofi > 0x7FFFFFFF){ // close up TOF may be negative |
280 |
tofi -= 0x80000000 ; // |
281 |
} |
282 |
|
283 |
// convert to seconds (as floating point)
|
284 |
tof = convertdevicetimetosec((int32_t)tofi) ; //this is divided by 4 to get single time of flight
|
285 |
inst_idistraw[idx] = distance = tof * SPEED_OF_LIGHT; |
286 |
|
287 |
#if (CORRECT_RANGE_BIAS == 1) |
288 |
//for the 6.81Mb data rate we assume gating gain of 6dB is used,
|
289 |
//thus a different range bias needs to be applied
|
290 |
//if(inst->configData.dataRate == DWT_BR_6M8)
|
291 |
if(instance_data[0].configData.smartPowerEn){ |
292 |
//1.31 for channel 2 and 1.51 for channel 5
|
293 |
if(instance_data[0].configData.chan == 5){ |
294 |
distance_to_correct = distance/1.51; |
295 |
} |
296 |
else{ //channel 2 |
297 |
distance_to_correct = distance/1.31; |
298 |
} |
299 |
} |
300 |
else{
|
301 |
distance_to_correct = distance; |
302 |
} |
303 |
|
304 |
distance = distance - dwt_getrangebias(instance_data[0].configData.chan, (float) distance_to_correct, instance_data[0].configData.prf); |
305 |
#endif
|
306 |
|
307 |
if ((distance < 0) || (distance > 20000.000)) // discard any results less than <0 cm or >20 km |
308 |
return 0; |
309 |
|
310 |
inst_idist[idx] = distance; |
311 |
|
312 |
instance_data[0].longTermRangeCount++ ; // for computing a long term average |
313 |
|
314 |
return 1; |
315 |
}// end of reportTOF
|
316 |
|
317 |
void setTagDist(int tidx, int aidx){ |
318 |
inst_tdist[tidx] = inst_idist[aidx]; |
319 |
} |
320 |
|
321 |
double getTagDist(int idx){ |
322 |
return inst_tdist[idx];
|
323 |
} |
324 |
|
325 |
void clearDistTable(int idx){ |
326 |
inst_idistraw[idx] = 0;
|
327 |
inst_idist[idx] = 0;
|
328 |
} |
329 |
|
330 |
void instancecleardisttableall(void){ |
331 |
int i;
|
332 |
|
333 |
for(i=0; i<MAX_ANCHOR_LIST_SIZE; i++) { |
334 |
inst_idistraw[i] = 0xffff;
|
335 |
inst_idist[i] = 0xffff;
|
336 |
} |
337 |
} |
338 |
|
339 |
// -------------------------------------------------------------------------------------------------------------------
|
340 |
#if NUM_INST != 1 |
341 |
#error These functions assume one instance only
|
342 |
#else
|
343 |
|
344 |
|
345 |
// -------------------------------------------------------------------------------------------------------------------
|
346 |
// Set this instance role as the Tag, Anchor or Listener
|
347 |
void instancesetrole(int inst_mode){ |
348 |
// assume instance 0, for this
|
349 |
instance_data[0].mode = (INST_MODE)inst_mode; // set the role |
350 |
} |
351 |
|
352 |
int instancegetrole(void){ |
353 |
return (int)instance_data[0].mode; |
354 |
} |
355 |
|
356 |
int instancenewrange(void){ |
357 |
int x = instance_data[0].newRange; |
358 |
instance_data[0].newRange = TOF_REPORT_NUL;
|
359 |
return x;
|
360 |
} |
361 |
|
362 |
int instancenewrangeancadd(void){ |
363 |
return instance_data[0].newRangeAncAddress; |
364 |
} |
365 |
|
366 |
int instancenewrangetagadd(void){ |
367 |
return instance_data[0].newRangeTagAddress; |
368 |
} |
369 |
|
370 |
uint32_t instancenewrangetim(void){
|
371 |
return instance_data[0].newRangeTime; |
372 |
} |
373 |
|
374 |
// -------------------------------------------------------------------------------------------------------------------
|
375 |
// function to clear counts/averages/range values
|
376 |
//
|
377 |
void instanceclearcounts(void){ |
378 |
int instance = 0 ; |
379 |
int i= 0 ; |
380 |
|
381 |
instance_data[instance].rxTimeouts = 0 ;
|
382 |
|
383 |
dwt_configeventcounters(1); //enable and clear - NOTE: the counters are not preserved when in DEEP SLEEP |
384 |
|
385 |
instance_data[instance].frameSN = 0;
|
386 |
|
387 |
instance_data[instance].longTermRangeCount = 0;
|
388 |
|
389 |
|
390 |
for(i=0; i<MAX_ANCHOR_LIST_SIZE; i++){ |
391 |
instance_data[instance].tofArray[i] = INVALID_TOF; |
392 |
} |
393 |
|
394 |
for(i=0; i<MAX_TAG_LIST_SIZE; i++){ |
395 |
instance_data[instance].tof[i] = INVALID_TOF; |
396 |
} |
397 |
|
398 |
} // end instanceclearcounts()
|
399 |
|
400 |
|
401 |
// -------------------------------------------------------------------------------------------------------------------
|
402 |
// function to initialise instance structures
|
403 |
//
|
404 |
// Returns 0 on success and -1 on error
|
405 |
int instance_init(DW1000Driver* drv){
|
406 |
int instance = 0 ; |
407 |
int i;
|
408 |
int result;
|
409 |
|
410 |
instance_data[instance].mode = ANCHOR; // assume listener,
|
411 |
instance_data[instance].testAppState = TA_INIT ; |
412 |
instance_data[instance].instToSleep = FALSE; |
413 |
|
414 |
|
415 |
// Reset the IC (might be needed if not getting here from POWER ON)
|
416 |
// ARM code: Remove soft reset here as using hard reset in the inittestapplication() in the main.c file
|
417 |
//dwt_softreset();
|
418 |
|
419 |
//this initialises DW1000 and uses specified configurations from OTP/ROM
|
420 |
result = dwt_initialise(DWT_LOADUCODE, drv); |
421 |
|
422 |
//this is platform dependent - only program if DW EVK/EVB
|
423 |
dwt_setleds(3) ; //configure the GPIOs which control the leds on EVBs |
424 |
|
425 |
if (DWT_SUCCESS != result){
|
426 |
return (-1) ; // device initialise has failed |
427 |
} |
428 |
|
429 |
|
430 |
instanceclearcounts() ; |
431 |
|
432 |
instance_data[instance].panID = 0xdeca ;
|
433 |
|
434 |
instance_data[instance].wait4ack = 0;
|
435 |
instance_data[instance].stopTimer = 0;
|
436 |
instance_data[instance].instanceTimerEn = 0;
|
437 |
|
438 |
instance_clearevents(); |
439 |
|
440 |
//dwt_geteui(instance_data[instance].eui64);
|
441 |
memset(instance_data[instance].eui64, 0, ADDR_BYTE_SIZE_L);
|
442 |
|
443 |
instance_data[instance].tagSleepCorrection = 0;
|
444 |
|
445 |
// dwt_setautorxreenable(0); //disable auto RX re-enable
|
446 |
dwt_setdblrxbuffmode(0); //disable double RX buffer |
447 |
|
448 |
// if using auto CRC check (DWT_INT_RFCG and DWT_INT_RFCE) are used instead of DWT_INT_RDFR flag
|
449 |
// other errors which need to be checked (as they disable receiver) are
|
450 |
//dwt_setinterrupt(DWT_INT_TFRS | DWT_INT_RFCG | (DWT_INT_SFDT | DWT_INT_RFTO /*| DWT_INT_RXPTO*/), 1);
|
451 |
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);
|
452 |
|
453 |
dwt_setcallbacks(instance_txcallback, instance_rxcallback, instance_rxtimeoutcallback,instance_rxerrorcallback ); |
454 |
|
455 |
instance_data[instance].monitor = 0;
|
456 |
|
457 |
instance_data[instance].lateTX = 0;
|
458 |
instance_data[instance].lateRX = 0;
|
459 |
|
460 |
instance_data[instance].responseTO = -1; //initialise |
461 |
for(i=0; i<256; i++) { |
462 |
instance_data[instance].rxResps[i] = -10;
|
463 |
} |
464 |
|
465 |
instance_data[instance].delayedReplyTime = 0;
|
466 |
|
467 |
return 0 ; |
468 |
} |
469 |
|
470 |
// -------------------------------------------------------------------------------------------------------------------
|
471 |
//
|
472 |
// Return the Device ID register value, enables higher level validation of physical device presence
|
473 |
//
|
474 |
|
475 |
uint32_t instancereaddeviceid(void){
|
476 |
return dwt_readdevid() ;
|
477 |
} |
478 |
|
479 |
|
480 |
// -------------------------------------------------------------------------------------------------------------------
|
481 |
//
|
482 |
// function to allow application configuration be passed into instance and affect underlying device operation
|
483 |
//
|
484 |
void instance_config(instanceConfig_t *config, sfConfig_t *sfConfig, DW1000Driver* drv){
|
485 |
int instance = 0 ; |
486 |
uint32_t power = 0;
|
487 |
uint8_t otprev ; |
488 |
|
489 |
instance_data[instance].configData.chan = config->channelNumber ; |
490 |
instance_data[instance].configData.rxCode = config->preambleCode ; |
491 |
instance_data[instance].configData.txCode = config->preambleCode ; |
492 |
instance_data[instance].configData.prf = config->pulseRepFreq ; |
493 |
instance_data[instance].configData.dataRate = config->dataRate ; |
494 |
instance_data[instance].configData.txPreambLength = config->preambleLen ; |
495 |
instance_data[instance].configData.rxPAC = config->pacSize ; |
496 |
instance_data[instance].configData.nsSFD = config->nsSFD ; |
497 |
instance_data[instance].configData.phrMode = DWT_PHRMODE_STD ; |
498 |
instance_data[instance].configData.sfdTO = config->sfdTO; |
499 |
|
500 |
//the DW1000 will automatically use gating gain for frames < 1ms duration (i.e. 6.81Mbps data rate)
|
501 |
//smartPowerEn should be set based on the frame length, but we can also use dtaa rate.
|
502 |
if(instance_data[instance].configData.dataRate == DWT_BR_6M8) {
|
503 |
instance_data[instance].configData.smartPowerEn = 1;
|
504 |
} |
505 |
else{
|
506 |
instance_data[instance].configData.smartPowerEn = 0;
|
507 |
} |
508 |
|
509 |
//configure the channel parameters
|
510 |
dwt_configure(&instance_data[instance].configData) ; |
511 |
|
512 |
instance_data[instance].configTX.PGdly = txSpectrumConfig[config->channelNumber].PGdelay ; //Default
|
513 |
|
514 |
//firstly check if there are calibrated TX power value in the DW1000 OTP
|
515 |
power = dwt_getotptxpower(config->pulseRepFreq, instance_data[instance].configData.chan); |
516 |
|
517 |
if((power == 0x0) || (power == 0xFFFFFFFF)) { //if there are no calibrated values... need to use defaults |
518 |
power = txSpectrumConfig[config->channelNumber].txPwr[config->pulseRepFreq- DWT_PRF_16M]; |
519 |
} |
520 |
|
521 |
//Configure TX power
|
522 |
instance_data[instance].configTX.power = power; |
523 |
|
524 |
//configure the tx spectrum parameters (power and PG delay)
|
525 |
dwt_configuretxrf(&instance_data[instance].configTX); |
526 |
|
527 |
otprev = dwt_otprevision() ; // this revision tells us how OTP is programmed.
|
528 |
|
529 |
if ((2 == otprev) || (3 == otprev)) { // board is calibrated with TREK1000 with antenna delays set for each use case) |
530 |
uint8_t mode = (instance_data[instance].mode == ANCHOR ? 1 : 0); |
531 |
uint8_t chanindex = 0;
|
532 |
|
533 |
instance_data[instance].txAntennaDelay |
534 |
= dwt_getTREKOTPantennadelay(mode, |
535 |
instance_data[instance].configData.chan, |
536 |
instance_data[instance].configData.dataRate) ; |
537 |
|
538 |
// if nothing was actually programmed then set a reasonable value anyway
|
539 |
if ((instance_data[instance].txAntennaDelay == 0) |
540 |
|| (instance_data[instance].txAntennaDelay == 0xffff)){
|
541 |
if(instance_data[instance].configData.chan == 5){ |
542 |
chanindex = 1;
|
543 |
} |
544 |
|
545 |
instance_data[instance].txAntennaDelay = rfDelaysTREK[chanindex]; |
546 |
} |
547 |
|
548 |
} |
549 |
else { // assume it is older EVK1000 programming. |
550 |
//get the antenna delay that was read from the OTP calibration area
|
551 |
instance_data[instance].txAntennaDelay = dwt_readantennadelay(config->pulseRepFreq) >> 1;
|
552 |
|
553 |
// if nothing was actually programmed then set a reasonable value anyway
|
554 |
if ((instance_data[instance].txAntennaDelay == 0) |
555 |
|| (instance_data[instance].txAntennaDelay == 0xffff)){
|
556 |
instance_data[instance].txAntennaDelay = rfDelays[config->pulseRepFreq - DWT_PRF_16M]; |
557 |
} |
558 |
} |
559 |
|
560 |
// -------------------------------------------------------------------------------------------------------------------
|
561 |
// set the antenna delay, we assume that the RX is the same as TX.
|
562 |
dwt_setrxantennadelay(instance_data[instance].txAntennaDelay); |
563 |
dwt_settxantennadelay(instance_data[instance].txAntennaDelay); |
564 |
|
565 |
instance_data[instance].rxAntennaDelay = instance_data[instance].txAntennaDelay; |
566 |
|
567 |
if(config->preambleLen == DWT_PLEN_64) { //if preamble length is 64 |
568 |
//reduce SPI to < 3MHz
|
569 |
setHighSpeed_SPI(FALSE, drv); |
570 |
dwt_loadopsettabfromotp(0);
|
571 |
//increase SPI to max
|
572 |
setHighSpeed_SPI(TRUE, drv); |
573 |
} |
574 |
|
575 |
instancesettagsleepdelay(sfConfig->pollSleepDly); //set the Tag sleep time
|
576 |
instance_data[instance].sframePeriod = sfConfig->sfPeriod; |
577 |
instance_data[instance].slotPeriod = sfConfig->slotPeriod; |
578 |
instance_data[instance].tagSleepRnd = sfConfig->slotPeriod; |
579 |
instance_data[instance].numSlots = sfConfig->numSlots; |
580 |
|
581 |
//last two slots are used for anchor to anchor ranging
|
582 |
instance_data[instance].a0SlotTime = (instance_data[instance].numSlots-2) * instance_data[instance].slotPeriod;
|
583 |
|
584 |
//set the default response delays
|
585 |
instancesetreplydelay(sfConfig->replyDly); |
586 |
|
587 |
} |
588 |
|
589 |
// -------------------------------------------------------------------------------------------------------------------
|
590 |
// function to set the tag sleep time (in ms)
|
591 |
//
|
592 |
void instancesettagsleepdelay(int sleepdelay) { //sleep in ms |
593 |
int instance = 0 ; |
594 |
instance_data[instance].tagSleepTime_ms = sleepdelay; //subtract the micro system delays (time it takes to switch states etc.)
|
595 |
} |
596 |
|
597 |
|
598 |
int instancegetrnum(void) { //get ranging number |
599 |
return instance_data[0].rangeNum; |
600 |
} |
601 |
|
602 |
int instancegetrnuma(int idx){ //get ranging number |
603 |
return instance_data[0].rangeNumA[idx]; |
604 |
} |
605 |
|
606 |
int instancegetrnumanc(int idx){ //get ranging number |
607 |
return instance_data[0].rangeNumAAnc[idx]; |
608 |
} |
609 |
|
610 |
int instancegetlcount(void) { //get count of ranges used for calculation of lt avg |
611 |
int x = instance_data[0].longTermRangeCount; |
612 |
|
613 |
return (x);
|
614 |
} |
615 |
|
616 |
double instancegetidist(int idx) { //get instantaneous range |
617 |
double x ;
|
618 |
|
619 |
idx &= (MAX_ANCHOR_LIST_SIZE - 1);
|
620 |
|
621 |
x = inst_idist[idx]; |
622 |
|
623 |
return (x);
|
624 |
} |
625 |
|
626 |
double instancegetidistraw(int idx) { //get instantaneous range (uncorrected) |
627 |
double x ;
|
628 |
|
629 |
idx &= (MAX_ANCHOR_LIST_SIZE - 1);
|
630 |
|
631 |
x = inst_idistraw[idx]; |
632 |
|
633 |
return (x);
|
634 |
} |
635 |
|
636 |
int instancegetidist_mm(int idx) { //get instantaneous range |
637 |
int x ;
|
638 |
|
639 |
idx &= (MAX_ANCHOR_LIST_SIZE - 1);
|
640 |
|
641 |
x = (int)(inst_idist[idx]*1000); |
642 |
|
643 |
return (x);
|
644 |
} |
645 |
|
646 |
int instancegetidistraw_mm(int idx) { //get instantaneous range (uncorrected) |
647 |
int x ;
|
648 |
|
649 |
idx &= (MAX_ANCHOR_LIST_SIZE - 1);
|
650 |
|
651 |
x = (int)(inst_idistraw[idx]*1000); |
652 |
|
653 |
return (x);
|
654 |
} |
655 |
|
656 |
void instance_backtoanchor(instance_data_t *inst){
|
657 |
//stay in RX and behave as anchor
|
658 |
inst->testAppState = TA_RXE_WAIT ; |
659 |
inst->mode = ANCHOR ; |
660 |
dwt_setrxtimeout(0);
|
661 |
dwt_setpreambledetecttimeout(0);
|
662 |
dwt_setrxaftertxdelay(0);
|
663 |
} |
664 |
|
665 |
|
666 |
#pragma GCC optimize ("O3") |
667 |
void inst_processrxtimeout(instance_data_t *inst){
|
668 |
|
669 |
//inst->responseTimeouts ++ ;
|
670 |
inst->rxTimeouts ++ ; |
671 |
inst->done = INST_NOT_DONE_YET; |
672 |
|
673 |
if(inst->mode == ANCHOR) { //we did not receive the final - wait for next poll |
674 |
//only enable receiver when not using double buffering
|
675 |
inst->testAppState = TA_RXE_WAIT ; // wait for next frame
|
676 |
dwt_setrxtimeout(0);
|
677 |
} |
678 |
|
679 |
else if(inst->mode == TAG) { |
680 |
//if tag times out - no response (check if we are to send a final)
|
681 |
//send the final only if it has received response from anchor 0
|
682 |
if((inst->previousState == TA_TXPOLL_WAIT_SEND) && ((inst->rxResponseMask & 0x1) == 0)) { |
683 |
inst->instToSleep = TRUE ; //set sleep to TRUE so that tag will go to DEEP SLEEP before next ranging attempt
|
684 |
inst->testAppState = TA_TXE_WAIT ; |
685 |
inst->nextState = TA_TXPOLL_WAIT_SEND ; |
686 |
} |
687 |
else if (inst->previousState == TA_TXFINAL_WAIT_SEND) { //got here from main (error sending final - handle as timeout) |
688 |
dwt_forcetrxoff(); //this will clear all events
|
689 |
inst->instToSleep = TRUE ; |
690 |
// initiate the re-transmission of the poll that was not responded to
|
691 |
inst->testAppState = TA_TXE_WAIT ; |
692 |
inst->nextState = TA_TXPOLL_WAIT_SEND ; |
693 |
} |
694 |
else { //send the final |
695 |
// initiate the re-transmission of the poll that was not responded to
|
696 |
inst->testAppState = TA_TXE_WAIT ; |
697 |
inst->nextState = TA_TXFINAL_WAIT_SEND ; |
698 |
} |
699 |
|
700 |
} |
701 |
else { //ANCHOR_RNG |
702 |
//no Response form the other anchor
|
703 |
if(
|
704 |
((inst->previousState == TA_TXPOLL_WAIT_SEND) |
705 |
&& ((A1_ANCHOR_ADDR == inst->instanceAddress16) && ((inst->rxResponseMaskAnc & 0x4) == 0))) |
706 |
|| |
707 |
((inst->previousState == TA_TXPOLL_WAIT_SEND) |
708 |
&& ((GATEWAY_ANCHOR_ADDR == inst->instanceAddress16) && ((inst->rxResponseMaskAnc & 0x2) == 0))) |
709 |
) { |
710 |
instance_backtoanchor(inst); |
711 |
} |
712 |
else if (inst->previousState == TA_TXFINAL_WAIT_SEND) { //got here from main (error ending final - handle as timeout) |
713 |
instance_backtoanchor(inst); |
714 |
} |
715 |
else { //send the final |
716 |
// initiate the re-transmission of the poll that was not responded to
|
717 |
inst->testAppState = TA_TXE_WAIT ; |
718 |
inst->nextState = TA_TXFINAL_WAIT_SEND ; |
719 |
} |
720 |
} |
721 |
|
722 |
//timeout - disable the radio (if using SW timeout the rx will not be off)
|
723 |
//dwt_forcetrxoff() ;
|
724 |
} |
725 |
|
726 |
//
|
727 |
// NB: This function is called from the (TX) interrupt handler
|
728 |
//
|
729 |
#pragma GCC optimize ("O3") |
730 |
void instance_txcallback(const dwt_cb_data_t *txd){ |
731 |
|
732 |
(void) txd;
|
733 |
int instance = 0; |
734 |
uint8_t txTimeStamp[5] = {0, 0, 0, 0, 0}; |
735 |
// uint8 txevent = txd->event;
|
736 |
event_data_t dw_event; |
737 |
|
738 |
dw_event.uTimeStamp = portGetTickCnt(); |
739 |
|
740 |
dwt_readtxtimestamp(txTimeStamp) ; |
741 |
dw_event.timeStamp32l = (uint32_t)txTimeStamp[0] + ((uint32_t)txTimeStamp[1] << 8) + ((uint32_t)txTimeStamp[2] << 16) + ((uint32_t)txTimeStamp[3] << 24); |
742 |
dw_event.timeStamp = txTimeStamp[4];
|
743 |
dw_event.timeStamp <<= 32;
|
744 |
dw_event.timeStamp += dw_event.timeStamp32l; |
745 |
dw_event.timeStamp32h = ((uint32_t)txTimeStamp[4] << 24) + (dw_event.timeStamp32l >> 8); |
746 |
|
747 |
instance_data[instance].stopTimer = 0;
|
748 |
|
749 |
dw_event.rxLength = instance_data[instance].psduLength; |
750 |
dw_event.type = 0;
|
751 |
dw_event.type_pend = 0;
|
752 |
dw_event.type_save = DWT_SIG_TX_DONE; |
753 |
|
754 |
memcpy((uint8_t *)&dw_event.msgu.frame[0], (uint8_t *)&instance_data[instance].msg_f, instance_data[instance].psduLength);
|
755 |
|
756 |
instance_putevent(dw_event, DWT_SIG_TX_DONE); |
757 |
|
758 |
instance_data[instance].txMsgCount++; |
759 |
|
760 |
instance_data[instance].monitor = 0;
|
761 |
} |
762 |
|
763 |
/**
|
764 |
* @brief function to re-enable the receiver and also adjust the timeout before sending the final message
|
765 |
* if it is time so send the final message, the callback will notify the application, else the receiver is
|
766 |
* automatically re-enabled
|
767 |
*
|
768 |
* this function is only used for tag when ranging to other anchors
|
769 |
*/
|
770 |
uint8_t tagrxreenable(uint16_t sourceAddress){ |
771 |
uint8_t type_pend = DWT_SIG_DW_IDLE; |
772 |
uint8_t anc = sourceAddress & 0x3;
|
773 |
int instance = 0; |
774 |
|
775 |
switch(anc){
|
776 |
//if we got Response from anchor 3 - this is the last expected response - send the final
|
777 |
case 3: |
778 |
type_pend = DWT_SIG_DW_IDLE; |
779 |
break;
|
780 |
|
781 |
//if we got Response from anchor 0, 1, or 2 - go back to wait for next anchor's response
|
782 |
case 0: |
783 |
case 1: |
784 |
case 2: |
785 |
default:
|
786 |
if(instance_data[instance].responseTO > 0) { //can get here as result of error frame so need to check |
787 |
dwt_setrxtimeout((uint16_t)(instance_data[instance].fwtoTime_sy * instance_data[instance].responseTO)); //reconfigure the timeout
|
788 |
dwt_rxenable(DWT_START_RX_IMMEDIATE) ; |
789 |
type_pend = DWT_SIG_RX_PENDING ; |
790 |
} |
791 |
else //last response was not received (got error/frame was corrupt) |
792 |
{ |
793 |
type_pend = DWT_SIG_DW_IDLE; //report timeout - send the final
|
794 |
} |
795 |
break;
|
796 |
} |
797 |
|
798 |
return type_pend;
|
799 |
} |
800 |
|
801 |
/**
|
802 |
* @brief function to re-enable the receiver and also adjust the timeout before sending the final message
|
803 |
* if it is time so send the final message, the callback will notify the application, else the receiver is
|
804 |
* automatically re-enabled
|
805 |
*
|
806 |
* this function is only used for anchors (having a role of ANCHOR_RNG) when ranging to other anchors
|
807 |
*/
|
808 |
uint8_t ancsendfinalorrxreenable(uint16_t sourceAddress){ |
809 |
uint8_t type_pend = DWT_SIG_DW_IDLE; |
810 |
uint8_t anc = sourceAddress & 0x3;
|
811 |
int instance = 0; |
812 |
|
813 |
if(instance_data[instance].instanceAddress16 == GATEWAY_ANCHOR_ADDR) {
|
814 |
switch(anc) {
|
815 |
//if we got Response from anchor 1 - go back to wait for next anchor's response
|
816 |
case 1: |
817 |
dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTime_sy); //reconfigure the timeout
|
818 |
dwt_rxenable(DWT_START_RX_IMMEDIATE) ; |
819 |
type_pend = DWT_SIG_RX_PENDING ; |
820 |
break;
|
821 |
|
822 |
//if we got Response from anchor 2 - this is the last expected response - send the final
|
823 |
case 2: |
824 |
default:
|
825 |
type_pend = DWT_SIG_DW_IDLE; |
826 |
break;
|
827 |
} |
828 |
} |
829 |
|
830 |
if(instance_data[instance].instanceAddress16 == A1_ANCHOR_ADDR){
|
831 |
switch(anc)
|
832 |
{ |
833 |
//if we got Response from anchor 2 - this is the last expected response - send the final
|
834 |
case 2: |
835 |
default:
|
836 |
type_pend = DWT_SIG_DW_IDLE; |
837 |
break;
|
838 |
} |
839 |
} |
840 |
return type_pend;
|
841 |
} |
842 |
|
843 |
/**
|
844 |
* @brief this function either enables the receiver (delayed)
|
845 |
*
|
846 |
**/
|
847 |
void ancenablerx(void){ |
848 |
int instance = 0; |
849 |
//subtract preamble length
|
850 |
dwt_setdelayedtrxtime(instance_data[instance].delayedReplyTime - instance_data[instance].fixedReplyDelayAncP) ; |
851 |
if(dwt_rxenable(DWT_START_RX_DELAYED)) { //delayed rx |
852 |
//if the delayed RX failed - time has passed - do immediate enable
|
853 |
//led_on(LED_PC9);
|
854 |
dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTimeAnc_sy*2); //reconfigure the timeout before enable |
855 |
//longer timeout as we cannot do delayed receive... so receiver needs to stay on for longer
|
856 |
dwt_rxenable(DWT_START_RX_IMMEDIATE); |
857 |
dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTimeAnc_sy); //restore the timeout for next RX enable
|
858 |
instance_data[instance].lateRX++; |
859 |
//led_off(LED_PC9);
|
860 |
} |
861 |
|
862 |
} |
863 |
|
864 |
/**
|
865 |
* @brief this function either re-enables the receiver (delayed or immediate) or transmits the response frame
|
866 |
*
|
867 |
* @param the sourceAddress is the address of the sender of the current received frame
|
868 |
* @param ancToAncTWR == 1 means that the anchor is ranging to another anchor, if == 0 then ranging to a tag
|
869 |
*
|
870 |
*/
|
871 |
#pragma GCC optimize ("O0") |
872 |
uint8_t anctxorrxreenable(uint16_t sourceAddress, int ancToAncTWR){
|
873 |
uint8_t type_pend = DWT_SIG_DW_IDLE; |
874 |
int sendResp = 0; |
875 |
int instance = 0; |
876 |
|
877 |
if(instance_data[instance].responseTO == 0) { //go back to RX without TO - ranging has finished. (wait for Final but no TO) |
878 |
dwt_setrxtimeout(0); //reconfigure the timeout |
879 |
dwt_setpreambledetecttimeout(0);
|
880 |
} |
881 |
|
882 |
if((ancToAncTWR & 1) == 1) { |
883 |
if(instance_data[instance].responseTO == 1) { //if one response left to receive (send a response now) |
884 |
sendResp = 1;
|
885 |
} |
886 |
//if A0 or A3 go back to RX as they do not send any responses when Anchor to Anchor ranging
|
887 |
if((instance_data[instance].gatewayAnchor)
|
888 |
|| (instance_data[instance].shortAdd_idx == 3)) { //if this is anchor ID 3 do not reply to anchor poll |
889 |
dwt_setrxtimeout(0);
|
890 |
dwt_rxenable(DWT_START_RX_IMMEDIATE); |
891 |
return DWT_SIG_RX_PENDING ;
|
892 |
} |
893 |
} |
894 |
|
895 |
//configure delayed reply time (this is incremented for each received frame) it is timed from Poll rx time
|
896 |
instance_data[instance].delayedReplyTime += (instance_data[instance].fixedReplyDelayAnc >> 8);
|
897 |
|
898 |
//this checks if to send a frame
|
899 |
if((((ancToAncTWR & 1) == 0) && ((instance_data[instance].responseTO + instance_data[instance].shortAdd_idx) == NUM_EXPECTED_RESPONSES)) //it's our turn to tx |
900 |
|| (sendResp == 1)) {
|
901 |
//led_on(LED_PC9);
|
902 |
//response is expected
|
903 |
instance_data[instance].wait4ack = DWT_RESPONSE_EXPECTED; //re has/will be re-enabled
|
904 |
|
905 |
dwt_setdelayedtrxtime(instance_data[instance].delayedReplyTime) ; |
906 |
if(dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED)) {
|
907 |
//if TX has failed - we need to re-enable RX for the next response or final reception...
|
908 |
dwt_setrxaftertxdelay(0);
|
909 |
instance_data[instance].wait4ack = 0; //clear the flag as the TX has failed the TRX is off |
910 |
instance_data[instance].lateTX++; |
911 |
instance_data[instance].delayedReplyTime += 2*(instance_data[instance].fixedReplyDelayAnc >> 8); //to take into account W4R |
912 |
ancenablerx(); |
913 |
type_pend = DWT_SIG_RX_PENDING ; |
914 |
} |
915 |
else {
|
916 |
instance_data[instance].delayedReplyTime += (instance_data[instance].fixedReplyDelayAnc >> 8); //to take into account W4R |
917 |
type_pend = DWT_SIG_TX_PENDING ; // exit this interrupt and notify the application/instance that TX is in progress.
|
918 |
instance_data[instance].timeofTx = portGetTickCnt(); |
919 |
instance_data[instance].monitor = 1;
|
920 |
} |
921 |
//led_off(LED_PC9);
|
922 |
} |
923 |
else { //stay in receive |
924 |
if(sourceAddress == 0) { //we got here after RX error, as we don't need to TX, we just enable RX |
925 |
dwt_setrxtimeout(0);
|
926 |
dwt_rxenable(DWT_START_RX_IMMEDIATE); |
927 |
} |
928 |
else{
|
929 |
//led_on(LED_PC9);
|
930 |
ancenablerx(); |
931 |
//led_off(LED_PC9);
|
932 |
} |
933 |
|
934 |
type_pend = DWT_SIG_RX_PENDING ; |
935 |
} |
936 |
//if time to send a response
|
937 |
|
938 |
return type_pend;
|
939 |
} |
940 |
|
941 |
/**
|
942 |
* @brief this function handles frame error event, it will either signal TO or re-enable the receiver
|
943 |
*/
|
944 |
void handle_error_unknownframe(event_data_t dw_event){
|
945 |
int instance = 0; |
946 |
//re-enable the receiver (after error frames as we are not using auto re-enable
|
947 |
//for ranging application rx error frame is same as TO - as we are not going to get the expected frame
|
948 |
if(instance_data[instance].mode == ANCHOR){
|
949 |
//if we are participating in the ranging (i.e. Poll was received)
|
950 |
//and we get an rx error (in one of the responses)
|
951 |
//need to consider this as a timeout as we could be sending our response next and
|
952 |
//the applications needs to know to change the state
|
953 |
//
|
954 |
if(instance_data[instance].responseTO > 0){ |
955 |
instance_data[instance].responseTO--; |
956 |
|
957 |
//send a response or re-enable rx
|
958 |
dw_event.type_pend = anctxorrxreenable(0, 0); |
959 |
dw_event.type = 0;
|
960 |
dw_event.type_save = 0x40 | DWT_SIG_RX_TIMEOUT;
|
961 |
dw_event.rxLength = 0;
|
962 |
|
963 |
instance_putevent(dw_event, DWT_SIG_RX_TIMEOUT); |
964 |
} |
965 |
else{
|
966 |
dwt_setrxtimeout(0); //reconfigure the timeout |
967 |
dwt_rxenable(DWT_START_RX_IMMEDIATE) ; |
968 |
} |
969 |
} |
970 |
else if(instance_data[instance].mode == LISTENER){ |
971 |
dwt_rxenable(DWT_START_RX_IMMEDIATE) ; |
972 |
} |
973 |
else{
|
974 |
instance_data[instance].responseTO--; //got something (need to reduce timeout (for remaining responses))
|
975 |
|
976 |
dw_event.type_pend = tagrxreenable(0); //check if receiver will be re-enabled or it's time to send the final |
977 |
dw_event.type = 0;
|
978 |
dw_event.type_save = 0x40 | DWT_SIG_RX_TIMEOUT;
|
979 |
dw_event.rxLength = 0;
|
980 |
|
981 |
instance_putevent(dw_event, DWT_SIG_RX_TIMEOUT); |
982 |
} |
983 |
} |
984 |
|
985 |
|
986 |
/**
|
987 |
* @brief this function prepares and writes the anchor to anchor response frame into the TX buffer
|
988 |
* it is called after anchor receives a Poll from an anchor
|
989 |
*/
|
990 |
void ancprepareresponse2(uint16_t sourceAddress, uint8_t srcAddr_index, uint8_t fcode_index, uint8_t *frame){
|
991 |
uint16_t frameLength = 0;
|
992 |
uint8_t tof_idx = (sourceAddress) & 0x3 ;
|
993 |
int instance = 0; |
994 |
|
995 |
instance_data[instance].psduLength = frameLength = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC; |
996 |
//set the destination address (copy source as this is a reply)
|
997 |
memcpy(&instance_data[instance].msg_f.destAddr[0], &frame[srcAddr_index], ADDR_BYTE_SIZE_S); //remember who to send the reply to (set destination address) |
998 |
instance_data[instance].msg_f.sourceAddr[0] = instance_data[instance].eui64[0]; |
999 |
instance_data[instance].msg_f.sourceAddr[1] = instance_data[instance].eui64[1]; |
1000 |
// Write calculated TOF into response message (get the previous ToF+range number from that anchor)
|
1001 |
memcpy(&(instance_data[instance].msg_f.messageData[TOFR]), &instance_data[instance].tofAnc[tof_idx], 4);
|
1002 |
instance_data[instance].msg_f.messageData[TOFRN] = instance_data[instance].rangeNumAAnc[tof_idx]; //get the previous range number
|
1003 |
|
1004 |
instance_data[instance].rangeNumAAnc[tof_idx] = 0; //clear the entry |
1005 |
instance_data[instance].rangeNumAnc = frame[POLL_RNUM + fcode_index] ; |
1006 |
instance_data[instance].msg_f.seqNum = instance_data[instance].frameSN++; |
1007 |
|
1008 |
//set the delayed rx on time (the final message will be sent after this delay)
|
1009 |
dwt_setrxaftertxdelay((uint32_t)instance_data[instance].ancRespRxDelay); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
|
1010 |
|
1011 |
instance_data[instance].tagSleepCorrection = 0;
|
1012 |
instance_data[instance].msg_f.messageData[RES_TAG_SLP0] = 0 ;
|
1013 |
instance_data[instance].msg_f.messageData[RES_TAG_SLP1] = 0 ;
|
1014 |
|
1015 |
instance_data[instance].msg_f.messageData[FCODE] = RTLS_DEMO_MSG_ANCH_RESP2; //message function code (specifies if message is a poll, response or other...)
|
1016 |
|
1017 |
//write the TX data
|
1018 |
dwt_writetxfctrl(frameLength, 0, 1); |
1019 |
dwt_writetxdata(frameLength, (uint8_t *) &instance_data[instance].msg_f, 0) ; // write the frame data |
1020 |
|
1021 |
} |
1022 |
|
1023 |
/**
|
1024 |
* @brief this function prepares and writes the anchor to tag response frame into the TX buffer
|
1025 |
* it is called after anchor receives a Poll from a tag
|
1026 |
*/
|
1027 |
void ancprepareresponse(uint16_t sourceAddress, uint8_t srcAddr_index, uint8_t fcode_index, uint8_t *frame, uint32_t uTimeStamp){
|
1028 |
uint16_t frameLength = 0;
|
1029 |
uint8_t tof_idx = (sourceAddress) & 0x7 ;
|
1030 |
int instance = 0; |
1031 |
|
1032 |
instance_data[instance].psduLength = frameLength = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC; |
1033 |
memcpy(&instance_data[instance].msg_f.destAddr[0], &frame[srcAddr_index], ADDR_BYTE_SIZE_S); //remember who to send the reply to (set destination address) |
1034 |
instance_data[instance].msg_f.sourceAddr[0] = instance_data[instance].eui64[0]; |
1035 |
instance_data[instance].msg_f.sourceAddr[1] = instance_data[instance].eui64[1]; |
1036 |
// Write calculated TOF into response message (get the previous ToF+range number from that tag)
|
1037 |
memcpy(&(instance_data[instance].msg_f.messageData[TOFR]), &instance_data[instance].tof[tof_idx], 4);
|
1038 |
instance_data[instance].msg_f.messageData[TOFRN] = instance_data[instance].rangeNumA[tof_idx]; //get the previous range number
|
1039 |
|
1040 |
instance_data[instance].rangeNumA[tof_idx] = 0; //clear after copy above... |
1041 |
instance_data[instance].rangeNum = frame[POLL_RNUM+fcode_index] ; |
1042 |
instance_data[instance].msg_f.seqNum = instance_data[instance].frameSN++; |
1043 |
|
1044 |
//we have our range - update the own mask entry...
|
1045 |
if(instance_data[instance].tof[tof_idx] != INVALID_TOF) { //check the last ToF entry is valid and copy into the current array |
1046 |
instance_data[instance].rxResponseMask = (uint8_t)(0x1 << instance_data[instance].shortAdd_idx);
|
1047 |
instance_data[instance].tofArray[instance_data[instance].shortAdd_idx] = instance_data[instance].tof[tof_idx]; |
1048 |
} |
1049 |
else { //reset response mask |
1050 |
instance_data[instance].tofArray[instance_data[instance].shortAdd_idx] = INVALID_TOF ; |
1051 |
instance_data[instance].rxResponseMask = 0; //reset the mask of received responses when rx poll |
1052 |
} |
1053 |
//set the delayed rx on time (the final message will be sent after this delay)
|
1054 |
dwt_setrxaftertxdelay((uint32_t)instance_data[instance].ancRespRxDelay); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
|
1055 |
|
1056 |
//if this is gateway anchor - calculate the slot period correction
|
1057 |
if(instance_data[instance].gatewayAnchor) {
|
1058 |
int error = 0; |
1059 |
int currentSlotTime = 0; |
1060 |
int expectedSlotTime = 0; |
1061 |
//find the time in the current superframe
|
1062 |
currentSlotTime = uTimeStamp % instance_data[instance].sframePeriod; |
1063 |
|
1064 |
//this is the slot time the poll should be received in (Mask 0x07 for the 8 MAX tags we support in TREK)
|
1065 |
expectedSlotTime = (sourceAddress&0xFF) * instance_data[instance].slotPeriod; // |
1066 |
|
1067 |
//error = expectedSlotTime - currentSlotTime
|
1068 |
error = expectedSlotTime - currentSlotTime; |
1069 |
|
1070 |
if(error < (-(instance_data[instance].sframePeriod>>1))) { //if error is more negative than 0.5 period, add whole period to give up to 1.5 period sleep |
1071 |
instance_data[instance].tagSleepCorrection = (instance_data[instance].sframePeriod + error); |
1072 |
} |
1073 |
else { //the minimum Sleep time will be 0.5 period |
1074 |
instance_data[instance].tagSleepCorrection = error; |
1075 |
} |
1076 |
instance_data[instance].msg_f.messageData[RES_TAG_SLP0] = instance_data[instance].tagSleepCorrection & 0xFF ;
|
1077 |
instance_data[instance].msg_f.messageData[RES_TAG_SLP1] = (instance_data[instance].tagSleepCorrection >> 8) & 0xFF; |
1078 |
} |
1079 |
else {
|
1080 |
instance_data[instance].tagSleepCorrection = 0;
|
1081 |
instance_data[instance].msg_f.messageData[RES_TAG_SLP0] = 0 ;
|
1082 |
instance_data[instance].msg_f.messageData[RES_TAG_SLP1] = 0 ;
|
1083 |
} |
1084 |
instance_data[instance].msg_f.messageData[FCODE] = RTLS_DEMO_MSG_ANCH_RESP; //message function code (specifies if message is a poll, response or other...)
|
1085 |
|
1086 |
//write the TX data
|
1087 |
dwt_writetxfctrl(frameLength, 0, 1); |
1088 |
dwt_writetxdata(frameLength, (uint8_t *) &instance_data[instance].msg_f, 0) ; // write the frame data |
1089 |
} |
1090 |
|
1091 |
/**
|
1092 |
* @brief this is the receive event callback handler, the received event is processed and the instance either
|
1093 |
* responds by sending a response frame or re-enables the receiver to await the next frame
|
1094 |
* once the immediate action is taken care of the event is queued up for application to process
|
1095 |
*/
|
1096 |
#pragma GCC optimize ("O3") |
1097 |
void instance_rxcallback(const dwt_cb_data_t *rxd){ |
1098 |
int instance = 0; |
1099 |
uint8_t rxTimeStamp[5] = {0, 0, 0, 0, 0}; |
1100 |
|
1101 |
uint8_t rxd_event = 0;
|
1102 |
uint8_t fcode_index = 0;
|
1103 |
uint8_t srcAddr_index = 0;
|
1104 |
event_data_t dw_event; |
1105 |
|
1106 |
//microcontroller time at which we received the frame
|
1107 |
dw_event.uTimeStamp = portGetTickCnt(); |
1108 |
|
1109 |
//if we got a frame with a good CRC - RX OK
|
1110 |
// if(rxd->event == DWT_SIG_RX_OKAY) { // Timeout and error are handle separately in the driver itself
|
1111 |
dw_event.rxLength = rxd->datalength; |
1112 |
|
1113 |
//need to process the frame control bytes to figure out what type of frame we have received
|
1114 |
if(((rxd->fctrl[0] == 0x41) || (rxd->fctrl[0] == 0x61)) |
1115 |
&& |
1116 |
((rxd->fctrl[1] & 0xCC) == 0x88)) { //short address |
1117 |
fcode_index = FRAME_CRTL_AND_ADDRESS_S; //function code is in first byte after source address
|
1118 |
srcAddr_index = FRAME_CTRLP + ADDR_BYTE_SIZE_S; |
1119 |
rxd_event = DWT_SIG_RX_OKAY; |
1120 |
} |
1121 |
else {
|
1122 |
rxd_event = SIG_RX_UNKNOWN; //not supported - all TREK1000 frames are short addressed
|
1123 |
} |
1124 |
|
1125 |
//read RX timestamp
|
1126 |
dwt_readrxtimestamp(rxTimeStamp) ; |
1127 |
dwt_readrxdata((uint8_t *)&dw_event.msgu.frame[0], rxd->datalength, 0); // Read Data Frame |
1128 |
dw_event.timeStamp32l = (uint32_t)rxTimeStamp[0] + ((uint32_t)rxTimeStamp[1] << 8) + ((uint32_t)rxTimeStamp[2] << 16) + ((uint32_t)rxTimeStamp[3] << 24); |
1129 |
dw_event.timeStamp = rxTimeStamp[4];
|
1130 |
dw_event.timeStamp <<= 32;
|
1131 |
dw_event.timeStamp += dw_event.timeStamp32l; |
1132 |
dw_event.timeStamp32h = ((uint32_t)rxTimeStamp[4] << 24) + (dw_event.timeStamp32l >> 8); |
1133 |
|
1134 |
dw_event.type = 0; //type will be added as part of adding to event queue |
1135 |
dw_event.type_save = rxd_event; |
1136 |
dw_event.type_pend = DWT_SIG_DW_IDLE; |
1137 |
|
1138 |
//if Listener then just report the received frame to the instance (application)
|
1139 |
if(rxd_event == DWT_SIG_RX_OKAY) { //Process good/known frame types |
1140 |
uint16_t sourceAddress = (uint16_t)(((uint16_t)dw_event.msgu.frame[srcAddr_index+1]) << 8) + (uint16_t)(dw_event.msgu.frame[srcAddr_index]); |
1141 |
|
1142 |
if(instance_data[instance].mode != LISTENER) {
|
1143 |
if(instance_data[instance].mode == TAG) //if tag got a good frame - this is probably a response, but could also be some other non-ranging frame |
1144 |
//(although due to frame filtering this is limited as non-addressed frames are filtered out)
|
1145 |
{ |
1146 |
instance_data[instance].responseTO--; //got 1 more response or other RX frame - need to reduce timeout (for next response)
|
1147 |
} |
1148 |
|
1149 |
//check if this is a TWR message (and also which one)
|
1150 |
switch(dw_event.msgu.frame[fcode_index]){
|
1151 |
//poll message from an anchor
|
1152 |
case RTLS_DEMO_MSG_ANCH_POLL:{
|
1153 |
//the anchor to anchor ranging poll frames are ignored by A0 and A3
|
1154 |
if(instance_data[instance].gatewayAnchor || (instance_data[instance].instanceAddress16 > A2_ANCHOR_ADDR)){
|
1155 |
//ignore poll from anchor 1 if gateway or anchor 3
|
1156 |
//anchors 2 and 3 will never send polls
|
1157 |
dw_event.type_pend = DWT_SIG_DW_IDLE ; |
1158 |
break;
|
1159 |
} |
1160 |
|
1161 |
if(instance_data[instance].mode == TAG) { //tag should ignore any other Polls from anchors |
1162 |
instance_data[instance].responseTO++; //as will be decremented in the function and was also decremented above
|
1163 |
handle_error_unknownframe(dw_event); |
1164 |
instance_data[instance].stopTimer = 1;
|
1165 |
instance_data[instance].rxMsgCount++; |
1166 |
return;
|
1167 |
} |
1168 |
|
1169 |
//update the response index and number of responses received tables
|
1170 |
instance_data[instance].rxRespsIdx = (uint8_t) ((dw_event.msgu.frame[POLL_RNUM+fcode_index] & 0xf)
|
1171 |
+ (((sourceAddress&0x3) + 8) << 4)); |
1172 |
instance_data[instance].rxResps[instance_data[instance].rxRespsIdx] = 0;
|
1173 |
//debug LED on
|
1174 |
// led_on(LED_PC9);
|
1175 |
|
1176 |
//prepare the response and write it to the tx buffer
|
1177 |
ancprepareresponse2(sourceAddress, srcAddr_index, fcode_index, &dw_event.msgu.frame[0]);
|
1178 |
|
1179 |
//A2 does not need timeout if ranging to A1
|
1180 |
if(sourceAddress != A1_ANCHOR_ADDR){
|
1181 |
dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTimeAnc_sy); //reconfigure the timeout for response
|
1182 |
} |
1183 |
|
1184 |
//set the bast reply time (the actual will be Poll rx time + instance_data[0].fixedReplyDelayAnc)
|
1185 |
instance_data[instance].delayedReplyTime = dw_event.timeStamp32h ; |
1186 |
instance_data[instance].responseTO = (instance_data[instance].instanceAddress16 - sourceAddress) & 0x3; //set number of expected responses |
1187 |
|
1188 |
dw_event.type_pend = anctxorrxreenable(instance_data[instance].instanceAddress16, 2+1); |
1189 |
|
1190 |
instance_data[instance].tofAnc[sourceAddress & 0x3] = INVALID_TOF; //clear ToF .. |
1191 |
//debug LED off
|
1192 |
// led_off(LED_PC9);
|
1193 |
break;
|
1194 |
} |
1195 |
|
1196 |
case RTLS_DEMO_MSG_TAG_POLL:{
|
1197 |
if(instance_data[instance].mode == TAG) { //tag should ignore any other Polls from tags |
1198 |
instance_data[instance].responseTO++; //as will be decremented in the function and was also decremented above
|
1199 |
handle_error_unknownframe(dw_event); |
1200 |
instance_data[instance].stopTimer = 1;
|
1201 |
instance_data[instance].rxMsgCount++; |
1202 |
return;
|
1203 |
} |
1204 |
instance_data[instance].rxRespsIdx = (uint8_t) ((dw_event.msgu.frame[POLL_RNUM+fcode_index] & 0xf)
|
1205 |
+ ((sourceAddress&0x7) << 4)); |
1206 |
instance_data[instance].rxResps[instance_data[instance].rxRespsIdx] = 0;
|
1207 |
|
1208 |
//prepare the response and write it to the tx buffer
|
1209 |
ancprepareresponse(sourceAddress, srcAddr_index, fcode_index, &dw_event.msgu.frame[0], dw_event.uTimeStamp);
|
1210 |
|
1211 |
dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTimeAnc_sy); //reconfigure the timeout for response
|
1212 |
|
1213 |
instance_data[0].delayedReplyTime = dw_event.timeStamp32h /*+ (instance_data[0].fixedReplyDelayAnc >> 8)*/ ; |
1214 |
instance_data[instance].responseTO = NUM_EXPECTED_RESPONSES; //set number of expected responses to 3 (from other anchors)
|
1215 |
|
1216 |
dw_event.type_pend = anctxorrxreenable(instance_data[instance].instanceAddress16, 2+0); |
1217 |
|
1218 |
instance_data[instance].tof[sourceAddress & 0x7] = INVALID_TOF; //clear ToF .. |
1219 |
} |
1220 |
break;
|
1221 |
|
1222 |
//we got a response from a "responder" (anchor)
|
1223 |
case RTLS_DEMO_MSG_ANCH_RESP:
|
1224 |
case RTLS_DEMO_MSG_ANCH_RESP2:{
|
1225 |
|
1226 |
//we are a tag
|
1227 |
if(instance_data[instance].mode == TAG){
|
1228 |
uint8_t index ; |
1229 |
instance_data[instance].rxResps[instance_data[instance].rangeNum]++; |
1230 |
dw_event.type_pend = tagrxreenable(sourceAddress); //responseTO decremented above...
|
1231 |
index = RRXT0 + 5*(sourceAddress & 0x3); |
1232 |
|
1233 |
instance_data[instance].rxResponseMask |= (0x1 << (sourceAddress & 0x3)); //add anchor ID to the mask |
1234 |
// Write Response RX time field of Final message
|
1235 |
memcpy(&(instance_data[instance].msg_f.messageData[index]), rxTimeStamp, 5);
|
1236 |
|
1237 |
} |
1238 |
else if (instance_data[instance].mode == ANCHOR_RNG) { //A0 and A1 only when ranging to other anchors |
1239 |
uint8_t index ; |
1240 |
instance_data[instance].rxResps[instance_data[instance].rangeNumAnc]++; |
1241 |
dw_event.type_pend = ancsendfinalorrxreenable(sourceAddress); |
1242 |
index = RRXT0 + 5*(sourceAddress & 0x3); |
1243 |
|
1244 |
instance_data[instance].rxResponseMaskAnc |= (0x1 << (sourceAddress & 0x3)); //add anchor ID to the mask |
1245 |
// Write Response RX time field of Final message
|
1246 |
memcpy(&(instance_data[instance].msg_f.messageData[index]), rxTimeStamp, 5);
|
1247 |
} |
1248 |
else { //normal anchor mode |
1249 |
//got a response... (check if we got a Poll with the same range number as in this response)
|
1250 |
if(RTLS_DEMO_MSG_ANCH_RESP == dw_event.msgu.frame[fcode_index]){
|
1251 |
if((instance_data[instance].rxResps[instance_data[instance].rxRespsIdx] >= 0) //we got the poll else ignore this response |
1252 |
&& (instance_data[instance].responseTO > 0) ) { //if responseTO == 0 we have already received all of the responses - meaning should not be here => error |
1253 |
instance_data[instance].rxResps[instance_data[instance].rxRespsIdx]++; //increment the number of responses received
|
1254 |
instance_data[instance].responseTO--; |
1255 |
|
1256 |
//send a response or re-enable rx
|
1257 |
dw_event.type_pend = anctxorrxreenable(sourceAddress, 4+0); |
1258 |
} |
1259 |
else { //like a timeout (error) ... |
1260 |
|
1261 |
//send a response or re-enable rx
|
1262 |
dwt_setrxtimeout(0); //reconfigure the timeout |
1263 |
dwt_rxenable(DWT_START_RX_IMMEDIATE) ; |
1264 |
dw_event.type_pend = DWT_SIG_RX_PENDING ; |
1265 |
} |
1266 |
} |
1267 |
else { //in anchor mode and got RTLS_DEMO_MSG_ANCH_RESP2 |
1268 |
if((instance_data[instance].gatewayAnchor) &&
|
1269 |
(instance_data[instance].rxResps[instance_data[instance].rangeNumAnc]) == 2){ //got two responses A1 and A2 this is third (A2's to A1) |
1270 |
instance_data[instance].rxResps[instance_data[instance].rangeNumAnc]++; |
1271 |
instance_data[instance].rxResponseMaskAnc |= 0x8 ;
|
1272 |
|
1273 |
dw_event.type_pend = anctxorrxreenable(sourceAddress, 4+1); //re-enable the RX |
1274 |
} |
1275 |
//A2 got A1's response to A0 - send A2 response (but only if we got the Poll from A0)
|
1276 |
else if((instance_data[instance].instanceAddress16 == A2_ANCHOR_ADDR) && |
1277 |
(instance_data[instance].rxResps[instance_data[instance].rxRespsIdx] >= 0) ){
|
1278 |
instance_data[instance].rxResps[instance_data[instance].rxRespsIdx]++; |
1279 |
instance_data[instance].responseTO--; |
1280 |
|
1281 |
dwt_setrxtimeout(0);
|
1282 |
dwt_setrxaftertxdelay(0); //clear rx on delay as Final will come sooner than if we were waiting for next Response |
1283 |
dw_event.type_pend = anctxorrxreenable(sourceAddress, 1);
|
1284 |
} |
1285 |
else { // if other anchor A1, A2, A3 .. ignore these responses when in ANCHOR mode |
1286 |
dwt_setrxtimeout(0); //reconfigure the timeout |
1287 |
dwt_rxenable(DWT_START_RX_IMMEDIATE) ; |
1288 |
dw_event.type_pend = DWT_SIG_RX_PENDING ; |
1289 |
} |
1290 |
} |
1291 |
} |
1292 |
|
1293 |
} |
1294 |
break;
|
1295 |
|
1296 |
case RTLS_DEMO_MSG_TAG_FINAL:
|
1297 |
case RTLS_DEMO_MSG_ANCH_FINAL:
|
1298 |
if(instance_data[instance].mode == TAG) { //tag should ignore any other Final from anchors |
1299 |
instance_data[instance].responseTO++; //as will be decremented in the function and was also decremented above
|
1300 |
handle_error_unknownframe(dw_event); |
1301 |
instance_data[instance].stopTimer = 1;
|
1302 |
instance_data[instance].rxMsgCount++; |
1303 |
return;
|
1304 |
} |
1305 |
break;
|
1306 |
// __attribute__ ((fallthrough));
|
1307 |
|
1308 |
//if anchor fall into case below and process the frame
|
1309 |
default: { //process rx frame |
1310 |
dw_event.type_pend = DWT_SIG_DW_IDLE; |
1311 |
} |
1312 |
break;
|
1313 |
|
1314 |
} |
1315 |
}//end of if not Listener mode
|
1316 |
instance_data[instance].stopTimer = 1;
|
1317 |
|
1318 |
instance_putevent(dw_event, rxd_event); |
1319 |
|
1320 |
instance_data[instance].rxMsgCount++; |
1321 |
} |
1322 |
else { //if (rxd_event == SIG_RX_UNKNOWN) //need to re-enable the rx (got unknown frame type) |
1323 |
handle_error_unknownframe(dw_event); |
1324 |
} |
1325 |
// }
|
1326 |
} |
1327 |
|
1328 |
|
1329 |
/*! Callback funtion for RX timeout (available from newer driver version) */
|
1330 |
#pragma GCC optimize ("O3") |
1331 |
void instance_rxtimeoutcallback(const dwt_cb_data_t *rxd){ |
1332 |
|
1333 |
(void) rxd;
|
1334 |
event_data_t dw_event; |
1335 |
|
1336 |
int instance = 0; |
1337 |
|
1338 |
dw_event.type_pend = DWT_SIG_DW_IDLE; |
1339 |
|
1340 |
if(instance_data[instance].mode == ANCHOR) {
|
1341 |
//check if anchor has received all of the responses from other anchors (it could have received only 1 or 2)
|
1342 |
//it's timed out (re-enable rx or tx response)
|
1343 |
if(instance_data[instance].responseTO > 0) { |
1344 |
instance_data[instance].responseTO--; |
1345 |
//send a response or re-enable rx
|
1346 |
dw_event.type_pend = anctxorrxreenable(instance_data[instance].instanceAddress16, 6+0); |
1347 |
// Print_On_Uart("Re-enable RX in rxtimeout callback\r\n");
|
1348 |
} |
1349 |
} |
1350 |
dw_event.type = 0;
|
1351 |
dw_event.type_save = DWT_SIG_RX_TIMEOUT; |
1352 |
dw_event.rxLength = 0;
|
1353 |
dw_event.timeStamp = 0;
|
1354 |
dw_event.timeStamp32l = 0;
|
1355 |
dw_event.timeStamp32h = 0;
|
1356 |
|
1357 |
instance_putevent(dw_event, DWT_SIG_RX_TIMEOUT); |
1358 |
// printf("RX timeout while in %d\n", instance_data[instance].testAppState);
|
1359 |
|
1360 |
} |
1361 |
|
1362 |
|
1363 |
/*! Callback funtion for RX error (available from newer driver version) */
|
1364 |
#pragma GCC optimize ("O3") |
1365 |
void instance_rxerrorcallback(const dwt_cb_data_t *rxd) { |
1366 |
|
1367 |
(void) rxd;
|
1368 |
event_data_t dw_event; |
1369 |
|
1370 |
dw_event.uTimeStamp = portGetTickCnt(); |
1371 |
|
1372 |
handle_error_unknownframe(dw_event); |
1373 |
} |
1374 |
|
1375 |
|
1376 |
|
1377 |
#pragma GCC optimize ("O3") |
1378 |
int instance_peekevent(void){ |
1379 |
int instance = 0; |
1380 |
return instance_data[instance].dwevent[instance_data[instance].dweventPeek].type; //return the type of event that is in front of the queue |
1381 |
} |
1382 |
|
1383 |
#pragma GCC optimize ("O3") |
1384 |
void instance_saveevent(event_data_t newevent, uint8_t etype){
|
1385 |
int instance = 0; |
1386 |
|
1387 |
instance_data[instance].saved_dwevent = newevent; |
1388 |
instance_data[instance].saved_dwevent.type = etype; |
1389 |
} |
1390 |
|
1391 |
#pragma GCC optimize ("O3") |
1392 |
event_data_t instance_getsavedevent(void){
|
1393 |
int instance = 0; |
1394 |
|
1395 |
return instance_data[instance].saved_dwevent;
|
1396 |
} |
1397 |
|
1398 |
#pragma GCC optimize ("O3") |
1399 |
void instance_putevent(event_data_t newevent, uint8_t etype){
|
1400 |
int instance = 0; |
1401 |
//newevent.eventtime = portGetTickCnt();
|
1402 |
newevent.gotit = 0 ; //newevent.eventtimeclr = 0; |
1403 |
|
1404 |
//copy event
|
1405 |
instance_data[instance].dwevent[instance_data[instance].dweventIdxIn] = newevent; |
1406 |
|
1407 |
//set type - this makes it a new event (making sure the event data is copied before event is set as new)
|
1408 |
//to make sure that the get event function does not get an incomplete event
|
1409 |
instance_data[instance].dwevent[instance_data[instance].dweventIdxIn].type = etype; |
1410 |
|
1411 |
instance_data[instance].dweventIdxIn++; |
1412 |
|
1413 |
if(MAX_EVENT_NUMBER == instance_data[instance].dweventIdxIn)
|
1414 |
{ |
1415 |
instance_data[instance].dweventIdxIn = 0;
|
1416 |
} |
1417 |
//eventIncount++;
|
1418 |
|
1419 |
//printf("put %d - in %d out %d @ %d\n", newevent.type, instance_data[instance].dweventCntIn, instance_data[instance].dweventCntOut, ptime);
|
1420 |
} |
1421 |
|
1422 |
event_data_t dw_event_g; |
1423 |
|
1424 |
#pragma GCC optimize ("O3") |
1425 |
event_data_t* instance_getevent(int x){
|
1426 |
int instance = 0; |
1427 |
int indexOut = instance_data[instance].dweventIdxOut;
|
1428 |
|
1429 |
//dw_event_g = instance_data[instance].dwevent[instance_data[instance].dweventCntOut]; //this holds any TX/RX events
|
1430 |
|
1431 |
//memcpy(&dw_event_g, &instance_data[instance].dwevent[instance_data[instance].dweventCntOut], sizeof(event_data_t));
|
1432 |
|
1433 |
if(instance_data[instance].dwevent[indexOut].type == 0) { //exit with "no event" |
1434 |
dw_event_g.type = 0;
|
1435 |
dw_event_g.type_save = 0;
|
1436 |
return &dw_event_g;
|
1437 |
} |
1438 |
|
1439 |
//copy the event
|
1440 |
dw_event_g.type_save = instance_data[instance].dwevent[indexOut].type_save ; |
1441 |
dw_event_g.type_pend = instance_data[instance].dwevent[indexOut].type_pend ; |
1442 |
dw_event_g.rxLength = instance_data[instance].dwevent[indexOut].rxLength ; |
1443 |
dw_event_g.timeStamp = instance_data[instance].dwevent[indexOut].timeStamp ; |
1444 |
dw_event_g.timeStamp32l = instance_data[instance].dwevent[indexOut].timeStamp32l ; |
1445 |
dw_event_g.timeStamp32h = instance_data[instance].dwevent[indexOut].timeStamp32h ; |
1446 |
dw_event_g.uTimeStamp = instance_data[instance].dwevent[indexOut].uTimeStamp ; |
1447 |
//dw_event_g.eventtime = instance_data[instance].dwevent[indexOut].eventtime ;
|
1448 |
//dw_event_g.eventtimeclr = instance_data[instance].dwevent[indexOut].eventtimeclr ;
|
1449 |
//dw_event_g.gotit = instance_data[instance].dwevent[indexOut].gotit ;
|
1450 |
|
1451 |
memcpy(&dw_event_g.msgu, &instance_data[instance].dwevent[indexOut].msgu, sizeof(instance_data[instance].dwevent[indexOut].msgu));
|
1452 |
|
1453 |
dw_event_g.type = instance_data[instance].dwevent[indexOut].type ; |
1454 |
|
1455 |
|
1456 |
instance_data[instance].dwevent[indexOut].gotit = (uint8_t)x; |
1457 |
|
1458 |
//instance_data[instance].dwevent[indexOut].eventtimeclr = portGetTickCnt();
|
1459 |
|
1460 |
instance_data[instance].dwevent[indexOut].type = 0; //clear the event |
1461 |
|
1462 |
instance_data[instance].dweventIdxOut++; |
1463 |
if(MAX_EVENT_NUMBER == instance_data[instance].dweventIdxOut){ //wrap the counter |
1464 |
instance_data[instance].dweventIdxOut = 0;
|
1465 |
} |
1466 |
instance_data[instance].dweventPeek = instance_data[instance].dweventIdxOut; //set the new peek value
|
1467 |
|
1468 |
//if(dw_event.type) printf("get %d - in %d out %d @ %d\n", dw_event.type, instance_data[instance].dweventCntIn, instance_data[instance].dweventCntOut, ptime);
|
1469 |
|
1470 |
//eventOutcount++;
|
1471 |
|
1472 |
|
1473 |
return &dw_event_g;
|
1474 |
} |
1475 |
|
1476 |
void instance_clearevents(void){ |
1477 |
int i = 0; |
1478 |
int instance = 0; |
1479 |
|
1480 |
for(i=0; i<MAX_EVENT_NUMBER; i++) { |
1481 |
memset(&instance_data[instance].dwevent[i], 0, sizeof(event_data_t)); |
1482 |
} |
1483 |
|
1484 |
instance_data[instance].dweventIdxIn = 0;
|
1485 |
instance_data[instance].dweventIdxOut = 0;
|
1486 |
instance_data[instance].dweventPeek = 0;
|
1487 |
|
1488 |
//eventOutcount = 0;
|
1489 |
//eventIncount = 0;
|
1490 |
} |
1491 |
|
1492 |
// -------------------------------------------------------------------------------------------------------------------
|
1493 |
#pragma GCC optimize ("O3") |
1494 |
int instance_run(void){ |
1495 |
int instance = 0 ; |
1496 |
int done = INST_NOT_DONE_YET;
|
1497 |
int message = instance_peekevent(); //get any of the received events from ISR |
1498 |
|
1499 |
|
1500 |
while(done == INST_NOT_DONE_YET){
|
1501 |
//int state = instance_data[instance].testAppState;
|
1502 |
done = testapprun(&instance_data[instance], message) ; // run the communications application
|
1503 |
|
1504 |
//we've processed message
|
1505 |
message = 0;
|
1506 |
} |
1507 |
|
1508 |
if(done == INST_DONE_WAIT_FOR_NEXT_EVENT_TO) { //we are in RX and need to timeout (Tag needs to send another poll if no Rx frame) |
1509 |
if(instance_data[instance].mode == TAG) { //Tag (is either in RX or sleeping) |
1510 |
int32_t nextPeriod ; |
1511 |
|
1512 |
// next period will be a positive number because correction is -0.5 to +1.5 periods, (and tagSleepTime_ms is the period)
|
1513 |
nextPeriod = instance_data[instance].tagSleepRnd + instance_data[instance].tagSleepTime_ms + instance_data[instance].tagSleepCorrection; |
1514 |
|
1515 |
instance_data[instance].nextSleepPeriod = (uint32_t) nextPeriod ; //set timeout time, CAST the positive period to UINT for correct wrapping.
|
1516 |
instance_data[instance].tagSleepCorrection2 = instance_data[instance].tagSleepCorrection; |
1517 |
instance_data[instance].tagSleepCorrection = 0; //clear the correction |
1518 |
instance_data[instance].instanceTimerEn = 1; //start timer |
1519 |
} |
1520 |
instance_data[instance].stopTimer = 0 ; //clear the flag - timer can run if instancetimer_en set (set above) |
1521 |
instance_data[instance].done = INST_NOT_DONE_YET; |
1522 |
} |
1523 |
|
1524 |
//check if timer has expired
|
1525 |
if((instance_data[instance].instanceTimerEn == 1) && (instance_data[instance].stopTimer == 0)) { |
1526 |
if(instance_data[instance].mode == TAG) {
|
1527 |
if((portGetTickCnt() - instance_data[instance].instanceWakeTime) > instance_data[instance].nextSleepPeriod) {
|
1528 |
event_data_t dw_event; |
1529 |
instance_data[instance].instanceTimerEn = 0;
|
1530 |
dw_event.rxLength = 0;
|
1531 |
dw_event.type = 0;
|
1532 |
dw_event.type_save = 0x80 | DWT_SIG_RX_TIMEOUT;
|
1533 |
//printf("PC timeout DWT_SIG_RX_TIMEOUT\n");
|
1534 |
instance_putevent(dw_event, DWT_SIG_RX_TIMEOUT); |
1535 |
} |
1536 |
} |
1537 |
#if (ANCTOANCTWR == 1) //allow anchor to anchor ranging |
1538 |
else if(instance_data[instance].mode == ANCHOR) { |
1539 |
uint32_t slotTime = portGetTickCnt() % instance_data[instance].sframePeriod; |
1540 |
|
1541 |
if(instance_data[instance].gatewayAnchor) {
|
1542 |
//if we are in the last slot - then A0 ranges to A1 and A2
|
1543 |
if( slotTime >= instance_data[instance].a0SlotTime) {
|
1544 |
port_DisableEXT_IRQ(); //enable ScenSor IRQ before starting
|
1545 |
//anchor0 sends poll to anchor1
|
1546 |
instance_data[instance].mode = ANCHOR_RNG; //change to ranging initiator
|
1547 |
dwt_forcetrxoff(); //disable DW1000
|
1548 |
instance_clearevents(); //clear any events
|
1549 |
//change state to send a Poll
|
1550 |
instance_data[instance].testAppState = TA_TXPOLL_WAIT_SEND ; |
1551 |
instance_data[instance].msg_f.destAddr[0] = 0x1 ; |
1552 |
instance_data[instance].msg_f.destAddr[1] = (GATEWAY_ANCHOR_ADDR >> 8); |
1553 |
instance_data[instance].instanceTimerEn = 0;
|
1554 |
instance_data[instance].rangeNumAnc++; |
1555 |
port_EnableEXT_IRQ(); //enable ScenSor IRQ before starting
|
1556 |
} |
1557 |
} |
1558 |
else if (instance_data[instance].instanceAddress16 == A1_ANCHOR_ADDR) { //A1 ranges to A2 in the 2nd half of last slot |
1559 |
if(portGetTickCnt() >= instance_data[instance].a1SlotTime) {
|
1560 |
port_DisableEXT_IRQ(); //enable ScenSor IRQ before starting
|
1561 |
//anchor1 sends poll to anchor2
|
1562 |
instance_data[instance].mode = ANCHOR_RNG; //change to ranging initiator
|
1563 |
dwt_forcetrxoff(); //disable DW1000
|
1564 |
instance_clearevents(); //clear any events
|
1565 |
//change state to send a Poll
|
1566 |
instance_data[instance].testAppState = TA_TXPOLL_WAIT_SEND ; |
1567 |
instance_data[instance].msg_f.destAddr[0] = 0x2 ; |
1568 |
instance_data[instance].msg_f.destAddr[1] = (GATEWAY_ANCHOR_ADDR >> 8); |
1569 |
|
1570 |
instance_data[instance].instanceTimerEn = 0;
|
1571 |
//instance_data[instance].a1SlotTime = 0;
|
1572 |
port_EnableEXT_IRQ(); //enable ScenSor IRQ before starting
|
1573 |
} |
1574 |
} |
1575 |
} |
1576 |
#endif
|
1577 |
} |
1578 |
|
1579 |
#if (ANCTOANCTWR == 1) //allow anchor to anchor ranging |
1580 |
else if (instance_data[instance].instanceTimerEn == 0){ |
1581 |
if((instance_data[instance].mode == ANCHOR) && (instance_data[instance].gatewayAnchor)){
|
1582 |
uint32_t slotTime = portGetTickCnt() % instance_data[instance].sframePeriod; |
1583 |
//enable the timer in 1st slot
|
1584 |
if(slotTime < instance_data[instance].slotPeriod){
|
1585 |
instance_data[instance].instanceTimerEn = 1;
|
1586 |
} |
1587 |
} |
1588 |
} |
1589 |
#endif
|
1590 |
return 0 ; |
1591 |
} |
1592 |
|
1593 |
|
1594 |
void instance_close(void){ |
1595 |
//wake up device from low power mode
|
1596 |
//NOTE - in the ARM code just drop chip select for 200us
|
1597 |
clear_SPI_chip_select(); //CS low
|
1598 |
Sleep(1); //200 us to wake up then waits 5ms for DW1000 XTAL to stabilise |
1599 |
set_SPI_chip_select(); //CS high
|
1600 |
Sleep(5);
|
1601 |
dwt_entersleepaftertx(0); // clear the "enter deep sleep after tx" bit |
1602 |
dwt_setinterrupt(0xFFFFFFFF, 0); //don't allow any interrupts |
1603 |
|
1604 |
} |
1605 |
|
1606 |
|
1607 |
void instance_notify_DW1000_inIDLE(int idle){ |
1608 |
instance_data[0].dwIDLE = idle;
|
1609 |
} |
1610 |
|
1611 |
void instanceconfigtxpower(uint32_t txpower){
|
1612 |
instance_data[0].txPower = txpower ;
|
1613 |
|
1614 |
instance_data[0].txPowerChanged = 1; |
1615 |
} |
1616 |
|
1617 |
void instancesettxpower(void){ |
1618 |
if(instance_data[0].txPowerChanged == 1){ |
1619 |
//Configure TX power
|
1620 |
dwt_write32bitreg(0x1E, instance_data[0].txPower); |
1621 |
|
1622 |
instance_data[0].txPowerChanged = 0; |
1623 |
} |
1624 |
} |
1625 |
|
1626 |
void instanceconfigantennadelays(uint16_t tx, uint16_t rx){
|
1627 |
instance_data[0].txAntennaDelay = tx ;
|
1628 |
instance_data[0].rxAntennaDelay = rx ;
|
1629 |
|
1630 |
instance_data[0].antennaDelayChanged = 1; |
1631 |
} |
1632 |
|
1633 |
void instancesetantennadelays(void){ |
1634 |
if(instance_data[0].antennaDelayChanged == 1){ |
1635 |
dwt_setrxantennadelay(instance_data[0].rxAntennaDelay);
|
1636 |
dwt_settxantennadelay(instance_data[0].txAntennaDelay);
|
1637 |
|
1638 |
instance_data[0].antennaDelayChanged = 0; |
1639 |
} |
1640 |
} |
1641 |
|
1642 |
|
1643 |
uint16_t instancetxantdly(void){
|
1644 |
return instance_data[0].txAntennaDelay; |
1645 |
} |
1646 |
|
1647 |
uint16_t instancerxantdly(void){
|
1648 |
return instance_data[0].rxAntennaDelay; |
1649 |
} |
1650 |
|
1651 |
uint8_t instancevalidranges(void){
|
1652 |
uint8_t x = instance_data[0].rxResponseMaskReport;
|
1653 |
instance_data[0].rxResponseMaskReport = 0; //reset mask as we have printed out the ToFs |
1654 |
return x;
|
1655 |
} |
1656 |
#endif
|
1657 |
|
1658 |
|
1659 |
/*! The following Functions are added for user application (previously on the API )*/
|
1660 |
|
1661 |
/*! ------------------------------------------------------------------------------------------------------------------
|
1662 |
* @fn dwt_getotptxpower()
|
1663 |
*
|
1664 |
* @brief This API function returns the tx power value read from OTP memory as part of initialisation
|
1665 |
*
|
1666 |
* input parameters
|
1667 |
* @param prf - this is the PRF e.g. DWT_PRF_16M or DWT_PRF_64M
|
1668 |
* @param chan - this is the channel e.g. 1 to 7
|
1669 |
*
|
1670 |
* output parameters
|
1671 |
*
|
1672 |
* returns tx power value for a given PRF and channel
|
1673 |
*/
|
1674 |
uint32_t dwt_getotptxpower(uint8_t prf, uint8_t chan){ |
1675 |
return platformLocalData.txPowCfg[(prf - DWT_PRF_16M) + (chan_idx[chan] * 2)]; |
1676 |
} |
1677 |
|
1678 |
/*! ------------------------------------------------------------------------------------------------------------------
|
1679 |
* @fn dwt_getTREKOTPantennadelay()
|
1680 |
*
|
1681 |
* @brief This API function returns the antenna delay read from the OTP memory as part of device initialisation
|
1682 |
* Note: the antenna delay will only be read if dwt_initialise is called with DWT_LOADANTDLY bit set in the config parameter
|
1683 |
* The values of antenna delay are only valid for TREK use case modes.
|
1684 |
*
|
1685 |
* input parameters:
|
1686 |
* @param anchor - this is the mode (Tag or Anchor) if Tag set to 0, if Anchor set to 1
|
1687 |
* @param chan - this is the channel (1, 2, 3, 4, 5, 7)
|
1688 |
* @param datarate - this is the datarate DWT_BR_6M8, DWT_BR_110K or DWT_BR_850K
|
1689 |
*
|
1690 |
*/
|
1691 |
uint16_t dwt_getTREKOTPantennadelay(uint8_t anchor, uint8_t chan, uint8_t datarate){ |
1692 |
uint32_t dly = 0;
|
1693 |
|
1694 |
// 32-bit antenna delay value previously read from OTP, high 16 bits is value for Anchor mode, low 16-bits for Tag mode
|
1695 |
switch(chan){
|
1696 |
case 2: |
1697 |
if(datarate == DWT_BR_6M8)
|
1698 |
dly = platformLocalData.antennaCals[0];
|
1699 |
else if(datarate == DWT_BR_110K) |
1700 |
dly = platformLocalData.antennaCals[1];
|
1701 |
break;
|
1702 |
case 5: |
1703 |
if(datarate == DWT_BR_6M8)
|
1704 |
dly = platformLocalData.antennaCals[2];
|
1705 |
else if(datarate == DWT_BR_110K) |
1706 |
dly = platformLocalData.antennaCals[3];
|
1707 |
break;
|
1708 |
default:
|
1709 |
dly = 0;
|
1710 |
break;
|
1711 |
} |
1712 |
|
1713 |
return (dly >> (16*(anchor & 0x1))) & 0xFFFF; |
1714 |
} |
1715 |
|
1716 |
/*! ------------------------------------------------------------------------------------------------------------------
|
1717 |
* @fn dwt_readantennadelay()
|
1718 |
*
|
1719 |
* @brief This API function returns the antenna delay read from the OTP memory as part of device initialisation
|
1720 |
* Note: the antenna delay will only be read if dwt_initialise is called with DWT_LOADANTDLY bit set in the config parameter
|
1721 |
*
|
1722 |
* input parameters:
|
1723 |
* @param prf - this is the PRF e.g. DWT_PRF_16M or DWT_PRF_64M
|
1724 |
*
|
1725 |
*/
|
1726 |
uint16_t dwt_readantennadelay(uint8_t prf){ |
1727 |
// 32-bit antenna delay value previously read from OTP, high 16 bits is value for 64 MHz PRF, low 16-bits for 16 MHz PRF
|
1728 |
return (platformLocalData.antennaDly >> (16*(prf-DWT_PRF_16M))) & 0xFFFF; |
1729 |
} |
1730 |
|
1731 |
|
1732 |
/* ==========================================================
|
1733 |
|
1734 |
Notes:
|
1735 |
|
1736 |
Previously code handled multiple instances in a single console application
|
1737 |
|
1738 |
Now have changed it to do a single instance only. With minimal code changes...(i.e. kept [instance] index but it is always 0.
|
1739 |
|
1740 |
Windows application should call instance_init() once and then in the "main loop" call instance_run().
|
1741 |
|
1742 |
*/
|
1743 |
|
1744 |
#endif /* defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 1) */ |