amiro-lld / source / DW1000 / v0 / deca_instance_tag.c @ 1d5bcc82
History | View | Annotate | Download (33.801 KB)
1 | fce9feec | Robin Ewers | /*! ----------------------------------------------------------------------------
|
---|---|---|---|
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 | 1d5bcc82 | Thomas Schöpping | #include <alld_DW1000.h> |
15 | |||
16 | #if (defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 0)) || defined(__DOXYGEN__) |
||
17 | fce9feec | Robin Ewers | |
18 | #include <string.h> |
||
19 | |||
20 | |||
21 | // -------------------------------------------------------------------------------------------------------------------
|
||
22 | // Data Definitions
|
||
23 | // -------------------------------------------------------------------------------------------------------------------
|
||
24 | |||
25 | // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
|
||
26 | // NOTE: the maximum RX timeout is ~ 65ms
|
||
27 | // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
|
||
28 | |||
29 | |||
30 | // -------------------------------------------------------------------------------------------------------------------
|
||
31 | // Functions
|
||
32 | // -------------------------------------------------------------------------------------------------------------------
|
||
33 | |||
34 | /**
|
||
35 | * @brief this function either enables the receiver (delayed)
|
||
36 | *
|
||
37 | **/
|
||
38 | void tag_enable_rx(uint32_t dlyTime)
|
||
39 | { |
||
40 | instance_data_t* inst = instance_get_local_structure_ptr(0);
|
||
41 | //subtract preamble duration (because when instructing delayed TX the time is the time of SFD,
|
||
42 | //however when doing delayed RX the time is RX on time)
|
||
43 | dwt_setdelayedtrxtime(dlyTime - inst->preambleDuration32h) ; |
||
44 | if(dwt_rxenable(DWT_START_RX_DELAYED|DWT_IDLE_ON_DLY_ERR)) //delayed rx |
||
45 | { |
||
46 | //if the delayed RX failed - time has passed - do immediate enable
|
||
47 | //led_on(LED_PC9);
|
||
48 | dwt_setpreambledetecttimeout(0); //clear preamble timeout as RX is turned on early/late |
||
49 | dwt_setrxtimeout((uint16_t)inst->fwto4RespFrame_sy*2); //reconfigure the timeout before enable |
||
50 | //longer timeout as we cannot do delayed receive... so receiver needs to stay on for longer
|
||
51 | dwt_rxenable(DWT_START_RX_IMMEDIATE); |
||
52 | dwt_setpreambledetecttimeout(PTO_PACS); //configure preamble timeout
|
||
53 | dwt_setrxtimeout((uint16_t)inst->fwto4RespFrame_sy); //restore the timeout for next RX enable
|
||
54 | //inst->lateRX++;
|
||
55 | //led_off(LED_PC9);
|
||
56 | } |
||
57 | |||
58 | } |
||
59 | |||
60 | /* @fn instanceProcessRXTimeoutTag
|
||
61 | * @brief function to process RX timeout event
|
||
62 | * */
|
||
63 | void tag_process_rx_timeout(instance_data_t *inst)
|
||
64 | { |
||
65 | //inst->rxTimeouts ++ ;
|
||
66 | |||
67 | #if(DISCOVERY == 1) |
||
68 | if(inst->twrMode == GREETER)
|
||
69 | { |
||
70 | inst->instToSleep = TRUE ; |
||
71 | // initiate the re-transmission of the poll that was not responded to
|
||
72 | inst->testAppState = TA_TXE_WAIT ; |
||
73 | inst->nextState = TA_TXBLINK_WAIT_SEND ; |
||
74 | } |
||
75 | else
|
||
76 | #endif
|
||
77 | { |
||
78 | |||
79 | #if (TAG_HASTO_RANGETO_A0 == 0) |
||
80 | if(inst->rxResponseMask == 0) //if any response have been received send a Final else go to SLEEP |
||
81 | { |
||
82 | inst->instToSleep = TRUE ; //set sleep to TRUE so that tag will go to DEEP SLEEP before next ranging attempt
|
||
83 | inst->testAppState = TA_TXE_WAIT ; |
||
84 | inst->nextState = TA_TXPOLL_WAIT_SEND ; |
||
85 | } |
||
86 | #else
|
||
87 | |||
88 | //if tag times out - no response (check if we are to send a final)
|
||
89 | //send the final only if it has received response from anchor 0
|
||
90 | if((inst->previousState == TA_TXPOLL_WAIT_SEND)
|
||
91 | && ((inst->rxResponseMask & 0x1) == 0) |
||
92 | ) |
||
93 | { |
||
94 | inst->instToSleep = TRUE ; //set sleep to TRUE so that tag will go to DEEP SLEEP before next ranging attempt
|
||
95 | inst->testAppState = TA_TXE_WAIT ; |
||
96 | inst->nextState = TA_TXPOLL_WAIT_SEND ; |
||
97 | } |
||
98 | #endif
|
||
99 | else if (inst->previousState == TA_TXFINAL_WAIT_SEND) //got here from main (error sending final - handle as timeout) |
||
100 | { |
||
101 | dwt_forcetrxoff(); //this will clear all events
|
||
102 | inst->instToSleep = TRUE ; |
||
103 | // initiate the re-transmission of the poll that was not responded to
|
||
104 | inst->testAppState = TA_TXE_WAIT ; |
||
105 | inst->nextState = TA_TXPOLL_WAIT_SEND ; |
||
106 | } |
||
107 | else //send the final |
||
108 | { |
||
109 | // initiate the transmission of the final
|
||
110 | inst->testAppState = TA_TXE_WAIT ; |
||
111 | inst->nextState = TA_TXFINAL_WAIT_SEND ; |
||
112 | } |
||
113 | |||
114 | } |
||
115 | } |
||
116 | |||
117 | /**
|
||
118 | * @brief function to re-enable the receiver and also adjust the timeout before sending the final message
|
||
119 | * if it is time so send the final message, the callback will notify the application, else the receiver is
|
||
120 | * automatically re-enabled
|
||
121 | *
|
||
122 | * this function is only used for tag when ranging to other anchors
|
||
123 | */
|
||
124 | uint8_t tag_rx_reenable(uint16_t sourceAddress, uint8_t error) |
||
125 | { |
||
126 | uint8_t type_pend = DWT_SIG_DW_IDLE; |
||
127 | uint8_t anc = sourceAddress & 0x3;
|
||
128 | instance_data_t* inst = instance_get_local_structure_ptr(0);
|
||
129 | |||
130 | switch(anc)
|
||
131 | { |
||
132 | //if we got Response from anchor 3 - this is the last expected response - send the final
|
||
133 | case 3: |
||
134 | type_pend = DWT_SIG_DW_IDLE; |
||
135 | break;
|
||
136 | |||
137 | //if we got response from anchor 0, 1, or 2 - go back to wait for next anchor's response
|
||
138 | //if we got response from 0, then still expecting 3, so remainingRespToRx set to 3
|
||
139 | case 0: |
||
140 | case 1: |
||
141 | case 2: |
||
142 | default:
|
||
143 | if(inst->remainingRespToRx > 0) //can get here as result of error frame so need to check |
||
144 | { |
||
145 | //can't use anc address as this is an error frame, so just re-enable TO based on remainingRespToRx count
|
||
146 | if(error == 0) |
||
147 | { |
||
148 | switch (anc)
|
||
149 | { |
||
150 | case 0: |
||
151 | inst->remainingRespToRx = 3; //expecting 3 more responses |
||
152 | break;
|
||
153 | case 1: |
||
154 | inst->remainingRespToRx = 2; //expecting 2 more responses |
||
155 | break;
|
||
156 | case 2: |
||
157 | inst->remainingRespToRx = 1; //expecting 1 more response |
||
158 | break;
|
||
159 | } |
||
160 | } |
||
161 | //Poll sent at tagPollTxTime_32bit
|
||
162 | //1st response is delayTime + fixedReplyDelayAnc32h - preambleDuration_32MSBs
|
||
163 | //2nd is delayTime + fixedReplyDelayAnc32h - preambleDuration_32MSBs + fixedReplyDelayAnc32h
|
||
164 | tag_enable_rx(inst->tagPollTxTime32h + |
||
165 | (MAX_ANCHOR_LIST_SIZE-inst->remainingRespToRx+1)*(inst->fixedReplyDelayAnc32h));
|
||
166 | |||
167 | type_pend = DWT_SIG_RX_PENDING ; |
||
168 | } |
||
169 | else //finished waiting for responses - no responses left to receive... send a final |
||
170 | { |
||
171 | type_pend = DWT_SIG_DW_IDLE; //report timeout - send the final if due to be sent
|
||
172 | } |
||
173 | break;
|
||
174 | |||
175 | } |
||
176 | |||
177 | return type_pend;
|
||
178 | } |
||
179 | |||
180 | /**
|
||
181 | * @brief this function handles frame error event, it will either signal TO or re-enable the receiver
|
||
182 | */
|
||
183 | void tag_handle_error_unknownframe(event_data_t dw_event)
|
||
184 | { |
||
185 | instance_data_t* inst = instance_get_local_structure_ptr(0);
|
||
186 | |||
187 | if(inst->twrMode != GREETER)
|
||
188 | { |
||
189 | //re-enable the receiver (after error frames as we are not using auto re-enable
|
||
190 | //for ranging application rx error frame is same as TO - as we are not going to get the expected frame
|
||
191 | inst->remainingRespToRx--; //got something (need to reduce timeout (for remaining responses))
|
||
192 | |||
193 | dw_event.typePend = tag_rx_reenable(0, 1); //check if receiver will be re-enabled or it's time to send the final |
||
194 | } |
||
195 | else
|
||
196 | { |
||
197 | dw_event.typePend = DWT_SIG_DW_IDLE; //in GREETER mode only waiting for 1 frame
|
||
198 | } |
||
199 | |||
200 | dw_event.type = 0;
|
||
201 | //dw_event.typeSave = 0x40 | DWT_SIG_RX_TIMEOUT;
|
||
202 | dw_event.rxLength = 0;
|
||
203 | |||
204 | instance_putevent(dw_event, DWT_SIG_RX_TIMEOUT); |
||
205 | } |
||
206 | |||
207 | /**
|
||
208 | * @brief this is the receive timeout event callback handler
|
||
209 | */
|
||
210 | void rx_to_cb_tag(const dwt_cb_data_t *rxd) |
||
211 | { |
||
212 | (void) rxd;
|
||
213 | event_data_t dw_event; |
||
214 | |||
215 | //microcontroller time at which we received the frame
|
||
216 | dw_event.uTimeStamp = portGetTickCnt(); |
||
217 | tag_handle_error_unknownframe(dw_event); |
||
218 | } |
||
219 | |||
220 | /**
|
||
221 | * @brief this is the receive error event callback handler
|
||
222 | */
|
||
223 | void rx_err_cb_tag(const dwt_cb_data_t *rxd) |
||
224 | { |
||
225 | (void) rxd;
|
||
226 | event_data_t dw_event; |
||
227 | |||
228 | //microcontroller time at which we received the frame
|
||
229 | dw_event.uTimeStamp = portGetTickCnt(); |
||
230 | tag_handle_error_unknownframe(dw_event); |
||
231 | } |
||
232 | |||
233 | /**
|
||
234 | * @brief this is the receive event callback handler, the received event is processed and the instance either
|
||
235 | * responds by sending a response frame or re-enables the receiver to await the next frame
|
||
236 | * once the immediate action is taken care of the event is queued up for application to process
|
||
237 | */
|
||
238 | void rx_ok_cb_tag(const dwt_cb_data_t *rxd) |
||
239 | { |
||
240 | instance_data_t* inst = instance_get_local_structure_ptr(0);
|
||
241 | uint8_t rxTimeStamp[5] = {0, 0, 0, 0, 0}; |
||
242 | |||
243 | uint8_t rxd_event = 0;
|
||
244 | uint8_t fcode_index = 0;
|
||
245 | uint8_t srcAddr_index = 0;
|
||
246 | event_data_t dw_event; |
||
247 | |||
248 | //microcontroller time at which we received the frame
|
||
249 | dw_event.uTimeStamp = portGetTickCnt(); |
||
250 | |||
251 | //if we got a frame with a good CRC - RX OK
|
||
252 | { |
||
253 | dw_event.rxLength = rxd->datalength; |
||
254 | |||
255 | //need to process the frame control bytes to figure out what type of frame we have received
|
||
256 | if(rxd->fctrl[0] == 0x41) |
||
257 | { |
||
258 | if((rxd->fctrl[1] & 0xCC) == 0x88) //short address |
||
259 | { |
||
260 | fcode_index = FRAME_CRTL_AND_ADDRESS_S; //function code is in first byte after source address
|
||
261 | srcAddr_index = FRAME_CTRLP + ADDR_BYTE_SIZE_S; |
||
262 | rxd_event = DWT_SIG_RX_OKAY; |
||
263 | } |
||
264 | #if (DISCOVERY == 1) |
||
265 | else if((rxd->fctrl[1] & 0xCC) == 0x8c) //long/short address - ranging init message |
||
266 | { |
||
267 | fcode_index = FRAME_CRTL_AND_ADDRESS_LS; //function code is in first byte after source address
|
||
268 | srcAddr_index = FRAME_CTRLP + ADDR_BYTE_SIZE_L; |
||
269 | rxd_event = DWT_SIG_RX_OKAY; |
||
270 | } |
||
271 | #endif
|
||
272 | else
|
||
273 | { |
||
274 | rxd_event = SIG_RX_UNKNOWN; //not supported - all TREK1000 frames are short addressed
|
||
275 | } |
||
276 | } |
||
277 | else
|
||
278 | { |
||
279 | rxd_event = SIG_RX_UNKNOWN; //not supported - all TREK1000 frames are short addressed
|
||
280 | } |
||
281 | |||
282 | //read RX timestamp
|
||
283 | dwt_readrxtimestamp(rxTimeStamp) ; |
||
284 | dwt_readrxdata((uint8_t *)&dw_event.msgu.frame[0], rxd->datalength, 0); // Read Data Frame |
||
285 | instance_seteventtime(&dw_event, rxTimeStamp); |
||
286 | |||
287 | dw_event.type = 0; //type will be added as part of adding to event queue |
||
288 | //dw_event.typeSave = rxd_event;
|
||
289 | dw_event.typePend = DWT_SIG_DW_IDLE; |
||
290 | |||
291 | if(rxd_event == DWT_SIG_RX_OKAY) //Process good/known frame types |
||
292 | { |
||
293 | uint16_t sourceAddress = (((uint16_t)dw_event.msgu.frame[srcAddr_index+1]) << 8) + dw_event.msgu.frame[srcAddr_index]; |
||
294 | |||
295 | //if tag got a good frame - this is probably a response, but could also be some other non-ranging frame
|
||
296 | //(although due to frame filtering this is limited as non-addressed frames are filtered out)
|
||
297 | |||
298 | //check if this is a TWR message (and also which one)
|
||
299 | switch(dw_event.msgu.frame[fcode_index])
|
||
300 | { |
||
301 | //we got a response from a "responder" (anchor)
|
||
302 | case RTLS_DEMO_MSG_ANCH_RESP:
|
||
303 | { |
||
304 | if(inst->twrMode == INITIATOR)
|
||
305 | { |
||
306 | //if tag is involved in the ranging exchange expecting responses
|
||
307 | uint8_t index ; |
||
308 | inst->remainingRespToRx--; //got 1 more response or other RX frame - need to reduce timeout (for next response)
|
||
309 | dw_event.typePend = tag_rx_reenable(sourceAddress, 0); //remainingRespToRx decremented above... |
||
310 | index = RRXT0 + 5*(sourceAddress & 0x3); |
||
311 | |||
312 | inst->rxResponseMask |= (0x1 << (sourceAddress & 0x3)); //add anchor ID to the mask |
||
313 | // Write Response RX time field of Final message
|
||
314 | memcpy(&(inst->msg_f.messageData[index]), rxTimeStamp, 5);
|
||
315 | break;
|
||
316 | } else {
|
||
317 | __attribute__ ((fallthrough)); |
||
318 | } |
||
319 | } |
||
320 | #if (DISCOVERY == 1) |
||
321 | case RTLS_DEMO_MSG_RNG_INIT:
|
||
322 | { |
||
323 | if(inst->twrMode == GREETER)
|
||
324 | { |
||
325 | rxd_event = RTLS_DEMO_MSG_RNG_INIT; |
||
326 | break; //process the event in the application |
||
327 | } |
||
328 | } |
||
329 | #endif
|
||
330 | case RTLS_DEMO_MSG_ANCH_POLL:
|
||
331 | case RTLS_DEMO_MSG_TAG_POLL:
|
||
332 | case RTLS_DEMO_MSG_TAG_FINAL:
|
||
333 | case RTLS_DEMO_MSG_ANCH_FINAL:
|
||
334 | case RTLS_DEMO_MSG_ANCH_RESP2:
|
||
335 | default:
|
||
336 | //tag should ignore any other frames - only receive responses
|
||
337 | { |
||
338 | tag_handle_error_unknownframe(dw_event); |
||
339 | //inst->rxMsgCount++;
|
||
340 | return;
|
||
341 | } |
||
342 | } |
||
343 | instance_putevent(dw_event, rxd_event); |
||
344 | |||
345 | //inst->rxMsgCount++;
|
||
346 | } |
||
347 | else //if (rxd_event == SIG_RX_UNKNOWN) //need to re-enable the rx (got unknown frame type) |
||
348 | { |
||
349 | tag_handle_error_unknownframe(dw_event); |
||
350 | } |
||
351 | } |
||
352 | } |
||
353 | // -------------------------------------------------------------------------------------------------------------------
|
||
354 | //
|
||
355 | // the main instance state machine for tag application
|
||
356 | //
|
||
357 | // -------------------------------------------------------------------------------------------------------------------
|
||
358 | //
|
||
359 | int tag_app_run(instance_data_t *inst)
|
||
360 | { |
||
361 | int instDone = INST_NOT_DONE_YET;
|
||
362 | int message = instance_peekevent(); //get any of the received events from ISR |
||
363 | |||
364 | switch (inst->testAppState)
|
||
365 | { |
||
366 | case TA_INIT :
|
||
367 | // printf("TA_INIT") ;
|
||
368 | switch (inst->mode)
|
||
369 | { |
||
370 | case TAG:
|
||
371 | { |
||
372 | uint16_t sleep_mode = 0;
|
||
373 | |||
374 | dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN); //allow data, ack frames;
|
||
375 | |||
376 | inst->eui64[0] += inst->instanceAddress16; //so switch 5,6,7 can be used to emulate more tags |
||
377 | dwt_seteui(inst->eui64); |
||
378 | |||
379 | dwt_setpanid(inst->panID); |
||
380 | #if (DISCOVERY == 1) |
||
381 | //Start off by sending Blinks and wait for Anchor to send Ranging Init
|
||
382 | inst->testAppState = TA_TXBLINK_WAIT_SEND; |
||
383 | inst->tagSleepTime_ms = BLINK_PERIOD ; |
||
384 | memcpy(inst->blinkmsg.tagID, inst->eui64, ADDR_BYTE_SIZE_L); |
||
385 | inst->newRangeTagAddress = inst->eui64[1];
|
||
386 | inst->newRangeTagAddress = (inst->newRangeTagAddress << 8) + inst->eui64[0]; |
||
387 | |||
388 | #else
|
||
389 | //dwt_setpanid(inst->panID);
|
||
390 | memcpy(inst->eui64, &inst->instanceAddress16, ADDR_BYTE_SIZE_S); |
||
391 | //set source address
|
||
392 | inst->newRangeTagAddress = inst->instanceAddress16 ; |
||
393 | dwt_setaddress16(inst->instanceAddress16); |
||
394 | |||
395 | //Start off by Sleeping 1st -> set instToSleep to TRUE
|
||
396 | inst->nextState = TA_TXPOLL_WAIT_SEND; |
||
397 | inst->testAppState = TA_TXE_WAIT; |
||
398 | inst->instToSleep = TRUE ; |
||
399 | inst->tagSleepTime_ms = inst->tagPeriod_ms ; |
||
400 | #endif
|
||
401 | inst->rangeNum = 0;
|
||
402 | inst->tagSleepCorrection_ms = 0;
|
||
403 | |||
404 | sleep_mode = (DWT_PRESRV_SLEEP|DWT_CONFIG|DWT_TANDV); |
||
405 | |||
406 | if(inst->configData.txPreambLength == DWT_PLEN_64) //if using 64 length preamble then use the corresponding OPSet |
||
407 | sleep_mode |= DWT_LOADOPSET; |
||
408 | |||
409 | #if (DEEP_SLEEP == 1) |
||
410 | dwt_configuresleep(sleep_mode, DWT_WAKE_WK|DWT_WAKE_CS|DWT_SLP_EN); //configure the on wake parameters (upload the IC config settings)
|
||
411 | #endif
|
||
412 | instance_config_frameheader_16bit(inst); |
||
413 | inst->instanceWakeTime_ms = portGetTickCnt(); |
||
414 | } |
||
415 | break;
|
||
416 | default:
|
||
417 | break;
|
||
418 | } |
||
419 | break; // end case TA_INIT |
||
420 | |||
421 | case TA_SLEEP_DONE :
|
||
422 | { |
||
423 | event_data_t* dw_event = instance_getevent(10); //clear the event from the queue |
||
424 | // waiting for timout from application to wakeup IC
|
||
425 | if (dw_event->type != DWT_SIG_RX_TIMEOUT)
|
||
426 | { |
||
427 | // if no pause and no wake-up timeout continu waiting for the sleep to be done.
|
||
428 | instDone = INST_DONE_WAIT_FOR_NEXT_EVENT; //wait here for sleep timeout
|
||
429 | break;
|
||
430 | } |
||
431 | |||
432 | instDone = INST_NOT_DONE_YET; |
||
433 | inst->instToSleep = FALSE ; |
||
434 | inst->testAppState = inst->nextState; |
||
435 | inst->nextState = 0; //clear |
||
436 | inst->instanceWakeTime_ms = portGetTickCnt(); // Record the time count when we wake-up
|
||
437 | #if (DEEP_SLEEP == 1) |
||
438 | { |
||
439 | //wake up device from low power mode
|
||
440 | //led_on(LED_PC9);
|
||
441 | |||
442 | port_wakeup_dw1000_fast(); |
||
443 | |||
444 | //led_off(LED_PC9);
|
||
445 | |||
446 | //this is platform dependent - only program if DW EVK/EVB
|
||
447 | dwt_setleds(1);
|
||
448 | |||
449 | //MP bug - TX antenna delay needs reprogramming as it is not preserved (only RX)
|
||
450 | dwt_settxantennadelay(inst->txAntennaDelay) ; |
||
451 | #if(DISCOVERY == 0) |
||
452 | //set EUI as it will not be preserved unless the EUI is programmed and loaded from NVM
|
||
453 | dwt_seteui(inst->eui64); |
||
454 | #endif
|
||
455 | } |
||
456 | #else
|
||
457 | Sleep(3); //to approximate match the time spent in the #if above |
||
458 | #endif
|
||
459 | |||
460 | instance_set_antennadelays(); //this will update the antenna delay if it has changed
|
||
461 | instance_set_txpower(); //configure TX power if it has changed
|
||
462 | #if (READ_EVENT_COUNTERS == 1) |
||
463 | dwt_configeventcounters(1);
|
||
464 | #endif
|
||
465 | } |
||
466 | break;
|
||
467 | |||
468 | case TA_TXE_WAIT : //either go to sleep or proceed to TX a message |
||
469 | //if we are scheduled to go to sleep before next transmission then sleep first.
|
||
470 | #if (DISCOVERY == 1) |
||
471 | if(((inst->nextState == TA_TXPOLL_WAIT_SEND)
|
||
472 | || (inst->nextState == TA_TXBLINK_WAIT_SEND)) |
||
473 | #else
|
||
474 | if((inst->nextState == TA_TXPOLL_WAIT_SEND)
|
||
475 | #endif
|
||
476 | && (inst->instToSleep) //go to sleep before sending the next poll/ starting new ranging exchange
|
||
477 | ) |
||
478 | { |
||
479 | inst->rangeNum++; //increment the range number before going to sleep
|
||
480 | //the app should put chip into low power state and wake up after tagSleepTime_ms time...
|
||
481 | //the app could go to *_IDLE state and wait for uP to wake it up...
|
||
482 | instDone = INST_DONE_WAIT_FOR_NEXT_EVENT_TO; //don't sleep here but kick off the Sleep timer countdown
|
||
483 | inst->testAppState = TA_SLEEP_DONE; |
||
484 | |||
485 | { |
||
486 | #if (READ_EVENT_COUNTERS == 1) |
||
487 | dwt_readeventcounters(&inst->ecounters); |
||
488 | #endif
|
||
489 | #if (DEEP_SLEEP == 1) |
||
490 | //put device into low power mode
|
||
491 | dwt_entersleep(); //go to sleep
|
||
492 | #endif
|
||
493 | if(inst->rxResponseMask != 0) |
||
494 | { |
||
495 | //DW1000 gone to sleep - report the received range
|
||
496 | inst->newRange = instance_calc_ranges(&inst->tofArray[0], MAX_ANCHOR_LIST_SIZE, TOF_REPORT_T2A, &inst->rxResponseMask);
|
||
497 | inst->rxResponseMaskReport = inst->rxResponseMask; |
||
498 | inst->rxResponseMask = 0;
|
||
499 | inst->newRangeTime = portGetTickCnt() ; |
||
500 | } |
||
501 | } |
||
502 | |||
503 | } |
||
504 | else //proceed to configuration and transmission of a frame |
||
505 | { |
||
506 | inst->testAppState = inst->nextState; |
||
507 | inst->nextState = 0; //clear |
||
508 | } |
||
509 | break ; // end case TA_TXE_WAIT |
||
510 | case TA_TXBLINK_WAIT_SEND :
|
||
511 | { |
||
512 | int flength = (BLINK_FRAME_CRTL_AND_ADDRESS + FRAME_CRC);
|
||
513 | |||
514 | //blink frames with IEEE EUI-64 tag ID
|
||
515 | inst->blinkmsg.frameCtrl = 0xC5 ;
|
||
516 | inst->blinkmsg.seqNum = inst->frameSN++; |
||
517 | |||
518 | dwt_writetxdata(flength, (uint8_t *) (&inst->blinkmsg), 0) ; // write the frame data |
||
519 | dwt_writetxfctrl(flength, 0, 1); |
||
520 | |||
521 | inst->twrMode = GREETER; |
||
522 | //using wait for response to do delayed receive
|
||
523 | inst->wait4ack = DWT_RESPONSE_EXPECTED; |
||
524 | inst->rxResponseMask = 0;
|
||
525 | |||
526 | dwt_setrxtimeout((uint16_t)inst->fwto4RespFrame_sy*2); //units are symbols (x2 as ranging init > response) |
||
527 | //set the delayed rx on time (the ranging init will be sent after this delay)
|
||
528 | dwt_setrxaftertxdelay((uint32_t)inst->tagRespRxDelay_sy); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
|
||
529 | |||
530 | dwt_starttx(DWT_START_TX_IMMEDIATE | inst->wait4ack); //always using immediate TX and enable delayed RX
|
||
531 | |||
532 | inst->instToSleep = 1; //go to Sleep after this blink |
||
533 | inst->testAppState = TA_RX_WAIT_DATA ; // to to RX, expecting ranging init response
|
||
534 | inst->previousState = TA_TXBLINK_WAIT_SEND ; |
||
535 | instDone = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below)
|
||
536 | |||
537 | } |
||
538 | break ; // end case TA_TXBLINK_WAIT_SEND |
||
539 | |||
540 | case TA_TXPOLL_WAIT_SEND :
|
||
541 | { |
||
542 | inst->msg_f.messageData[POLL_RNUM] = inst->rangeNum; //copy new range number
|
||
543 | inst->msg_f.messageData[FCODE] = RTLS_DEMO_MSG_TAG_POLL; //message function code (specifies if message is a poll, response or other...)
|
||
544 | inst->psduLength = (TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC); |
||
545 | inst->msg_f.seqNum = inst->frameSN++; //copy sequence number and then increment
|
||
546 | inst->msg_f.sourceAddr[0] = inst->instanceAddress16 & 0xff; //inst->eui64[0]; //copy the address |
||
547 | inst->msg_f.sourceAddr[1] = (inst->instanceAddress16>>8) & 0xff; //inst->eui64[1]; //copy the address |
||
548 | inst->msg_f.destAddr[0] = 0xff; //set the destination address (broadcast == 0xffff) |
||
549 | inst->msg_f.destAddr[1] = 0xff; //set the destination address (broadcast == 0xffff) |
||
550 | dwt_writetxdata(inst->psduLength, (uint8_t *) &inst->msg_f, 0) ; // write the frame data |
||
551 | |||
552 | //set the delayed rx on time (the response message will be sent after this delay (from A0))
|
||
553 | dwt_setrxaftertxdelay((uint32_t)inst->tagRespRxDelay_sy); //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
|
||
554 | |||
555 | inst->remainingRespToRx = MAX_ANCHOR_LIST_SIZE; //expecting 4 responses
|
||
556 | dwt_setrxtimeout((uint16_t)inst->fwto4RespFrame_sy); //configure the RX FWTO
|
||
557 | dwt_setpreambledetecttimeout(PTO_PACS); //configure preamble timeout
|
||
558 | |||
559 | inst->rxResponseMask = 0; //reset/clear the mask of received responses when tx poll |
||
560 | |||
561 | inst->wait4ack = DWT_RESPONSE_EXPECTED; //response is expected - automatically enable the receiver
|
||
562 | |||
563 | dwt_writetxfctrl(inst->psduLength, 0, 1); //write frame control |
||
564 | |||
565 | inst->twrMode = INITIATOR; |
||
566 | |||
567 | dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED); //transmit the frame
|
||
568 | |||
569 | inst->testAppState = TA_TX_WAIT_CONF ; // wait confirmation
|
||
570 | inst->previousState = TA_TXPOLL_WAIT_SEND ; |
||
571 | instDone = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set above)
|
||
572 | |||
573 | } |
||
574 | break;
|
||
575 | |||
576 | case TA_TXFINAL_WAIT_SEND :
|
||
577 | { |
||
578 | //the final has the same range number as the poll (part of the same ranging exchange)
|
||
579 | inst->msg_f.messageData[POLL_RNUM] = inst->rangeNum; |
||
580 | //the mask is sent so the anchors know whether the response RX time is valid
|
||
581 | inst->msg_f.messageData[VRESP] = inst->rxResponseMask; |
||
582 | inst->msg_f.messageData[FCODE] = RTLS_DEMO_MSG_TAG_FINAL; //message function code (specifies if message is a poll, response or other...)
|
||
583 | inst->psduLength = (TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC); |
||
584 | inst->msg_f.seqNum = inst->frameSN++; |
||
585 | dwt_writetxdata(inst->psduLength, (uint8_t *) &inst->msg_f, 0) ; // write the frame data |
||
586 | |||
587 | inst->wait4ack = 0; //clear the flag not using wait for response as this message ends the ranging exchange |
||
588 | |||
589 | if(instance_send_delayed_frame(inst, DWT_START_TX_DELAYED))
|
||
590 | { |
||
591 | // initiate the re-transmission
|
||
592 | inst->testAppState = TA_TXE_WAIT ; //go to TA_TXE_WAIT first to check if it's sleep time
|
||
593 | inst->nextState = TA_TXPOLL_WAIT_SEND ; |
||
594 | inst->instToSleep = TRUE ; |
||
595 | break; //exit this switch case... |
||
596 | } |
||
597 | else
|
||
598 | { |
||
599 | inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
|
||
600 | } |
||
601 | |||
602 | inst->previousState = TA_TXFINAL_WAIT_SEND; |
||
603 | inst->instToSleep = TRUE ; |
||
604 | instDone = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set above)
|
||
605 | } |
||
606 | break;
|
||
607 | |||
608 | |||
609 | case TA_TX_WAIT_CONF :
|
||
610 | { |
||
611 | event_data_t* dw_event = instance_getevent(11); //get and clear this event |
||
612 | |||
613 | if(dw_event->type != DWT_SIG_TX_DONE) //wait for TX done confirmation |
||
614 | { |
||
615 | instDone = INST_DONE_WAIT_FOR_NEXT_EVENT; |
||
616 | break;
|
||
617 | } |
||
618 | |||
619 | instDone = INST_NOT_DONE_YET; |
||
620 | |||
621 | if(inst->previousState == TA_TXFINAL_WAIT_SEND)
|
||
622 | { |
||
623 | inst->testAppState = TA_TXE_WAIT ; |
||
624 | inst->nextState = TA_TXPOLL_WAIT_SEND ; |
||
625 | break;
|
||
626 | } |
||
627 | else
|
||
628 | { |
||
629 | inst->txu.txTimeStamp = dw_event->timeStamp; |
||
630 | inst->tagPollTxTime32h = dw_event->timeStamp32h; |
||
631 | |||
632 | if(inst->previousState == TA_TXPOLL_WAIT_SEND)
|
||
633 | { |
||
634 | uint64_t tagCalculatedFinalTxTime ; |
||
635 | // Embed into Final message: 40-bit pollTXTime, 40-bit respRxTime, 40-bit finalTxTime
|
||
636 | tagCalculatedFinalTxTime = (inst->txu.txTimeStamp + inst->pollTx2FinalTxDelay) & MASK_TXDTS; |
||
637 | |||
638 | inst->delayedTRXTime32h = tagCalculatedFinalTxTime >> 8; //high 32-bits |
||
639 | // Calculate Time Final message will be sent and write this field of Final message
|
||
640 | // Sending time will be delayedReplyTime, snapped to ~125MHz or ~250MHz boundary by
|
||
641 | // zeroing its low 9 bits, and then having the TX antenna delay added
|
||
642 | // getting antenna delay from the device and add it to the Calculated TX Time
|
||
643 | tagCalculatedFinalTxTime = tagCalculatedFinalTxTime + inst->txAntennaDelay; |
||
644 | tagCalculatedFinalTxTime &= MASK_40BIT; |
||
645 | |||
646 | // Write Calculated TX time field of Final message
|
||
647 | memcpy(&(inst->msg_f.messageData[FTXT]), (uint8_t *)&tagCalculatedFinalTxTime, 5);
|
||
648 | // Write Poll TX time field of Final message
|
||
649 | memcpy(&(inst->msg_f.messageData[PTXT]), (uint8_t *)&inst->txu.tagPollTxTime, 5);
|
||
650 | |||
651 | } |
||
652 | |||
653 | inst->testAppState = TA_RX_WAIT_DATA ; // After sending, tag expects response/report, anchor waits to receive a final/new poll
|
||
654 | |||
655 | message = 0;
|
||
656 | //fall into the next case (turn on the RX)
|
||
657 | __attribute__ ((fallthrough)); |
||
658 | } |
||
659 | |||
660 | } |
||
661 | |||
662 | //break ; // end case TA_TX_WAIT_CONF
|
||
663 | |||
664 | case TA_RX_WAIT_DATA : // Wait RX data |
||
665 | //printf("TA_RX_WAIT_DATA %d", message) ;
|
||
666 | |||
667 | switch (message)
|
||
668 | { |
||
669 | |||
670 | //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
|
||
671 | case DWT_SIG_RX_OKAY :
|
||
672 | { |
||
673 | event_data_t* dw_event = instance_getevent(15); //get and clear this event |
||
674 | uint8_t srcAddr[8] = {0,0,0,0,0,0,0,0}; |
||
675 | uint8_t dstAddr[8] = {0,0,0,0,0,0,0,0}; |
||
676 | int fcode = 0; |
||
677 | uint8_t tof_idx = 0;
|
||
678 | uint8_t *messageData; |
||
679 | |||
680 | memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ss.sourceAddr[0]), ADDR_BYTE_SIZE_S); |
||
681 | memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_ss.destAddr[0]), ADDR_BYTE_SIZE_S); |
||
682 | fcode = dw_event->msgu.rxmsg_ss.messageData[FCODE]; |
||
683 | messageData = &dw_event->msgu.rxmsg_ss.messageData[0];
|
||
684 | |||
685 | tof_idx = srcAddr[0] & 0x3 ; |
||
686 | //process ranging messages
|
||
687 | switch(fcode)
|
||
688 | { |
||
689 | case RTLS_DEMO_MSG_ANCH_RESP:
|
||
690 | { |
||
691 | uint8_t currentRangeNum = (messageData[TOFRN] + 1); //current = previous + 1 |
||
692 | |||
693 | if(GATEWAY_ANCHOR_ADDR == (srcAddr[0] | ((uint32_t)(srcAddr[1] << 8)))) //if response from gateway then use the correction factor |
||
694 | { |
||
695 | // int sleepCorrection = (int16_t) (((uint16_t) messageData[RES_TAG_SLP1] << 8) + messageData[RES_TAG_SLP0]);
|
||
696 | // casting received bytes to int because this is a signed correction -0.5 periods to +1.5 periods
|
||
697 | inst->tagSleepCorrection_ms = (int16_t) (((uint16_t) messageData[RES_TAG_SLP1] << 8) + messageData[RES_TAG_SLP0]);
|
||
698 | inst->tagSleepRnd_ms = 0; // once we have initial response from Anchor #0 the slot correction acts and we don't need this anymore |
||
699 | } |
||
700 | |||
701 | if(dw_event->typePend == DWT_SIG_RX_PENDING)
|
||
702 | { |
||
703 | // stay in TA_RX_WAIT_DATA - receiver is already enabled, waiting for next response.
|
||
704 | } |
||
705 | //DW1000 idle - send the final
|
||
706 | else //if(dw_event->type_pend == DWT_SIG_DW_IDLE) |
||
707 | { |
||
708 | #if (TAG_HASTO_RANGETO_A0 == 1) |
||
709 | if(inst->rxResponseMask & 0x1)//if this is tag and A0's response received send the final |
||
710 | #endif
|
||
711 | { |
||
712 | inst->testAppState = TA_TXFINAL_WAIT_SEND ; // send our response / the final
|
||
713 | } |
||
714 | #if (TAG_HASTO_RANGETO_A0 == 1) |
||
715 | else //go to sleep |
||
716 | { |
||
717 | inst->testAppState = TA_TXE_WAIT ; //go to TA_TXE_WAIT first to check if it's sleep time
|
||
718 | inst->nextState = TA_TXPOLL_WAIT_SEND ; |
||
719 | inst->instToSleep = TRUE; |
||
720 | } |
||
721 | #endif
|
||
722 | } |
||
723 | |||
724 | if(currentRangeNum == inst->rangeNum) //these are the previous ranges... |
||
725 | { |
||
726 | //copy the ToF and put into array (array holds last 4 ToFs)
|
||
727 | memcpy(&inst->tofArray[tof_idx], &(messageData[TOFR]), 4);
|
||
728 | |||
729 | //check if the ToF is valid, this makes sure we only report valid ToFs
|
||
730 | //e.g. consider the case of reception of response from anchor a1 (we are anchor a2)
|
||
731 | //if a1 got a Poll with previous Range number but got no Final, then the response will have
|
||
732 | //the correct range number but the range will be INVALID_TOF
|
||
733 | if(inst->tofArray[tof_idx] != INVALID_TOF)
|
||
734 | { |
||
735 | inst->rxResponseMask |= (0x1 << tof_idx);
|
||
736 | } |
||
737 | |||
738 | } |
||
739 | else
|
||
740 | { |
||
741 | if(inst->tofArray[tof_idx] != INVALID_TOF)
|
||
742 | { |
||
743 | inst->tofArray[tof_idx] = INVALID_TOF; |
||
744 | } |
||
745 | } |
||
746 | |||
747 | |||
748 | } |
||
749 | break; //RTLS_DEMO_MSG_ANCH_RESP |
||
750 | |||
751 | default:
|
||
752 | { |
||
753 | tag_process_rx_timeout(inst); //if unknown message process as timeout
|
||
754 | } |
||
755 | break;
|
||
756 | } //end switch (fcode)
|
||
757 | |||
758 | } |
||
759 | break ; //end of DWT_SIG_RX_OKAY |
||
760 | |||
761 | case RTLS_DEMO_MSG_RNG_INIT :
|
||
762 | { |
||
763 | event_data_t* dw_event = instance_getevent(16); //get and clear this event |
||
764 | uint8_t srcAddr[8] = {0,0,0,0,0,0,0,0}; |
||
765 | |||
766 | uint8_t* messageData = &dw_event->msgu.rxmsg_ls.messageData[0];
|
||
767 | memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ls.sourceAddr[0]), ADDR_BYTE_SIZE_S); |
||
768 | |||
769 | if(GATEWAY_ANCHOR_ADDR == (srcAddr[0] | ((uint32_t)(srcAddr[1] << 8)))) //if response from gateway then use the correction factor |
||
770 | { |
||
771 | // casting received bytes to int because this is a signed correction -0.5 periods to +1.5 periods
|
||
772 | inst->tagSleepCorrection_ms = (int16_t) (((uint16_t) messageData[RES_TAG_SLP1] << 8) + messageData[RES_TAG_SLP0]);
|
||
773 | inst->tagSleepRnd_ms = 0; // once we have initial response from Anchor #0 the slot correction acts and we don't need this anymore |
||
774 | } |
||
775 | |||
776 | //get short address from anchor
|
||
777 | inst->instanceAddress16 = (int16_t) (((uint16_t) messageData[RES_TAG_ADD1] << 8) + messageData[RES_TAG_ADD0]);
|
||
778 | |||
779 | //set source address
|
||
780 | inst->newRangeTagAddress = inst->instanceAddress16 ; |
||
781 | dwt_setaddress16(inst->instanceAddress16); |
||
782 | |||
783 | inst->nextState = TA_TXPOLL_WAIT_SEND; |
||
784 | inst->testAppState = TA_TXE_WAIT; |
||
785 | inst->instToSleep = TRUE ; |
||
786 | |||
787 | inst->tagSleepTime_ms = inst->tagPeriod_ms ; |
||
788 | |||
789 | //inst->twrMode = INITIATOR;
|
||
790 | |||
791 | break; //RTLS_DEMO_MSG_RNG_INIT |
||
792 | } |
||
793 | |||
794 | case DWT_SIG_RX_TIMEOUT :
|
||
795 | { |
||
796 | event_data_t* dw_event = instance_getevent(17); //get and clear this event |
||
797 | |||
798 | //printf("PD_DATA_TIMEOUT %d\n", inst->previousState) ;
|
||
799 | |||
800 | //Anchor can time out and then need to send response - so will be in TX pending
|
||
801 | if(dw_event->typePend == DWT_SIG_TX_PENDING)
|
||
802 | { |
||
803 | inst->testAppState = TA_TX_WAIT_CONF; // wait confirmation
|
||
804 | inst->previousState = TA_TXRESPONSE_SENT_TORX ; //wait for TX confirmation of sent response
|
||
805 | } |
||
806 | else if(dw_event->typePend == DWT_SIG_DW_IDLE) //if timed out and back in receive then don't process as timeout |
||
807 | { |
||
808 | tag_process_rx_timeout(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 | default :
|
||
816 | { |
||
817 | if(message) // == DWT_SIG_TX_DONE) |
||
818 | { |
||
819 | instDone = INST_DONE_WAIT_FOR_NEXT_EVENT; |
||
820 | } |
||
821 | |||
822 | if(instDone == INST_NOT_DONE_YET) instDone = 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 instDone;
|
||
834 | } // end testapprun_tag()
|
||
835 | |||
836 | // -------------------------------------------------------------------------------------------------------------------
|
||
837 | int tag_run(void) |
||
838 | { |
||
839 | instance_data_t* inst = instance_get_local_structure_ptr(0);
|
||
840 | int done = INST_NOT_DONE_YET;
|
||
841 | |||
842 | while(done == INST_NOT_DONE_YET)
|
||
843 | { |
||
844 | done = tag_app_run(inst) ; // run the communications application
|
||
845 | } |
||
846 | |||
847 | if(done == INST_DONE_WAIT_FOR_NEXT_EVENT_TO) //tag has finished the ranging exchange and needs to configure sleep time |
||
848 | { |
||
849 | int32_t nextPeriod ; |
||
850 | |||
851 | // next period will be a positive number because correction is -0.5 to +1.5 periods, (and tagSleepTime_ms is the period)
|
||
852 | nextPeriod = inst->tagSleepRnd_ms + inst->tagSleepTime_ms + inst->tagSleepCorrection_ms; |
||
853 | |||
854 | inst->nextWakeUpTime_ms = (uint32_t) nextPeriod ; //set timeout time, CAST the positive period to UINT for correct wrapping.
|
||
855 | inst->tagSleepCorrection_ms = 0; //clear the correction |
||
856 | inst->instanceTimerEn = 1; //start timer |
||
857 | } |
||
858 | |||
859 | //check if timer has expired
|
||
860 | if(inst->instanceTimerEn == 1) |
||
861 | { |
||
862 | if((portGetTickCnt() - inst->instanceWakeTime_ms) > inst->nextWakeUpTime_ms)
|
||
863 | { |
||
864 | event_data_t dw_event; |
||
865 | inst->instanceTimerEn = 0;
|
||
866 | dw_event.rxLength = 0;
|
||
867 | dw_event.type = 0;
|
||
868 | //dw_event.typeSave = 0x80 | DWT_SIG_RX_TIMEOUT;
|
||
869 | instance_putevent(dw_event, DWT_SIG_RX_TIMEOUT); |
||
870 | } |
||
871 | } |
||
872 | return 0 ; |
||
873 | } |
||
874 | |||
875 | /* ==========================================================
|
||
876 | |||
877 | Notes:
|
||
878 | |||
879 | Previously code handled multiple instances in a single console application
|
||
880 | |||
881 | Now have changed it to do a single instance only. With minimal code changes...(i.e. kept [instance] index but it is always 0.
|
||
882 | |||
883 | Windows application should call instance_init() once and then in the "main loop" call instance_run().
|
||
884 | |||
885 | */
|
||
886 | |||
887 | |||
888 | 1d5bcc82 | Thomas Schöpping | #endif /* defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 0) */ |