amiro-lld / source / DW1000 / v1 / deca_instance_tag_anchor_v1.c @ 84450926
History | View | Annotate | Download (42.32 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 | 33f54213 | Cung Sang | #include <v1/deca_instance_v1.h> |
19 | 69a601a5 | Cung Sang | #include <string.h> |
20 | 26dead12 | Cung Sang | #include <math.h> |
21 | 69a601a5 | Cung Sang | #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 | 26dead12 | Cung Sang | if (dwt_starttx((uint8_t)(delayedTx | inst->wait4ack))){ // delayed start was too late |
52 | 69a601a5 | Cung Sang | 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 | 26dead12 | Cung Sang | Sleep(3); //to approximate match the time spent in the #if above |
241 | 69a601a5 | Cung Sang | #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 | 26dead12 | Cung Sang | inst->delayedReplyTime = (uint32_t)(tagCalculatedFinalTxTime >> 8); //high 32-bits |
412 | 69a601a5 | Cung Sang | // 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 | 26dead12 | Cung Sang | case TA_RX_WAIT_DATA : // Wait RX data |
462 | 69a601a5 | Cung Sang | //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 | 26dead12 | Cung Sang | uint8_t *messageData = NULL;
|
476 | 69a601a5 | Cung Sang | |
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 | 26dead12 | Cung Sang | int32_t tof = (int32_t)INVALID_TOF; |
670 | 69a601a5 | Cung Sang | |
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 | 26dead12 | Cung Sang | tof = (int32_t)(RaRbxDaDb/(RbyDb + RayDa)); |
736 | 69a601a5 | Cung Sang | } |
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 | 26dead12 | Cung Sang | inst->tof[inst->newRangeTagAddress & 0x7] = (uint32_t)tof;
|
743 | 69a601a5 | Cung Sang | //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 | 26dead12 | Cung Sang | if(tof != (int32_t)INVALID_TOF) { //check the last ToF entry is valid and copy into the current array |
749 | 69a601a5 | Cung Sang | setTagDist(srcAddr[0], inst->shortAdd_idx); //copy distance from this anchor to the tag into array |
750 | |||
751 | 26dead12 | Cung Sang | inst->rxResponseMask = (uint8_t)(0x1 << inst->shortAdd_idx);
|
752 | inst->tofArray[inst->shortAdd_idx] = (uint32_t)tof; |
||
753 | 69a601a5 | Cung Sang | } |
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 | 26dead12 | Cung Sang | inst->tofAnc[tof_idx] = (uint32_t)tof; |
760 | 69a601a5 | Cung Sang | } |
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 | 26dead12 | Cung Sang | float x = 0; |
862 | 69a601a5 | Cung Sang | |
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 | 26dead12 | Cung Sang | x = (float)ceil((double)(msgdatalen*8)/(double)330.0f); |
869 | 69a601a5 | Cung Sang | |
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 | 26dead12 | Cung Sang | x = (float) ceil((double)(margin*8)/(double)330.0f); |
876 | 69a601a5 | Cung Sang | |
877 | 26dead12 | Cung Sang | margin = (int) (margin*8 + x*48); |
878 | 69a601a5 | Cung Sang | |
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 | 26dead12 | Cung Sang | respframe_sy = (16 + (int)((double)((double)preamblelen + ((double)(msgdatalen + margin)/1000.0))/ 1.0256)) ; |
924 | 69a601a5 | Cung Sang | |
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 | 26dead12 | Cung Sang | respframe = (int)((double)preamblelen + ((double)msgdatalen/1000.0)); //length of response frame (micro seconds) |
937 | 69a601a5 | Cung Sang | 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 | 26dead12 | Cung Sang | instance_data[instance].fixedReplyDelayAncP = (uint32_t) (((uint64_t) convertmicrosectodevicetimeu ((double)preamblelen)) >> 8) + 16; |
945 | 69a601a5 | Cung Sang | |
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 | 26dead12 | Cung Sang | instance_data[instance].fixedReplyDelayAncP = (uint32_t) (((uint64_t) convertmicrosectodevicetimeu ((double)preamblelen)) >> 8) + 16; |
956 | 69a601a5 | Cung Sang | |
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) */ |