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