amiro-lld / source / DW1000 / v1 / deca_instance_tag_anchor_v1.c @ 26dead12
History | View | Annotate | Download (42.32 KB)
1 |
/*! ----------------------------------------------------------------------------
|
---|---|
2 |
* @file instance_tag.c
|
3 |
* @brief Decawave tag application state machine for TREK demo
|
4 |
*
|
5 |
* @attention
|
6 |
*
|
7 |
* Copyright 2016 (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 <v1/deca_instance_v1.h> |
19 |
#include <string.h> |
20 |
#include <math.h> |
21 |
#include "module.h" |
22 |
|
23 |
// -------------------------------------------------------------------------------------------------------------------
|
24 |
//
|
25 |
// function to construct the message/frame header bytes
|
26 |
//
|
27 |
// -------------------------------------------------------------------------------------------------------------------
|
28 |
//
|
29 |
void instanceconfigframeheader16(instance_data_t *inst){
|
30 |
//set frame type (0-2), SEC (3), Pending (4), ACK (5), PanIDcomp(6)
|
31 |
inst->msg_f.frameCtrl[0] = 0x1 /*frame type 0x1 == data*/ | 0x40 /*PID comp*/; |
32 |
|
33 |
//source/dest addressing modes and frame version
|
34 |
inst->msg_f.frameCtrl[1] = 0x8 /*dest extended address (16bits)*/ | 0x80 /*src extended address (16bits)*/; |
35 |
|
36 |
inst->msg_f.panID[0] = (inst->panID) & 0xff; |
37 |
inst->msg_f.panID[1] = inst->panID >> 8; |
38 |
|
39 |
inst->msg_f.seqNum = 0;
|
40 |
} |
41 |
|
42 |
int instancesenddlypacket(instance_data_t *inst, int delayedTx){ |
43 |
int result = 0; |
44 |
|
45 |
dwt_writetxfctrl(inst->psduLength, 0, 1); |
46 |
if(delayedTx == DWT_START_TX_DELAYED){
|
47 |
dwt_setdelayedtrxtime(inst->delayedReplyTime) ; //should be high 32-bits of delayed TX TS
|
48 |
} |
49 |
|
50 |
//begin delayed TX of frame
|
51 |
if (dwt_starttx((uint8_t)(delayedTx | inst->wait4ack))){ // delayed start was too late |
52 |
result = 1; //late/error |
53 |
inst->lateTX++; |
54 |
} |
55 |
else {
|
56 |
inst->timeofTx = portGetTickCnt(); |
57 |
inst->monitor = 1;
|
58 |
} |
59 |
return result; // state changes |
60 |
} |
61 |
|
62 |
int instance_calcranges(uint32_t *array, uint16_t size, int reportRange, uint8_t* mask){ |
63 |
int i;
|
64 |
int newRange = TOF_REPORT_NUL;
|
65 |
int distance = 0; |
66 |
|
67 |
for(i=0; i<size; i++) { |
68 |
uint32_t tofx = array[i]; |
69 |
if(tofx != INVALID_TOF) { //if ToF == 0 - then no new range to report |
70 |
distance = reportTOF(i, tofx); |
71 |
} |
72 |
|
73 |
if(distance == 1){ |
74 |
newRange = reportRange; |
75 |
} |
76 |
else {
|
77 |
//clear mask
|
78 |
*mask &= ~(0x1 << i) ;
|
79 |
clearDistTable(i); |
80 |
} |
81 |
array[i] = INVALID_TOF; |
82 |
|
83 |
distance = 0;
|
84 |
} |
85 |
|
86 |
return newRange;
|
87 |
} |
88 |
|
89 |
// -------------------------------------------------------------------------------------------------------------------
|
90 |
//
|
91 |
// the main instance state machine (all the instance modes Tag, Anchor or Listener use the same statemachine....)
|
92 |
//
|
93 |
// -------------------------------------------------------------------------------------------------------------------
|
94 |
//
|
95 |
int testapprun(instance_data_t *inst, int message){ |
96 |
|
97 |
switch (inst->testAppState){
|
98 |
case TA_INIT :
|
99 |
// printf("TA_INIT") ;
|
100 |
switch (inst->mode) {
|
101 |
case TAG: {
|
102 |
uint16_t sleep_mode = 0;
|
103 |
|
104 |
dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN); //allow data, ack frames;
|
105 |
dwt_setpanid(inst->panID); |
106 |
|
107 |
memcpy(inst->eui64, &inst->instanceAddress16, ADDR_BYTE_SIZE_S); |
108 |
dwt_seteui(inst->eui64); |
109 |
|
110 |
//set source address
|
111 |
inst->newRangeTagAddress = inst->instanceAddress16 ; |
112 |
dwt_setaddress16(inst->instanceAddress16); |
113 |
|
114 |
//Start off by Sleeping 1st -> set instToSleep to TRUE
|
115 |
inst->nextState = TA_TXPOLL_WAIT_SEND; |
116 |
inst->testAppState = TA_TXE_WAIT; |
117 |
inst->instToSleep = TRUE ; |
118 |
|
119 |
inst->rangeNum = 0;
|
120 |
inst->tagSleepCorrection = 0;
|
121 |
|
122 |
sleep_mode = (DWT_LOADUCODE|DWT_PRESRV_SLEEP|DWT_CONFIG|DWT_TANDV); |
123 |
|
124 |
if(inst->configData.txPreambLength == DWT_PLEN_64) //if using 64 length preamble then use the corresponding OPSet |
125 |
sleep_mode |= DWT_LOADOPSET; |
126 |
|
127 |
#if (DEEP_SLEEP == 1) |
128 |
dwt_configuresleep(sleep_mode, DWT_WAKE_WK|DWT_WAKE_CS|DWT_SLP_EN); //configure the on wake parameters (upload the IC config settings)
|
129 |
#endif
|
130 |
instanceconfigframeheader16(inst); |
131 |
inst->instanceWakeTime = portGetTickCnt(); |
132 |
} |
133 |
break;
|
134 |
case ANCHOR: {
|
135 |
memcpy(inst->eui64, &inst->instanceAddress16, ADDR_BYTE_SIZE_S); |
136 |
dwt_seteui(inst->eui64); |
137 |
|
138 |
dwt_setpanid(inst->panID); |
139 |
|
140 |
//set source address
|
141 |
inst->shortAdd_idx = (inst->instanceAddress16 & 0x3) ;
|
142 |
dwt_setaddress16(inst->instanceAddress16); |
143 |
|
144 |
//if address = 0x8000
|
145 |
if(inst->instanceAddress16 == GATEWAY_ANCHOR_ADDR){
|
146 |
inst->gatewayAnchor = TRUE; |
147 |
} |
148 |
|
149 |
dwt_enableframefilter(DWT_FF_NOTYPE_EN); //allow data, ack frames;
|
150 |
|
151 |
// First time anchor listens we don't do a delayed RX
|
152 |
dwt_setrxaftertxdelay(0);
|
153 |
//change to next state - wait to receive a message
|
154 |
inst->testAppState = TA_RXE_WAIT ; |
155 |
|
156 |
dwt_setrxtimeout(0);
|
157 |
dwt_setpreambledetecttimeout(0);
|
158 |
instanceconfigframeheader16(inst); |
159 |
|
160 |
} |
161 |
break;
|
162 |
case LISTENER:{
|
163 |
dwt_enableframefilter(DWT_FF_NOTYPE_EN); //disable frame filtering
|
164 |
dwt_setrxaftertxdelay(0); //no delay of turning on of RX |
165 |
dwt_setrxtimeout(0);
|
166 |
dwt_setpreambledetecttimeout(0);
|
167 |
//change to next state - wait to receive a message
|
168 |
inst->testAppState = TA_RXE_WAIT ; |
169 |
} |
170 |
break ; // end case TA_INIT |
171 |
default:
|
172 |
break;
|
173 |
} |
174 |
break; // end case TA_INIT |
175 |
|
176 |
case TA_SLEEP_DONE : {
|
177 |
event_data_t* dw_event = instance_getevent(10); //clear the event from the queue |
178 |
// waiting for timout from application to wakup IC
|
179 |
if (dw_event->type != DWT_SIG_RX_TIMEOUT){
|
180 |
// if no pause and no wake-up timeout continu waiting for the sleep to be done.
|
181 |
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //wait here for sleep timeout
|
182 |
break;
|
183 |
} |
184 |
|
185 |
inst->done = INST_NOT_DONE_YET; |
186 |
inst->instToSleep = FALSE ; |
187 |
inst->testAppState = inst->nextState; |
188 |
inst->nextState = 0; //clear |
189 |
inst->instanceWakeTime = portGetTickCnt(); // Record the time count when we wake-up
|
190 |
#if (DEEP_SLEEP == 1) |
191 |
{ |
192 |
uint32 x = 0;
|
193 |
|
194 |
//wake up device from low power mode
|
195 |
//NOTE - in the ARM code just drop chip select for 200us
|
196 |
led_on(LED_PC9); |
197 |
port_SPIx_clear_chip_select(); //CS low
|
198 |
instance_data[0].dwIDLE = 0; //reset DW1000 IDLE flag |
199 |
|
200 |
setup_DW1000RSTnIRQ(1); //enable RSTn IRQ |
201 |
|
202 |
Sleep(2); //200 us to wake up - need 2 as Sleep(1) is ~ 175 us |
203 |
// chThdSleepMilliseconds(2);
|
204 |
//then wait 5ms for DW1000 XTAL to stabilise - instead of wait we wait for RSTn to go high
|
205 |
//Sleep(5);
|
206 |
|
207 |
//need to poll to check when the DW1000 is in IDLE, the CPLL interrupt is not reliable
|
208 |
//when RSTn goes high the DW1000 is in INIT, it will enter IDLE after PLL lock (in 5 us)
|
209 |
while(instance_data[0].dwIDLE == 0) // this variable will be sent in the IRQ (process_dwRSTn_irq) |
210 |
{ |
211 |
//wait for DW1000 to go to IDLE state RSTn pin to go high
|
212 |
x++; |
213 |
} |
214 |
setup_DW1000RSTnIRQ(0); //disable RSTn IRQ |
215 |
port_SPIx_set_chip_select(); //CS high
|
216 |
|
217 |
//!!! NOTE it takes ~35us for the DW1000 to download AON and lock the PLL and be in IDLE state
|
218 |
//do some dummy reads of the dev ID register to make sure DW1000 is in IDLE before setting LEDs
|
219 |
x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz)
|
220 |
x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz)
|
221 |
x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz)
|
222 |
x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz)
|
223 |
|
224 |
x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz)
|
225 |
/*if(x != DWT_DEVICE_ID)
|
226 |
{
|
227 |
x = dwt_readdevid(); //dummy read... need to wait for 5 us to exit INIT state (5 SPI bytes @ ~18 MHz)
|
228 |
}*/
|
229 |
led_off(LED_PC9); |
230 |
//this is platform dependent - only program if DW EVK/EVB
|
231 |
dwt_setleds(1);
|
232 |
|
233 |
//MP bug - TX antenna delay needs reprogramming as it is not preserved (only RX)
|
234 |
dwt_settxantennadelay(inst->txAntennaDelay) ; |
235 |
|
236 |
//set EUI as it will not be preserved unless the EUI is programmed and loaded from NVM
|
237 |
dwt_seteui(inst->eui64); |
238 |
} |
239 |
#else
|
240 |
Sleep(3); //to approximate match the time spent in the #if above |
241 |
#endif
|
242 |
|
243 |
instancesetantennadelays(); //this will update the antenna delay if it has changed
|
244 |
instancesettxpower(); //configure TX power if it has changed
|
245 |
|
246 |
} |
247 |
break;
|
248 |
|
249 |
case TA_TXE_WAIT : //either go to sleep or proceed to TX a message |
250 |
// printf("TA_TXE_WAIT") ;
|
251 |
//if we are scheduled to go to sleep before next transmission then sleep first.
|
252 |
if((inst->nextState == TA_TXPOLL_WAIT_SEND)
|
253 |
&& (inst->instToSleep) //go to sleep before sending the next poll/ starting new ranging exchange
|
254 |
){ |
255 |
inst->rangeNum++; //increment the range number before going to sleep
|
256 |
//the app should put chip into low power state and wake up after tagSleepTime_ms time...
|
257 |
//the app could go to *_IDLE state and wait for uP to wake it up...
|
258 |
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT_TO; //don't sleep here but kick off the Sleep timer countdown
|
259 |
inst->testAppState = TA_SLEEP_DONE; { |
260 |
#if (DEEP_SLEEP == 1) |
261 |
//put device into low power mode
|
262 |
dwt_entersleep(); //go to sleep
|
263 |
#endif
|
264 |
//DW1000 gone to sleep - report the received range
|
265 |
inst->newRange = instance_calcranges(&inst->tofArray[0], MAX_ANCHOR_LIST_SIZE, TOF_REPORT_T2A, &inst->rxResponseMask);
|
266 |
inst->rxResponseMaskReport = inst->rxResponseMask; |
267 |
inst->rxResponseMask = 0;
|
268 |
inst->newRangeTime = portGetTickCnt() ; |
269 |
} |
270 |
} |
271 |
else { //proceed to configuration and transmission of a frame |
272 |
inst->testAppState = inst->nextState; |
273 |
inst->nextState = 0; //clear |
274 |
} |
275 |
break ; // end case TA_TXE_WAIT |
276 |
|
277 |
case TA_TXPOLL_WAIT_SEND : {
|
278 |
|
279 |
inst->msg_f.messageData[POLL_RNUM] = (inst->mode == TAG) ? inst->rangeNum : inst->rangeNumAnc; //copy new range number
|
280 |
inst->msg_f.messageData[FCODE] = (inst->mode == TAG) ? RTLS_DEMO_MSG_TAG_POLL : RTLS_DEMO_MSG_ANCH_POLL; //message function code (specifies if message is a poll, response or other...)
|
281 |
inst->psduLength = (TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC); |
282 |
inst->msg_f.seqNum = inst->frameSN++; //copy sequence number and then increment
|
283 |
inst->msg_f.sourceAddr[0] = inst->eui64[0]; //copy the address |
284 |
inst->msg_f.sourceAddr[1] = inst->eui64[1]; //copy the address |
285 |
inst->msg_f.destAddr[0] = 0xff; //set the destination address (broadcast == 0xffff) |
286 |
inst->msg_f.destAddr[1] = 0xff; //set the destination address (broadcast == 0xffff) |
287 |
dwt_writetxdata(inst->psduLength, (uint8_t *) &inst->msg_f, 0) ; // write the frame data |
288 |
|
289 |
//set the delayed rx on time (the response message will be sent after this delay (from A0))
|
290 |
dwt_setrxaftertxdelay((uint32_t)RX_RESPONSE1_TURNAROUND); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
|
291 |
|
292 |
if(inst->mode == TAG){
|
293 |
inst->rxResps[inst->rangeNum] = 0; //reset the number of received responses |
294 |
inst->responseTO = MAX_ANCHOR_LIST_SIZE; //expecting 4 responses
|
295 |
dwt_setrxtimeout((uint16_t)inst->fwtoTime_sy * MAX_ANCHOR_LIST_SIZE); //configure the RX FWTO
|
296 |
} |
297 |
else {
|
298 |
inst->rxResps[inst->rangeNumAnc] = 0; //reset number of responses |
299 |
inst->responseTO = NUM_EXPECTED_RESPONSES_ANC0; //2 responses A1, A2
|
300 |
dwt_setrxtimeout((uint16_t)inst->fwtoTime_sy * (NUM_EXPECTED_RESPONSES_ANC0)); //units are
|
301 |
} |
302 |
|
303 |
inst->rxResponseMask = 0; //reset/clear the mask of received responses when tx poll |
304 |
inst->rxResponseMaskAnc = 0;
|
305 |
|
306 |
inst->wait4ack = DWT_RESPONSE_EXPECTED; //response is expected - automatically enable the receiver
|
307 |
|
308 |
dwt_writetxfctrl(inst->psduLength, 0, 1); //write frame control |
309 |
|
310 |
dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED); //transmit the frame
|
311 |
|
312 |
inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation
|
313 |
inst->previousState = TA_TXPOLL_WAIT_SEND ; |
314 |
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set above)
|
315 |
} |
316 |
break;
|
317 |
|
318 |
case TA_TXFINAL_WAIT_SEND : {
|
319 |
//the final has the same range number as the poll (part of the same ranging exchange)
|
320 |
inst->msg_f.messageData[POLL_RNUM] = (inst->mode == TAG) ? inst->rangeNum : inst->rangeNumAnc; |
321 |
//the mask is sent so the anchors know whether the response RX time is valid
|
322 |
inst->msg_f.messageData[VRESP] = (inst->mode == TAG) ? inst->rxResponseMask : inst->rxResponseMaskAnc; |
323 |
inst->msg_f.messageData[FCODE] = (inst->mode == TAG) ? RTLS_DEMO_MSG_TAG_FINAL : RTLS_DEMO_MSG_ANCH_FINAL; //message function code (specifies if message is a poll, response or other...)
|
324 |
inst->psduLength = (TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC); |
325 |
inst->msg_f.seqNum = inst->frameSN++; |
326 |
dwt_writetxdata(inst->psduLength, (uint8_t *) &inst->msg_f, 0) ; // write the frame data |
327 |
|
328 |
inst->wait4ack = 0; //clear the flag not using wait for response as this message ends the ranging exchange |
329 |
|
330 |
if(instancesenddlypacket(inst, DWT_START_TX_DELAYED)) {
|
331 |
// initiate the re-transmission
|
332 |
if(inst->mode == TAG){
|
333 |
inst->testAppState = TA_TXE_WAIT ; //go to TA_TXE_WAIT first to check if it's sleep time
|
334 |
inst->nextState = TA_TXPOLL_WAIT_SEND ; |
335 |
} |
336 |
else {
|
337 |
//A0 - failed to send Final
|
338 |
//A1 - failed to send Final
|
339 |
//go back to RX and behave as anchor
|
340 |
instance_backtoanchor(inst); |
341 |
} |
342 |
break; //exit this switch case... |
343 |
} |
344 |
else {
|
345 |
inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
|
346 |
inst->previousState = TA_TXFINAL_WAIT_SEND; |
347 |
} |
348 |
if(inst->mode == TAG){
|
349 |
inst->instToSleep = TRUE ; |
350 |
} |
351 |
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set above)
|
352 |
} |
353 |
break;
|
354 |
|
355 |
case TA_TX_WAIT_CONF : {
|
356 |
//printf("TA_TX_WAIT_CONF %d m%d %d states %08x %08x\n", inst->previousState, message, inst->newReportSent, dwt_read32bitreg(0x19), dwt_read32bitreg(0x0f)) ;
|
357 |
|
358 |
event_data_t* dw_event = instance_getevent(11); //get and clear this event |
359 |
|
360 |
//NOTE: Can get the ACK before the TX confirm event for the frame requesting the ACK
|
361 |
//this happens because if polling the ISR the RX event will be processed 1st and then the TX event
|
362 |
//thus the reception of the ACK will be processed before the TX confirmation of the frame that requested it.
|
363 |
if(dw_event->type != DWT_SIG_TX_DONE) { //wait for TX done confirmation |
364 |
if(dw_event->type != 0) { |
365 |
if(dw_event->type == DWT_SIG_RX_TIMEOUT){ //got RX timeout - i.e. did not get the response (e.g. ACK) |
366 |
//printf("RX timeout in TA_TX_WAIT_CONF (%d)\n", inst->previousState);
|
367 |
//we need to wait for SIG_TX_DONE and then process the timeout and re-send the frame if needed
|
368 |
inst->gotTO = 1;
|
369 |
} |
370 |
else{
|
371 |
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; |
372 |
} |
373 |
} |
374 |
|
375 |
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; |
376 |
break;
|
377 |
|
378 |
} |
379 |
|
380 |
inst->done = INST_NOT_DONE_YET; |
381 |
|
382 |
if(inst->previousState == TA_TXFINAL_WAIT_SEND) {
|
383 |
if(inst->mode == TAG){
|
384 |
inst->testAppState = TA_TXE_WAIT ; |
385 |
inst->nextState = TA_TXPOLL_WAIT_SEND ; |
386 |
break;
|
387 |
} |
388 |
else{
|
389 |
instance_backtoanchor(inst); |
390 |
} |
391 |
} |
392 |
else if (inst->gotTO == 1) { //timeout |
393 |
//printf("got TO in TA_TX_WAIT_CONF\n");
|
394 |
inst_processrxtimeout(inst); |
395 |
inst->gotTO = 0;
|
396 |
inst->wait4ack = 0 ; //clear this |
397 |
break;
|
398 |
} |
399 |
else{
|
400 |
inst->txu.txTimeStamp = dw_event->timeStamp; |
401 |
|
402 |
if(inst->previousState == TA_TXPOLL_WAIT_SEND){
|
403 |
uint64_t tagCalculatedFinalTxTime ; |
404 |
// Embed into Final message: 40-bit pollTXTime, 40-bit respRxTime, 40-bit finalTxTime
|
405 |
if(inst->mode == TAG){
|
406 |
tagCalculatedFinalTxTime = (inst->txu.txTimeStamp + inst->pollTx2FinalTxDelay) & MASK_TXDTS; |
407 |
} |
408 |
else { //for anchor make the final half the delay ..... (this is ok, as A0 awaits 2 responses) |
409 |
tagCalculatedFinalTxTime = (inst->txu.txTimeStamp + inst->pollTx2FinalTxDelayAnc) & MASK_TXDTS; |
410 |
} |
411 |
inst->delayedReplyTime = (uint32_t)(tagCalculatedFinalTxTime >> 8); //high 32-bits |
412 |
// Calculate Time Final message will be sent and write this field of Final message
|
413 |
// Sending time will be delayedReplyTime, snapped to ~125MHz or ~250MHz boundary by
|
414 |
// zeroing its low 9 bits, and then having the TX antenna delay added
|
415 |
// getting antenna delay from the device and add it to the Calculated TX Time
|
416 |
tagCalculatedFinalTxTime = tagCalculatedFinalTxTime + inst->txAntennaDelay; |
417 |
tagCalculatedFinalTxTime &= MASK_40BIT; |
418 |
|
419 |
// Write Calculated TX time field of Final message
|
420 |
memcpy(&(inst->msg_f.messageData[FTXT]), (uint8_t *)&tagCalculatedFinalTxTime, 5);
|
421 |
// Write Poll TX time field of Final message
|
422 |
memcpy(&(inst->msg_f.messageData[PTXT]), (uint8_t *)&inst->txu.tagPollTxTime, 5);
|
423 |
|
424 |
//change the w4r for the second and remaining anchors to 50 us
|
425 |
//dwt_setrxaftertxdelay((uint32)RX_RESPONSEX_TURNAROUND); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
|
426 |
} |
427 |
|
428 |
if(inst->previousState == TA_TXRESPONSE_SENT_TORX) {
|
429 |
inst->previousState = TA_TXRESPONSE_WAIT_SEND ; |
430 |
} |
431 |
inst->testAppState = TA_RXE_WAIT ; // After sending, tag expects response/report, anchor waits to receive a final/new poll
|
432 |
|
433 |
message = 0;
|
434 |
//fall into the next case (turn on the RX)
|
435 |
} |
436 |
} |
437 |
break ; // end case TA_TX_WAIT_CONF |
438 |
|
439 |
|
440 |
case TA_RXE_WAIT : {
|
441 |
// printf("TA_RXE_WAIT") ;
|
442 |
if(inst->wait4ack == 0) { //if this is set the RX will turn on automatically after TX |
443 |
//turn RX on
|
444 |
dwt_rxenable(DWT_START_RX_IMMEDIATE) ; // turn RX on, without delay
|
445 |
} |
446 |
else{
|
447 |
inst->wait4ack = 0 ; //clear the flag, the next time we want to turn the RX on it might not be auto |
448 |
} |
449 |
|
450 |
if (inst->mode != LISTENER){
|
451 |
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; //using RX FWTO
|
452 |
} |
453 |
|
454 |
inst->testAppState = TA_RX_WAIT_DATA; // let this state handle it
|
455 |
|
456 |
// end case TA_RXE_WAIT, don't break, but fall through into the TA_RX_WAIT_DATA state to process it immediately.
|
457 |
if(message == 0) break; |
458 |
} |
459 |
break;
|
460 |
|
461 |
case TA_RX_WAIT_DATA : // Wait RX data |
462 |
//printf("TA_RX_WAIT_DATA %d", message) ;
|
463 |
switch (message){
|
464 |
|
465 |
//if we have received a DWT_SIG_RX_OKAY event - this means that the message is IEEE data type - need to check frame control to know which addressing mode is used
|
466 |
case DWT_SIG_RX_OKAY : {
|
467 |
event_data_t* dw_event = instance_getevent(15); //get and clear this event |
468 |
uint8_t srcAddr[8] = {0,0,0,0,0,0,0,0}; |
469 |
uint8_t dstAddr[8] = {0,0,0,0,0,0,0,0}; |
470 |
int fcode = 0; |
471 |
int fn_code = 0; |
472 |
//int srclen = 0;
|
473 |
//int fctrladdr_len;
|
474 |
uint8_t tof_idx = 0;
|
475 |
uint8_t *messageData = NULL;
|
476 |
|
477 |
inst->stopTimer = 0; //clear the flag, as we have received a message |
478 |
|
479 |
// handle 16 and 64 bit source and destination addresses
|
480 |
switch(dw_event->msgu.frame[1] & 0xCC){ |
481 |
case 0xCC: // |
482 |
memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ll.sourceAddr[0]), ADDR_BYTE_SIZE_L); |
483 |
memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_ll.destAddr[0]), ADDR_BYTE_SIZE_L); |
484 |
fn_code = dw_event->msgu.rxmsg_ll.messageData[FCODE]; |
485 |
messageData = &dw_event->msgu.rxmsg_ll.messageData[0];
|
486 |
//srclen = ADDR_BYTE_SIZE_L;
|
487 |
//fctrladdr_len = FRAME_CRTL_AND_ADDRESS_L;
|
488 |
break;
|
489 |
case 0xC8: // |
490 |
memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_sl.sourceAddr[0]), ADDR_BYTE_SIZE_L); |
491 |
memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_sl.destAddr[0]), ADDR_BYTE_SIZE_S); |
492 |
fn_code = dw_event->msgu.rxmsg_sl.messageData[FCODE]; |
493 |
messageData = &dw_event->msgu.rxmsg_sl.messageData[0];
|
494 |
//srclen = ADDR_BYTE_SIZE_L;
|
495 |
//fctrladdr_len = FRAME_CRTL_AND_ADDRESS_LS;
|
496 |
break;
|
497 |
case 0x8C: // |
498 |
memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ls.sourceAddr[0]), ADDR_BYTE_SIZE_S); |
499 |
memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_ls.destAddr[0]), ADDR_BYTE_SIZE_L); |
500 |
fn_code = dw_event->msgu.rxmsg_ls.messageData[FCODE]; |
501 |
messageData = &dw_event->msgu.rxmsg_ls.messageData[0];
|
502 |
//srclen = ADDR_BYTE_SIZE_S;
|
503 |
//fctrladdr_len = FRAME_CRTL_AND_ADDRESS_LS;
|
504 |
break;
|
505 |
case 0x88: // |
506 |
memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ss.sourceAddr[0]), ADDR_BYTE_SIZE_S); |
507 |
memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_ss.destAddr[0]), ADDR_BYTE_SIZE_S); |
508 |
fn_code = dw_event->msgu.rxmsg_ss.messageData[FCODE]; |
509 |
messageData = &dw_event->msgu.rxmsg_ss.messageData[0];
|
510 |
//srclen = ADDR_BYTE_SIZE_S;
|
511 |
//fctrladdr_len = FRAME_CRTL_AND_ADDRESS_S;
|
512 |
break;
|
513 |
} |
514 |
|
515 |
if((inst->instToSleep == FALSE) && (inst->mode == LISTENER)){ //update received data, and go back to receiving frames |
516 |
//do something with message data (e.g. could extract any ToFs and print them)
|
517 |
inst->testAppState = TA_RXE_WAIT ; // wait for next frame
|
518 |
dwt_setrxaftertxdelay(0);
|
519 |
} |
520 |
else{
|
521 |
//process ranging messages
|
522 |
fcode = fn_code; |
523 |
tof_idx = srcAddr[0] & 0x3 ; |
524 |
|
525 |
switch(fcode){
|
526 |
|
527 |
case RTLS_DEMO_MSG_ANCH_POLL:
|
528 |
case RTLS_DEMO_MSG_TAG_POLL: {
|
529 |
inst->tagPollRxTime = dw_event->timeStamp ; //save Poll's Rx time
|
530 |
if(fcode == RTLS_DEMO_MSG_TAG_POLL){ //got poll from Tag |
531 |
inst->rangeNumA[srcAddr[0]&0x7] = messageData[POLL_RNUM]; //when anchor receives a poll, we need to remember the new range number |
532 |
} |
533 |
else{ //got poll from Anchor (initiator) |
534 |
inst->rangeNumAAnc[tof_idx] = messageData[POLL_RNUM]; //when anchor receives poll from another anchor - save the range number
|
535 |
} |
536 |
|
537 |
if (A1_ANCHOR_ADDR == inst->instanceAddress16) { //this is A1 |
538 |
|
539 |
if(GATEWAY_ANCHOR_ADDR == (srcAddr[0] | ((uint32_t)(srcAddr[1] << 8)))) { //poll is from A0 |
540 |
|
541 |
//configure the time A1 will poll A2 (it should be in half slot time from now)
|
542 |
inst->a1SlotTime = dw_event->uTimeStamp + (inst->slotPeriod); |
543 |
|
544 |
//inst->instanceTimerEn = 1; - THIS IS ENABLED BELOW AFTER FINAL
|
545 |
// - means that if final is not received then A1 will not range to A2
|
546 |
} |
547 |
} |
548 |
|
549 |
//the response has been sent - await TX done event
|
550 |
if(dw_event->type_pend == DWT_SIG_TX_PENDING){
|
551 |
inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
|
552 |
inst->previousState = TA_TXRESPONSE_SENT_POLLRX ; //wait for TX confirmation of sent response
|
553 |
} |
554 |
//already re-enabled the receiver
|
555 |
else if (dw_event->type_pend == DWT_SIG_RX_PENDING){ |
556 |
//stay in RX wait for next frame...
|
557 |
//RX is already enabled...
|
558 |
inst->testAppState = TA_RX_WAIT_DATA ; // wait for next frame
|
559 |
} |
560 |
else{ //the DW1000 is idle (re-enable from the application level) |
561 |
//stay in RX wait for next frame...
|
562 |
inst->testAppState = TA_RXE_WAIT ; // wait for next frame
|
563 |
} |
564 |
|
565 |
|
566 |
} |
567 |
break; //RTLS_DEMO_MSG_TAG_POLL |
568 |
|
569 |
case RTLS_DEMO_MSG_ANCH_RESP2:
|
570 |
case RTLS_DEMO_MSG_ANCH_RESP:{
|
571 |
uint8_t currentRangeNum = (messageData[TOFRN] + 1); //current = previous + 1 |
572 |
|
573 |
if(GATEWAY_ANCHOR_ADDR == (srcAddr[0] | ((uint32_t)(srcAddr[1] << 8)))){ //if response from gateway then use the correction factor |
574 |
if(inst->mode == TAG){
|
575 |
// casting received bytes to int because this is a signed correction -0.5 periods to +1.5 periods
|
576 |
inst->tagSleepCorrection = (int16_t) (((uint16_t) messageData[RES_TAG_SLP1] << 8) + messageData[RES_TAG_SLP0]);
|
577 |
inst->tagSleepRnd = 0; // once we have initial response from Anchor #0 the slot correction acts and we don't need this anymore |
578 |
} |
579 |
} |
580 |
|
581 |
//the response has been sent - await TX done event
|
582 |
if(dw_event->type_pend == DWT_SIG_TX_PENDING) { //anchor received response from anchor ID - 1 so is sending it's response now back to tag |
583 |
inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
|
584 |
inst->previousState = TA_TXRESPONSE_SENT_RESPRX ; //wait for TX confirmation of sent response
|
585 |
} |
586 |
//already re-enabled the receiver
|
587 |
else if(dw_event->type_pend == DWT_SIG_RX_PENDING) { |
588 |
// stay in TA_RX_WAIT_DATA - receiver is already enabled.
|
589 |
} |
590 |
//DW1000 idle - send the final
|
591 |
else { //if(dw_event->type_pend == DWT_SIG_DW_IDLE) |
592 |
|
593 |
if(((TAG == inst->mode) && (inst->rxResponseMask & 0x1)) //if A0's response received send the final |
594 |
|| ((A1_ANCHOR_ADDR == inst->instanceAddress16) && (inst->rxResponseMaskAnc & 0x4))
|
595 |
|| ((GATEWAY_ANCHOR_ADDR == inst->instanceAddress16) && (inst->rxResponseMaskAnc & 0x2)) ) { //if A1's response received |
596 |
|
597 |
inst->testAppState = TA_TXFINAL_WAIT_SEND ; // send our response / the final
|
598 |
} |
599 |
else { //go to sleep |
600 |
|
601 |
if(TAG == inst->mode){
|
602 |
inst->testAppState = TA_TXE_WAIT ; //go to TA_TXE_WAIT first to check if it's sleep time
|
603 |
inst->nextState = TA_TXPOLL_WAIT_SEND ; |
604 |
inst->instToSleep = TRUE; |
605 |
} |
606 |
else {
|
607 |
instance_backtoanchor(inst); |
608 |
} |
609 |
} |
610 |
} |
611 |
|
612 |
if(fcode == RTLS_DEMO_MSG_ANCH_RESP) { //tag to anchor mode |
613 |
if(currentRangeNum == inst->rangeNum) { //these are the previous ranges... |
614 |
//copy the ToF and put into array (array holds last 4 ToFs)
|
615 |
memcpy(&inst->tofArray[(srcAddr[0]&0x3)], &(messageData[TOFR]), 4); |
616 |
|
617 |
//check if the ToF is valid, this makes sure we only report valid ToFs
|
618 |
//e.g. consider the case of reception of response from anchor a1 (we are anchor a2)
|
619 |
//if a1 got a Poll with previous Range number but got no Final, then the response will have
|
620 |
//the correct range number but the range will be INVALID_TOF
|
621 |
if(inst->tofArray[(srcAddr[0]&0x3)] != INVALID_TOF){ |
622 |
inst->rxResponseMask |= (0x1 << (srcAddr[0]&0x3)); |
623 |
} |
624 |
|
625 |
} |
626 |
else {
|
627 |
if(inst->tofArray[(srcAddr[0]&0x3)] != INVALID_TOF) { |
628 |
inst->tofArray[(srcAddr[0]&0x3)] = INVALID_TOF; |
629 |
} |
630 |
} |
631 |
|
632 |
|
633 |
} |
634 |
else { //anchor to anchor (only gateway processes anchor to anchor ToFs) |
635 |
//report the correct set of ranges (ranges from anchors A1, A2 need to match owns range number)
|
636 |
if((inst->gatewayAnchor)&&(currentRangeNum == inst->rangeNumAnc)) { //these are the previous ranges... |
637 |
inst->rangeNumAAnc[0] = inst->rangeNumAnc ;
|
638 |
|
639 |
//once A0 receives A2's response then it can report the 3 ToFs.
|
640 |
if(inst->rxResps[inst->rangeNumAnc] == 3) |
641 |
//if(A2_ANCHOR_ADDR == (srcAddr[0] | ((uint32)(srcAddr[1] << 8))))
|
642 |
{ |
643 |
//copy the ToF and put into array, the array should have 3 ToFs A0-A1, A0-A2 and A1-A2
|
644 |
memcpy(&inst->tofArrayAnc[(srcAddr[0]+dstAddr[0])&0x3], &(messageData[TOFR]), 4); |
645 |
//calculate all anchor - anchor ranges... and report
|
646 |
inst->newRange = instance_calcranges(&inst->tofArrayAnc[0], MAX_ANCHOR_LIST_SIZE, TOF_REPORT_A2A, &inst->rxResponseMaskAnc);
|
647 |
inst->rxResponseMaskReport = inst->rxResponseMaskAnc; |
648 |
inst->rxResponseMaskAnc = 0;
|
649 |
inst->newRangeTime = dw_event->uTimeStamp ; |
650 |
} |
651 |
else {
|
652 |
//copy the ToF and put into array (array holds last 4 ToFs)
|
653 |
memcpy(&inst->tofArrayAnc[(srcAddr[0]+dstAddr[0])&0x3], &(messageData[TOFR]), 4); |
654 |
} |
655 |
} |
656 |
} |
657 |
|
658 |
} |
659 |
break; //RTLS_DEMO_MSG_ANCH_RESP |
660 |
|
661 |
|
662 |
case RTLS_DEMO_MSG_ANCH_FINAL:
|
663 |
case RTLS_DEMO_MSG_TAG_FINAL: {
|
664 |
int64_t Rb, Da, Ra, Db ; |
665 |
uint64_t tagFinalTxTime = 0;
|
666 |
uint64_t tagFinalRxTime = 0;
|
667 |
uint64_t tagPollTxTime = 0;
|
668 |
uint64_t anchorRespRxTime = 0;
|
669 |
int32_t tof = (int32_t)INVALID_TOF; |
670 |
|
671 |
double RaRbxDaDb = 0; |
672 |
double RbyDb = 0; |
673 |
double RayDa = 0; |
674 |
|
675 |
uint8_t validResp = messageData[VRESP]; |
676 |
uint8_t index = RRXT0 + 5*(inst->shortAdd_idx);
|
677 |
|
678 |
if((RTLS_DEMO_MSG_TAG_FINAL == fcode) &&
|
679 |
(inst->rangeNumA[srcAddr[0]&0x7] != messageData[POLL_RNUM])) { //Final's range number needs to match Poll's or else discard this message |
680 |
inst->testAppState = TA_RXE_WAIT ; // wait for next frame
|
681 |
break;
|
682 |
} |
683 |
|
684 |
if((RTLS_DEMO_MSG_ANCH_FINAL == fcode) &&
|
685 |
(((inst->rangeNumAAnc[tof_idx] != messageData[POLL_RNUM]) //Final's range number needs to match Poll's or else discard this message
|
686 |
|| inst->gatewayAnchor) //gateway can ignore the Final (from A1 to A2 exchange)
|
687 |
|| (A3_ANCHOR_ADDR == inst->instanceAddress16))) //A3 does not care about Final from A1 or A0
|
688 |
{ |
689 |
inst->testAppState = TA_RXE_WAIT ; // wait for next frame
|
690 |
break;
|
691 |
} |
692 |
|
693 |
if (A1_ANCHOR_ADDR == inst->instanceAddress16) { //this is A1 |
694 |
if(GATEWAY_ANCHOR_ADDR == (srcAddr[0] | ((uint32_t)(srcAddr[1] << 8)))) { //final is from A0 |
695 |
//ENABLE TIMER ONLY IF FINAL RECEIVED
|
696 |
inst->instanceTimerEn = 1;
|
697 |
} |
698 |
} |
699 |
//output data over USB...
|
700 |
inst->newRangeAncAddress = inst->instanceAddress16; |
701 |
|
702 |
//if we got the final, maybe the tag did not get our response, so
|
703 |
//we can use other anchors responses/ToF if there are any.. and output..
|
704 |
//but we cannot calculate new range
|
705 |
if(((validResp & (0x1<<(inst->shortAdd_idx))) != 0)) { |
706 |
// time of arrival of Final message
|
707 |
tagFinalRxTime = dw_event->timeStamp ; //Final's Rx time
|
708 |
|
709 |
//printf("FinalRx Timestamp: %4.15e\n", convertdevicetimetosecu(dw_event.timeStamp));
|
710 |
inst->delayedReplyTime = 0 ;
|
711 |
|
712 |
// times measured at Tag extracted from the message buffer
|
713 |
// extract 40bit times
|
714 |
memcpy(&tagPollTxTime, &(messageData[PTXT]), 5);
|
715 |
memcpy(&anchorRespRxTime, &(messageData[index]), 5);
|
716 |
memcpy(&tagFinalTxTime, &(messageData[FTXT]), 5);
|
717 |
|
718 |
// poll response round trip delay time is calculated as
|
719 |
// (anchorRespRxTime - tagPollTxTime) - (anchorRespTxTime - tagPollRxTime)
|
720 |
Ra = (int64_t)((anchorRespRxTime - tagPollTxTime) & MASK_40BIT); |
721 |
Db = (int64_t)((inst->txu.anchorRespTxTime - inst->tagPollRxTime) & MASK_40BIT); |
722 |
|
723 |
// response final round trip delay time is calculated as
|
724 |
// (tagFinalRxTime - anchorRespTxTime) - (tagFinalTxTime - anchorRespRxTime)
|
725 |
Rb = (int64_t)((tagFinalRxTime - inst->txu.anchorRespTxTime) & MASK_40BIT); |
726 |
Da = (int64_t)((tagFinalTxTime - anchorRespRxTime) & MASK_40BIT); |
727 |
|
728 |
RaRbxDaDb = (((double)Ra))*(((double)Rb)) |
729 |
- (((double)Da))*(((double)Db)); |
730 |
|
731 |
RbyDb = ((double)Rb + (double)Db); |
732 |
|
733 |
RayDa = ((double)Ra + (double)Da); |
734 |
|
735 |
tof = (int32_t)(RaRbxDaDb/(RbyDb + RayDa)); |
736 |
} |
737 |
|
738 |
//tag to anchor ranging
|
739 |
if(RTLS_DEMO_MSG_TAG_FINAL == fcode) {
|
740 |
inst->newRangeTagAddress = srcAddr[0] + ((uint16_t) srcAddr[1] << 8); |
741 |
//time-of-flight
|
742 |
inst->tof[inst->newRangeTagAddress & 0x7] = (uint32_t)tof;
|
743 |
//calculate all tag - anchor ranges... and report
|
744 |
inst->newRange = instance_calcranges(&inst->tofArray[0], MAX_ANCHOR_LIST_SIZE, TOF_REPORT_T2A, &inst->rxResponseMask);
|
745 |
inst->rxResponseMaskReport = inst->rxResponseMask; //copy the valid mask to report
|
746 |
inst->rxResponseMask = 0;
|
747 |
//we have our range - update the own mask entry...
|
748 |
if(tof != (int32_t)INVALID_TOF) { //check the last ToF entry is valid and copy into the current array |
749 |
setTagDist(srcAddr[0], inst->shortAdd_idx); //copy distance from this anchor to the tag into array |
750 |
|
751 |
inst->rxResponseMask = (uint8_t)(0x1 << inst->shortAdd_idx);
|
752 |
inst->tofArray[inst->shortAdd_idx] = (uint32_t)tof; |
753 |
} |
754 |
inst->newRangeTime = dw_event->uTimeStamp ; |
755 |
} |
756 |
else { //anchor to anchor ranging |
757 |
inst->newRangeTagAddress = srcAddr[0] + ((uint16_t) srcAddr[1] << 8); |
758 |
//time-of-flight
|
759 |
inst->tofAnc[tof_idx] = (uint32_t)tof; |
760 |
} |
761 |
|
762 |
//reset the response count
|
763 |
if(inst->rxResps[inst->rxRespsIdx] >= 0) { |
764 |
inst->rxResps[inst->rxRespsIdx] = -1 * inst->rxResps[inst->rxRespsIdx];
|
765 |
if(inst->rxResps[inst->rxRespsIdx] == 0) //as A0 will have this as 0 when ranging to A1 |
766 |
inst->rxResps[inst->rxRespsIdx] = -1 ;
|
767 |
} |
768 |
|
769 |
instancesetantennadelays(); //this will update the antenna delay if it has changed
|
770 |
instancesettxpower(); // configure TX power if it has changed
|
771 |
|
772 |
inst->testAppState = TA_RXE_WAIT ; // wait for next frame
|
773 |
|
774 |
} |
775 |
break; //RTLS_DEMO_MSG_TAG_FINAL |
776 |
|
777 |
|
778 |
default: {
|
779 |
//only enable receiver when not using double buffering
|
780 |
inst->testAppState = TA_RXE_WAIT ; // wait for next frame
|
781 |
dwt_setrxaftertxdelay(0);
|
782 |
|
783 |
} |
784 |
break;
|
785 |
} //end switch (fcode)
|
786 |
|
787 |
if(dw_event->msgu.frame[0] & 0x20){ |
788 |
//as we only pass the received frame with the ACK request bit set after the ACK has been sent
|
789 |
instance_getevent(16); //get and clear the ACK sent event |
790 |
} |
791 |
} //end else
|
792 |
|
793 |
} |
794 |
break ; //end of DWT_SIG_RX_OKAY |
795 |
|
796 |
case DWT_SIG_RX_TIMEOUT :{
|
797 |
|
798 |
event_data_t* dw_event = instance_getevent(17); //get and clear this event |
799 |
|
800 |
//printf("PD_DATA_TIMEOUT %d\n", inst->previousState) ;
|
801 |
|
802 |
//Anchor can time out and then need to send response - so will be in TX pending
|
803 |
if(dw_event->type_pend == DWT_SIG_TX_PENDING) {
|
804 |
inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
|
805 |
inst->previousState = TA_TXRESPONSE_SENT_TORX ; //wait for TX confirmation of sent response
|
806 |
} |
807 |
else if(dw_event->type_pend == DWT_SIG_DW_IDLE) { //if timed out and back in receive then don't process as timeout |
808 |
inst_processrxtimeout(inst); |
809 |
} |
810 |
//else if RX_PENDING then wait for next RX event...
|
811 |
message = 0; //clear the message as we have processed the event |
812 |
} |
813 |
break ;
|
814 |
|
815 |
case DWT_SIG_TX_AA_DONE: //ignore this event - just process the rx frame that was received before the ACK response |
816 |
case 0: |
817 |
default :{
|
818 |
if(message) { // == DWT_SIG_TX_DONE) |
819 |
inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT; |
820 |
} |
821 |
|
822 |
if(inst->done == INST_NOT_DONE_YET) inst->done = INST_DONE_WAIT_FOR_NEXT_EVENT;
|
823 |
} |
824 |
break;
|
825 |
|
826 |
} |
827 |
break ; // end case TA_RX_WAIT_DATA |
828 |
default:
|
829 |
//printf("\nERROR - invalid state %d - what is going on??\n", inst->testAppState) ;
|
830 |
break;
|
831 |
} // end switch on testAppState
|
832 |
|
833 |
return inst->done;
|
834 |
} // end testapprun()
|
835 |
|
836 |
// -------------------------------------------------------------------------------------------------------------------
|
837 |
#if NUM_INST != 1 |
838 |
#error These functions assume one instance only
|
839 |
#else
|
840 |
|
841 |
|
842 |
// -------------------------------------------------------------------------------------------------------------------
|
843 |
// function to set the fixed reply delay time (in us)
|
844 |
//
|
845 |
// This sets delay for RX to TX - Delayed Send, and for TX to RX delayed receive (wait for response) functionality,
|
846 |
// and the frame wait timeout value to use. This is a function of data rate, preamble length, and PRF
|
847 |
|
848 |
extern uint8_t dwnsSFDlen[];
|
849 |
|
850 |
void instancesetreplydelay(int delayus) { //delay in us |
851 |
|
852 |
int instance = 0; |
853 |
int margin = 3000; //2000 symbols |
854 |
int respframe = 0; |
855 |
int respframe_sy = 0; |
856 |
|
857 |
//configure the rx delay receive delay time, it is dependent on the message length
|
858 |
float msgdatalen = 0; |
859 |
float preamblelen = 0; |
860 |
int sfdlen = 0; |
861 |
float x = 0; |
862 |
|
863 |
//Set the RX timeouts based on the longest expected message - the Final message
|
864 |
//Poll = 13, Response = 20, Final = 44 bytes
|
865 |
//msgdatalen = TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
|
866 |
msgdatalen = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC; |
867 |
|
868 |
x = (float)ceil((double)(msgdatalen*8)/(double)330.0f); |
869 |
|
870 |
msgdatalen = msgdatalen*8 + x*48; |
871 |
|
872 |
//add some margin so we don't timeout too soon
|
873 |
margin = 0; //(TAG_FINAL_MSG_LEN - TAG_POLL_MSG_LEN); |
874 |
|
875 |
x = (float) ceil((double)(margin*8)/(double)330.0f); |
876 |
|
877 |
margin = (int) (margin*8 + x*48); |
878 |
|
879 |
//assume PHR length is 172308ns for 110k and 21539ns for 850k/6.81M
|
880 |
if(instance_data[instance].configData.dataRate == DWT_BR_110K) {
|
881 |
msgdatalen *= 8205.13f; |
882 |
msgdatalen += 172308; // PHR length in nanoseconds |
883 |
|
884 |
margin *= 8205.13f; |
885 |
|
886 |
} |
887 |
else if(instance_data[instance].configData.dataRate == DWT_BR_850K) { |
888 |
msgdatalen *= 1025.64f; |
889 |
msgdatalen += 21539; // PHR length in nanoseconds |
890 |
|
891 |
margin *= 1025.64f; |
892 |
} |
893 |
else {
|
894 |
msgdatalen *= 128.21f; |
895 |
msgdatalen += 21539; // PHR length in nanoseconds |
896 |
|
897 |
margin *= 128.21f; |
898 |
} |
899 |
|
900 |
//SFD length is 64 for 110k (always)
|
901 |
//SFD length is 8 for 6.81M, and 16 for 850k, but can vary between 8 and 16 bytes
|
902 |
sfdlen = dwnsSFDlen[instance_data[instance].configData.dataRate]; |
903 |
|
904 |
switch (instance_data[instance].configData.txPreambLength) {
|
905 |
case DWT_PLEN_4096 : preamblelen = 4096.0f; break; |
906 |
case DWT_PLEN_2048 : preamblelen = 2048.0f; break; |
907 |
case DWT_PLEN_1536 : preamblelen = 1536.0f; break; |
908 |
case DWT_PLEN_1024 : preamblelen = 1024.0f; break; |
909 |
case DWT_PLEN_512 : preamblelen = 512.0f; break; |
910 |
case DWT_PLEN_256 : preamblelen = 256.0f; break; |
911 |
case DWT_PLEN_128 : preamblelen = 128.0f; break; |
912 |
case DWT_PLEN_64 : preamblelen = 64.0f; break; |
913 |
} |
914 |
|
915 |
//preamble = plen * (994 or 1018) depending on 16 or 64 PRF
|
916 |
if(instance_data[instance].configData.prf == DWT_PRF_16M) {
|
917 |
preamblelen = (sfdlen + preamblelen) * 0.99359f; |
918 |
} |
919 |
else {
|
920 |
preamblelen = (sfdlen + preamblelen) * 1.01763f; |
921 |
} |
922 |
|
923 |
respframe_sy = (16 + (int)((double)((double)preamblelen + ((double)(msgdatalen + margin)/1000.0))/ 1.0256)) ; |
924 |
|
925 |
//this is the delay used for the delayed transmit (when sending the response, and final messages)
|
926 |
instance_data[instance].pollTx2FinalTxDelay = convertmicrosectodevicetimeu (delayus); |
927 |
//the anchor to anchor ranging consist of A0 ranging to A1 and A2 and A1 ranging to A2
|
928 |
//as there are less messages the ranging time is shorter (thus divide by 2)
|
929 |
instance_data[instance].pollTx2FinalTxDelayAnc = convertmicrosectodevicetimeu (delayus/2 + 100); |
930 |
|
931 |
//this is the delay the anchors 1, 2, etc.. will send the response back at...
|
932 |
//anchor 2 will have the delay set to 2 * fixedReplyDelayAnc
|
933 |
//andhor 3 will have the delay set to 3 * fixedReplyDelayAnc and so on...
|
934 |
//this delay depends on how quickly the tag can receive and process the message from previous anchor
|
935 |
//(and also the frame length of course)
|
936 |
respframe = (int)((double)preamblelen + ((double)msgdatalen/1000.0)); //length of response frame (micro seconds) |
937 |
if(instance_data[instance].configData.dataRate == DWT_BR_110K) {
|
938 |
|
939 |
//set the frame wait timeout time - total time the frame takes in symbols
|
940 |
instance_data[instance].fwtoTime_sy = respframe_sy + RX_RESPONSE1_TURNAROUND_110K + 400; //add some margin because of the resp to resp RX turn on time |
941 |
|
942 |
instance_data[instance].fwtoTimeAnc_sy = respframe_sy; //add some margin so we don't timeout too soon
|
943 |
instance_data[instance].fixedReplyDelayAnc = convertmicrosectodevicetimeu (respframe + RX_RESPONSE1_TURNAROUND_110K); |
944 |
instance_data[instance].fixedReplyDelayAncP = (uint32_t) (((uint64_t) convertmicrosectodevicetimeu ((double)preamblelen)) >> 8) + 16; |
945 |
|
946 |
instance_data[instance].ancRespRxDelay = RX_RESPONSE1_TURNAROUND_110K ; |
947 |
} |
948 |
else {
|
949 |
|
950 |
//set the frame wait timeout time - total time the frame takes in symbols
|
951 |
instance_data[instance].fwtoTime_sy = respframe_sy + RX_RESPONSE1_TURNAROUND_6M81; //add some margin because of the resp to resp RX turn on time
|
952 |
|
953 |
instance_data[instance].fwtoTimeAnc_sy = respframe_sy; |
954 |
instance_data[instance].fixedReplyDelayAnc = convertmicrosectodevicetimeu (respframe + RX_RESPONSE1_TURNAROUND_6M81); |
955 |
instance_data[instance].fixedReplyDelayAncP = (uint32_t) (((uint64_t) convertmicrosectodevicetimeu ((double)preamblelen)) >> 8) + 16; |
956 |
|
957 |
instance_data[instance].ancRespRxDelay = RX_RESPONSE1_TURNAROUND_6M81 ; |
958 |
} |
959 |
} |
960 |
|
961 |
// -------------------------------------------------------------------------------------------------------------------
|
962 |
//
|
963 |
// Set Payload parameters for the instance
|
964 |
//
|
965 |
// -------------------------------------------------------------------------------------------------------------------
|
966 |
void instancesetaddresses(uint16_t address) {
|
967 |
int instance = 0 ; |
968 |
|
969 |
instance_data[instance].instanceAddress16 = address ; // copy configurations
|
970 |
} |
971 |
|
972 |
|
973 |
#endif
|
974 |
|
975 |
|
976 |
|
977 |
|
978 |
/* ==========================================================
|
979 |
|
980 |
Notes:
|
981 |
|
982 |
Previously code handled multiple instances in a single console application
|
983 |
|
984 |
Now have changed it to do a single instance only. With minimal code changes...(i.e. kept [instance] index but it is always 0.
|
985 |
|
986 |
Windows application should call instance_init() once and then in the "main loop" call instance_run().
|
987 |
|
988 |
*/
|
989 |
|
990 |
|
991 |
#endif /* defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 1) */ |