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