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