Statistics
| Branch: | Tag: | Revision:

amiro-lld / source / DW1000 / v0 / deca_instance_tag.c @ 1d5bcc82

History | View | Annotate | Download (33.8 KB)

1
/*! ----------------------------------------------------------------------------
2
 *  @file    instance_tag.c
3
 *  @brief   Decawave tag application state machine for TREK demo
4
 *
5
 * @attention
6
 *
7
 * Copyright 2016 (c) Decawave Ltd, Dublin, Ireland.
8
 *
9
 * All rights reserved.
10
 *
11
 * @author Decawave
12
 */
13

    
14
#include <alld_DW1000.h>
15

    
16
#if (defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 0)) || defined(__DOXYGEN__)
17

    
18
#include <string.h>
19

    
20

    
21
// -------------------------------------------------------------------------------------------------------------------
22
//      Data Definitions
23
// -------------------------------------------------------------------------------------------------------------------
24

    
25
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
26
// NOTE: the maximum RX timeout is ~ 65ms
27
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
28

    
29

    
30
// -------------------------------------------------------------------------------------------------------------------
31
// Functions
32
// -------------------------------------------------------------------------------------------------------------------
33

    
34
/**
35
 * @brief this function either enables the receiver (delayed)
36
 *
37
 **/
38
void tag_enable_rx(uint32_t dlyTime)
39
{
40
        instance_data_t* inst = instance_get_local_structure_ptr(0);
41
        //subtract preamble duration (because when instructing delayed TX the time is the time of SFD,
42
        //however when doing delayed RX the time is RX on time)
43
        dwt_setdelayedtrxtime(dlyTime - inst->preambleDuration32h) ;
44
        if(dwt_rxenable(DWT_START_RX_DELAYED|DWT_IDLE_ON_DLY_ERR)) //delayed rx
45
        {
46
                //if the delayed RX failed - time has passed - do immediate enable
47
                //led_on(LED_PC9);
48
                dwt_setpreambledetecttimeout(0); //clear preamble timeout as RX is turned on early/late
49
        dwt_setrxtimeout((uint16_t)inst->fwto4RespFrame_sy*2); //reconfigure the timeout before enable
50
                //longer timeout as we cannot do delayed receive... so receiver needs to stay on for longer
51
                dwt_rxenable(DWT_START_RX_IMMEDIATE);
52
                dwt_setpreambledetecttimeout(PTO_PACS); //configure preamble timeout
53
        dwt_setrxtimeout((uint16_t)inst->fwto4RespFrame_sy); //restore the timeout for next RX enable
54
                //inst->lateRX++;
55
                //led_off(LED_PC9);
56
        }
57

    
58
}
59

    
60
/* @fn           instanceProcessRXTimeoutTag
61
 * @brief function to process RX timeout event
62
 * */
63
void tag_process_rx_timeout(instance_data_t *inst)
64
{
65
    //inst->rxTimeouts ++ ;
66

    
67
#if(DISCOVERY == 1)
68
    if(inst->twrMode == GREETER)
69
    {
70
            inst->instToSleep = TRUE ;
71
                // initiate the re-transmission of the poll that was not responded to
72
                inst->testAppState = TA_TXE_WAIT ;
73
                inst->nextState = TA_TXBLINK_WAIT_SEND ;
74
    }
75
    else
76
#endif
77
    {
78

    
79
#if (TAG_HASTO_RANGETO_A0 == 0)
80
        if(inst->rxResponseMask == 0) //if any response have been received send a Final else go to SLEEP
81
        {
82
                  inst->instToSleep = TRUE ; //set sleep to TRUE so that tag will go to DEEP SLEEP before next ranging attempt
83
                inst->testAppState = TA_TXE_WAIT ;
84
                inst->nextState = TA_TXPOLL_WAIT_SEND ;
85
        }
86
#else
87

    
88
        //if tag times out - no response (check if we are to send a final)
89
        //send the final only if it has received response from anchor 0
90
        if((inst->previousState == TA_TXPOLL_WAIT_SEND)
91
                        && ((inst->rxResponseMask & 0x1) == 0)
92
                        )
93
        {
94
                inst->instToSleep = TRUE ; //set sleep to TRUE so that tag will go to DEEP SLEEP before next ranging attempt
95
                inst->testAppState = TA_TXE_WAIT ;
96
                inst->nextState = TA_TXPOLL_WAIT_SEND ;
97
        }
98
#endif
99
        else if (inst->previousState == TA_TXFINAL_WAIT_SEND) //got here from main (error sending final - handle as timeout)
100
        {
101
                dwt_forcetrxoff();        //this will clear all events
102
                inst->instToSleep = TRUE ;
103
                // initiate the re-transmission of the poll that was not responded to
104
                inst->testAppState = TA_TXE_WAIT ;
105
                inst->nextState = TA_TXPOLL_WAIT_SEND ;
106
        }
107
        else //send the final
108
        {
109
                // initiate the transmission of the final
110
                inst->testAppState = TA_TXE_WAIT ;
111
                inst->nextState = TA_TXFINAL_WAIT_SEND ;
112
        }
113

    
114
    }
115
}
116

    
117
/**
118
 * @brief function to re-enable the receiver and also adjust the timeout before sending the final message
119
 * if it is time so send the final message, the callback will notify the application, else the receiver is
120
 * automatically re-enabled
121
 *
122
 * this function is only used for tag when ranging to other anchors
123
 */
124
uint8_t tag_rx_reenable(uint16_t sourceAddress, uint8_t error)
125
{
126
    uint8_t type_pend = DWT_SIG_DW_IDLE;
127
    uint8_t anc = sourceAddress & 0x3;
128
        instance_data_t* inst = instance_get_local_structure_ptr(0);
129

    
130
        switch(anc)
131
        {
132
                //if we got Response from anchor 3 - this is the last expected response - send the final
133
                case 3:
134
                        type_pend = DWT_SIG_DW_IDLE;
135
                        break;
136

    
137
                //if we got response from anchor 0, 1, or 2 - go back to wait for next anchor's response
138
                //if we got response from 0, then still expecting 3, so remainingRespToRx set to 3
139
                case 0:
140
                case 1:
141
                case 2:
142
                default:
143
                        if(inst->remainingRespToRx > 0) //can get here as result of error frame so need to check
144
                        {
145
                                //can't use anc address as this is an error frame, so just re-enable TO based on remainingRespToRx count
146
                                if(error == 0)
147
                                {
148
                                        switch (anc)
149
                                        {
150
                                                case 0:
151
                                                        inst->remainingRespToRx = 3; //expecting 3 more responses
152
                                                        break;
153
                                                case 1:
154
                                                        inst->remainingRespToRx = 2; //expecting 2 more responses
155
                                                        break;
156
                                                case 2:
157
                                                        inst->remainingRespToRx = 1; //expecting 1 more response
158
                                                        break;
159
                                        }
160
                                }
161
                                //Poll sent at tagPollTxTime_32bit
162
                                //1st response is delayTime + fixedReplyDelayAnc32h - preambleDuration_32MSBs
163
                                //2nd is delayTime + fixedReplyDelayAnc32h - preambleDuration_32MSBs + fixedReplyDelayAnc32h
164
                                tag_enable_rx(inst->tagPollTxTime32h +
165
                                                (MAX_ANCHOR_LIST_SIZE-inst->remainingRespToRx+1)*(inst->fixedReplyDelayAnc32h));
166

    
167
                                type_pend = DWT_SIG_RX_PENDING ;
168
                        }
169
                        else //finished waiting for responses - no responses left to receive... send a final
170
                        {
171
                                type_pend = DWT_SIG_DW_IDLE; //report timeout - send the final if due to be sent
172
                        }
173
                        break;
174

    
175
        }
176

    
177
        return type_pend;
178
}
179

    
180
/**
181
 * @brief this function handles frame error event, it will either signal TO or re-enable the receiver
182
 */
183
void tag_handle_error_unknownframe(event_data_t dw_event)
184
{
185
        instance_data_t* inst = instance_get_local_structure_ptr(0);
186

    
187
        if(inst->twrMode != GREETER)
188
        {
189
                //re-enable the receiver (after error frames as we are not using auto re-enable
190
                //for ranging application rx error frame is same as TO - as we are not going to get the expected frame
191
                inst->remainingRespToRx--; //got something (need to reduce timeout (for remaining responses))
192

    
193
                dw_event.typePend = tag_rx_reenable(0, 1); //check if receiver will be re-enabled or it's time to send the final
194
        }
195
        else
196
        {
197
                dw_event.typePend = DWT_SIG_DW_IDLE; //in GREETER mode only waiting for 1 frame
198
        }
199

    
200
        dw_event.type = 0;
201
        //dw_event.typeSave = 0x40 | DWT_SIG_RX_TIMEOUT;
202
        dw_event.rxLength = 0;
203

    
204
        instance_putevent(dw_event, DWT_SIG_RX_TIMEOUT);
205
}
206

    
207
/**
208
 * @brief this is the receive timeout event callback handler
209
 */
210
void rx_to_cb_tag(const dwt_cb_data_t *rxd)
211
{
212
    (void) rxd;
213
        event_data_t dw_event;
214

    
215
        //microcontroller time at which we received the frame
216
    dw_event.uTimeStamp = portGetTickCnt();
217
    tag_handle_error_unknownframe(dw_event);
218
}
219

    
220
/**
221
 * @brief this is the receive error event callback handler
222
 */
223
void rx_err_cb_tag(const dwt_cb_data_t *rxd)
224
{
225
    (void) rxd;
226
        event_data_t dw_event;
227

    
228
        //microcontroller time at which we received the frame
229
    dw_event.uTimeStamp = portGetTickCnt();
230
    tag_handle_error_unknownframe(dw_event);
231
}
232

    
233
/**
234
 * @brief this is the receive event callback handler, the received event is processed and the instance either
235
 * responds by sending a response frame or re-enables the receiver to await the next frame
236
 * once the immediate action is taken care of the event is queued up for application to process
237
 */
238
void rx_ok_cb_tag(const dwt_cb_data_t *rxd)
239
{
240
        instance_data_t* inst = instance_get_local_structure_ptr(0);
241
    uint8_t rxTimeStamp[5]  = {0, 0, 0, 0, 0};
242

    
243
    uint8_t rxd_event = 0;
244
    uint8_t fcode_index  = 0;
245
    uint8_t srcAddr_index = 0;
246
        event_data_t dw_event;
247

    
248
        //microcontroller time at which we received the frame
249
    dw_event.uTimeStamp = portGetTickCnt();
250

    
251
    //if we got a frame with a good CRC - RX OK
252
    {
253
                 dw_event.rxLength = rxd->datalength;
254

    
255
                //need to process the frame control bytes to figure out what type of frame we have received
256
                if(rxd->fctrl[0] == 0x41)
257
                {
258
                        if((rxd->fctrl[1] & 0xCC) == 0x88) //short address
259
                        {
260
                                fcode_index = FRAME_CRTL_AND_ADDRESS_S; //function code is in first byte after source address
261
                                srcAddr_index = FRAME_CTRLP + ADDR_BYTE_SIZE_S;
262
                                rxd_event = DWT_SIG_RX_OKAY;
263
                        }
264
#if (DISCOVERY == 1)
265
                        else if((rxd->fctrl[1] & 0xCC) == 0x8c) //long/short address - ranging init message
266
                        {
267
                                fcode_index = FRAME_CRTL_AND_ADDRESS_LS; //function code is in first byte after source address
268
                                srcAddr_index = FRAME_CTRLP + ADDR_BYTE_SIZE_L;
269
                                rxd_event = DWT_SIG_RX_OKAY;
270
                        }
271
#endif
272
                        else
273
                        {
274
                                rxd_event = SIG_RX_UNKNOWN; //not supported - all TREK1000 frames are short addressed
275
                        }
276
                }
277
                else
278
                {
279
                        rxd_event = SIG_RX_UNKNOWN; //not supported - all TREK1000 frames are short addressed
280
                }
281

    
282
        //read RX timestamp
283
        dwt_readrxtimestamp(rxTimeStamp) ;
284
        dwt_readrxdata((uint8_t *)&dw_event.msgu.frame[0], rxd->datalength, 0);  // Read Data Frame
285
        instance_seteventtime(&dw_event, rxTimeStamp);
286

    
287
        dw_event.type = 0; //type will be added as part of adding to event queue
288
                //dw_event.typeSave = rxd_event;
289
                dw_event.typePend = DWT_SIG_DW_IDLE;
290

    
291
                if(rxd_event == DWT_SIG_RX_OKAY) //Process good/known frame types
292
                {
293
            uint16_t sourceAddress = (((uint16_t)dw_event.msgu.frame[srcAddr_index+1]) << 8) + dw_event.msgu.frame[srcAddr_index];
294

    
295
                        //if tag got a good frame - this is probably a response, but could also be some other non-ranging frame
296
                        //(although due to frame filtering this is limited as non-addressed frames are filtered out)
297

    
298
                        //check if this is a TWR message (and also which one)
299
                        switch(dw_event.msgu.frame[fcode_index])
300
                        {
301
                                //we got a response from a "responder" (anchor)
302
                                case RTLS_DEMO_MSG_ANCH_RESP:
303
                                {
304
                                        if(inst->twrMode == INITIATOR)
305
                                        {
306
                                                //if tag is involved in the ranging exchange expecting responses
307
                        uint8_t index ;
308
                                                inst->remainingRespToRx--; //got 1 more response or other RX frame - need to reduce timeout (for next response)
309
                                                dw_event.typePend = tag_rx_reenable(sourceAddress, 0); //remainingRespToRx decremented above...
310
                                                index = RRXT0 + 5*(sourceAddress & 0x3);
311

    
312
                                                inst->rxResponseMask |= (0x1 << (sourceAddress & 0x3)); //add anchor ID to the mask
313
                                                // Write Response RX time field of Final message
314
                                                memcpy(&(inst->msg_f.messageData[index]), rxTimeStamp, 5);
315
                                                break;
316
                    } else {
317
                     __attribute__ ((fallthrough));
318
                    }
319
                                }
320
#if (DISCOVERY == 1)
321
                                case RTLS_DEMO_MSG_RNG_INIT:
322
                                {
323
                                        if(inst->twrMode == GREETER)
324
                                        {
325
                                                rxd_event = RTLS_DEMO_MSG_RNG_INIT;
326
                                                break; //process the event in the application
327
                                        }
328
                                }
329
#endif
330
                                case RTLS_DEMO_MSG_ANCH_POLL:
331
                                case RTLS_DEMO_MSG_TAG_POLL:
332
                                case RTLS_DEMO_MSG_TAG_FINAL:
333
                                case RTLS_DEMO_MSG_ANCH_FINAL:
334
                                case RTLS_DEMO_MSG_ANCH_RESP2:
335
                                default:
336
                                //tag should ignore any other frames - only receive responses
337
                                {
338
                                        tag_handle_error_unknownframe(dw_event);
339
                                        //inst->rxMsgCount++;
340
                                        return;
341
                                }
342
                        }
343
            instance_putevent(dw_event, rxd_event);
344

    
345
                        //inst->rxMsgCount++;
346
                }
347
                else //if (rxd_event == SIG_RX_UNKNOWN) //need to re-enable the rx (got unknown frame type)
348
                {
349
                        tag_handle_error_unknownframe(dw_event);
350
                }
351
        }
352
}
353
// -------------------------------------------------------------------------------------------------------------------
354
//
355
// the main instance state machine for tag application
356
//
357
// -------------------------------------------------------------------------------------------------------------------
358
//
359
int tag_app_run(instance_data_t *inst)
360
{
361
        int instDone = INST_NOT_DONE_YET;
362
    int message = instance_peekevent(); //get any of the received events from ISR
363

    
364
    switch (inst->testAppState)
365
    {
366
        case TA_INIT :
367
            // printf("TA_INIT") ;
368
            switch (inst->mode)
369
            {
370
                case TAG:
371
                {
372
                    uint16_t sleep_mode = 0;
373

    
374
                    dwt_enableframefilter(DWT_FF_DATA_EN | DWT_FF_ACK_EN); //allow data, ack frames;
375

    
376
                    inst->eui64[0] += inst->instanceAddress16; //so switch 5,6,7 can be used to emulate more tags
377
                    dwt_seteui(inst->eui64);
378

    
379
                    dwt_setpanid(inst->panID);
380
#if (DISCOVERY == 1)
381
                                        //Start off by sending Blinks and wait for Anchor to send Ranging Init
382
                        inst->testAppState = TA_TXBLINK_WAIT_SEND;
383
                        inst->tagSleepTime_ms = BLINK_PERIOD ;
384
                                    memcpy(inst->blinkmsg.tagID, inst->eui64, ADDR_BYTE_SIZE_L);
385
                                    inst->newRangeTagAddress = inst->eui64[1];
386
                                    inst->newRangeTagAddress = (inst->newRangeTagAddress << 8) + inst->eui64[0];
387

    
388
#else
389
                    //dwt_setpanid(inst->panID);
390
                    memcpy(inst->eui64, &inst->instanceAddress16, ADDR_BYTE_SIZE_S);
391
                    //set source address
392
                    inst->newRangeTagAddress = inst->instanceAddress16 ;
393
                    dwt_setaddress16(inst->instanceAddress16);
394

    
395
                    //Start off by Sleeping 1st -> set instToSleep to TRUE
396
                    inst->nextState = TA_TXPOLL_WAIT_SEND;
397
                    inst->testAppState = TA_TXE_WAIT;
398
                    inst->instToSleep = TRUE ;
399
                    inst->tagSleepTime_ms = inst->tagPeriod_ms ;
400
#endif
401
                    inst->rangeNum = 0;
402
                    inst->tagSleepCorrection_ms = 0;
403

    
404
                    sleep_mode = (DWT_PRESRV_SLEEP|DWT_CONFIG|DWT_TANDV);
405

    
406
                                        if(inst->configData.txPreambLength == DWT_PLEN_64)  //if using 64 length preamble then use the corresponding OPSet
407
                                                sleep_mode |= DWT_LOADOPSET;
408

    
409
#if (DEEP_SLEEP == 1)
410
                                    dwt_configuresleep(sleep_mode, DWT_WAKE_WK|DWT_WAKE_CS|DWT_SLP_EN); //configure the on wake parameters (upload the IC config settings)
411
#endif
412
                                    instance_config_frameheader_16bit(inst);
413
                                    inst->instanceWakeTime_ms = portGetTickCnt();
414
                }
415
                break;
416
                default:
417
                break;
418
            }
419
            break; // end case TA_INIT
420

    
421
        case TA_SLEEP_DONE :
422
        {
423
                event_data_t* dw_event = instance_getevent(10); //clear the event from the queue
424
                        // waiting for timout from application to wakeup IC
425
                        if (dw_event->type != DWT_SIG_RX_TIMEOUT)
426
                        {
427
                                // if no pause and no wake-up timeout continu waiting for the sleep to be done.
428
                instDone = INST_DONE_WAIT_FOR_NEXT_EVENT; //wait here for sleep timeout
429
                break;
430
            }
431

    
432
            instDone = INST_NOT_DONE_YET;
433
            inst->instToSleep = FALSE ;
434
            inst->testAppState = inst->nextState;
435
            inst->nextState = 0; //clear
436
                        inst->instanceWakeTime_ms = portGetTickCnt(); // Record the time count when we wake-up
437
#if (DEEP_SLEEP == 1)
438
            {
439
                //wake up device from low power mode
440
                //led_on(LED_PC9);
441

    
442
                    port_wakeup_dw1000_fast();
443

    
444
                //led_off(LED_PC9);
445

    
446
                //this is platform dependent - only program if DW EVK/EVB
447
                dwt_setleds(1);
448

    
449
                //MP bug - TX antenna delay needs reprogramming as it is not preserved (only RX)
450
                dwt_settxantennadelay(inst->txAntennaDelay) ;
451
#if(DISCOVERY == 0)
452
                //set EUI as it will not be preserved unless the EUI is programmed and loaded from NVM
453
                                dwt_seteui(inst->eui64);
454
#endif
455
            }
456
#else
457
            Sleep(3); //to approximate match the time spent in the #if above
458
#endif
459

    
460
            instance_set_antennadelays(); //this will update the antenna delay if it has changed
461
            instance_set_txpower(); //configure TX power if it has changed
462
#if (READ_EVENT_COUNTERS == 1)
463
            dwt_configeventcounters(1);
464
#endif
465
       }
466
            break;
467

    
468
        case TA_TXE_WAIT : //either go to sleep or proceed to TX a message
469
            //if we are scheduled to go to sleep before next transmission then sleep first.
470
#if (DISCOVERY == 1)
471
                if(((inst->nextState == TA_TXPOLL_WAIT_SEND)
472
                                || (inst->nextState == TA_TXBLINK_WAIT_SEND))
473
#else
474
                if((inst->nextState == TA_TXPOLL_WAIT_SEND)
475
#endif
476
                    && (inst->instToSleep)  //go to sleep before sending the next poll/ starting new ranging exchange
477
                    )
478
            {
479
                    inst->rangeNum++; //increment the range number before going to sleep
480
                //the app should put chip into low power state and wake up after tagSleepTime_ms time...
481
                //the app could go to *_IDLE state and wait for uP to wake it up...
482
                instDone = INST_DONE_WAIT_FOR_NEXT_EVENT_TO; //don't sleep here but kick off the Sleep timer countdown
483
                inst->testAppState = TA_SLEEP_DONE;
484

    
485
                {
486
#if (READ_EVENT_COUNTERS == 1)
487
                        dwt_readeventcounters(&inst->ecounters);
488
#endif
489
#if (DEEP_SLEEP == 1)
490
                        //put device into low power mode
491
                                        dwt_entersleep(); //go to sleep
492
#endif
493
                                        if(inst->rxResponseMask != 0)
494
                                        {
495
                                                //DW1000 gone to sleep - report the received range
496
                                                inst->newRange = instance_calc_ranges(&inst->tofArray[0], MAX_ANCHOR_LIST_SIZE, TOF_REPORT_T2A, &inst->rxResponseMask);
497
                                                inst->rxResponseMaskReport = inst->rxResponseMask;
498
                                                inst->rxResponseMask = 0;
499
                                                inst->newRangeTime = portGetTickCnt() ;
500
                                        }
501
                }
502

    
503
            }
504
            else //proceed to configuration and transmission of a frame
505
            {
506
                inst->testAppState = inst->nextState;
507
                inst->nextState = 0; //clear
508
            }
509
            break ; // end case TA_TXE_WAIT
510
        case TA_TXBLINK_WAIT_SEND :
511
            {
512
                                int flength = (BLINK_FRAME_CRTL_AND_ADDRESS + FRAME_CRC);
513

    
514
                //blink frames with IEEE EUI-64 tag ID
515
                inst->blinkmsg.frameCtrl = 0xC5 ;
516
                inst->blinkmsg.seqNum = inst->frameSN++;
517

    
518
                dwt_writetxdata(flength, (uint8_t *)  (&inst->blinkmsg), 0) ;        // write the frame data
519
                                dwt_writetxfctrl(flength, 0, 1);
520

    
521
                                inst->twrMode = GREETER;
522
                                //using wait for response to do delayed receive
523
                                inst->wait4ack = DWT_RESPONSE_EXPECTED;
524
                                inst->rxResponseMask = 0;
525

    
526
                dwt_setrxtimeout((uint16_t)inst->fwto4RespFrame_sy*2);  //units are symbols (x2 as ranging init > response)
527
                                //set the delayed rx on time (the ranging init will be sent after this delay)
528
                dwt_setrxaftertxdelay((uint32_t)inst->tagRespRxDelay_sy);  //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
529

    
530
                                dwt_starttx(DWT_START_TX_IMMEDIATE | inst->wait4ack); //always using immediate TX and enable delayed RX
531

    
532
                                inst->instToSleep = 1; //go to Sleep after this blink
533
                inst->testAppState = TA_RX_WAIT_DATA ; // to to RX, expecting ranging init response
534
                inst->previousState = TA_TXBLINK_WAIT_SEND ;
535
                instDone = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set below)
536

    
537
            }
538
            break ; // end case TA_TXBLINK_WAIT_SEND
539

    
540
        case TA_TXPOLL_WAIT_SEND :
541
            {
542
                inst->msg_f.messageData[POLL_RNUM] = inst->rangeNum; //copy new range number
543
                    inst->msg_f.messageData[FCODE] = RTLS_DEMO_MSG_TAG_POLL; //message function code (specifies if message is a poll, response or other...)
544
                inst->psduLength = (TAG_POLL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC);
545
                inst->msg_f.seqNum = inst->frameSN++; //copy sequence number and then increment
546
                inst->msg_f.sourceAddr[0] = inst->instanceAddress16 & 0xff; //inst->eui64[0]; //copy the address
547
                inst->msg_f.sourceAddr[1] = (inst->instanceAddress16>>8) & 0xff; //inst->eui64[1]; //copy the address
548
                    inst->msg_f.destAddr[0] = 0xff;  //set the destination address (broadcast == 0xffff)
549
                    inst->msg_f.destAddr[1] = 0xff;  //set the destination address (broadcast == 0xffff)
550
                dwt_writetxdata(inst->psduLength, (uint8_t *)  &inst->msg_f, 0) ;        // write the frame data
551

    
552
                                //set the delayed rx on time (the response message will be sent after this delay (from A0))
553
                dwt_setrxaftertxdelay((uint32_t)inst->tagRespRxDelay_sy);  //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)
554

    
555
                                inst->remainingRespToRx = MAX_ANCHOR_LIST_SIZE; //expecting 4 responses
556
                dwt_setrxtimeout((uint16_t)inst->fwto4RespFrame_sy);  //configure the RX FWTO
557
                                dwt_setpreambledetecttimeout(PTO_PACS); //configure preamble timeout
558

    
559
                                inst->rxResponseMask = 0;        //reset/clear the mask of received responses when tx poll
560

    
561
                                inst->wait4ack = DWT_RESPONSE_EXPECTED; //response is expected - automatically enable the receiver
562

    
563
                                dwt_writetxfctrl(inst->psduLength, 0, 1); //write frame control
564

    
565
                                inst->twrMode = INITIATOR;
566

    
567
                                dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED); //transmit the frame
568

    
569
                inst->testAppState = TA_TX_WAIT_CONF ;  // wait confirmation
570
                inst->previousState = TA_TXPOLL_WAIT_SEND ;
571
                instDone = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set above)
572

    
573
            }
574
            break;
575

    
576
        case TA_TXFINAL_WAIT_SEND :
577
            {
578
                    //the final has the same range number as the poll (part of the same ranging exchange)
579
                inst->msg_f.messageData[POLL_RNUM] = inst->rangeNum;
580
                //the mask is sent so the anchors know whether the response RX time is valid
581
                                inst->msg_f.messageData[VRESP] = inst->rxResponseMask;
582
                    inst->msg_f.messageData[FCODE] = RTLS_DEMO_MSG_TAG_FINAL; //message function code (specifies if message is a poll, response or other...)
583
                inst->psduLength = (TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC);
584
                inst->msg_f.seqNum = inst->frameSN++;
585
                dwt_writetxdata(inst->psduLength, (uint8_t *)  &inst->msg_f, 0) ;        // write the frame data
586

    
587
                                inst->wait4ack = 0; //clear the flag not using wait for response as this message ends the ranging exchange
588

    
589
                                if(instance_send_delayed_frame(inst, DWT_START_TX_DELAYED))
590
                {
591
                    // initiate the re-transmission
592
                                        inst->testAppState = TA_TXE_WAIT ; //go to TA_TXE_WAIT first to check if it's sleep time
593
                                        inst->nextState = TA_TXPOLL_WAIT_SEND ;
594
                                        inst->instToSleep = TRUE ;
595
                    break; //exit this switch case...
596
                }
597
                else
598
                {
599
                    inst->testAppState = TA_TX_WAIT_CONF;   // wait confirmation
600
                }
601

    
602
                                inst->previousState = TA_TXFINAL_WAIT_SEND;
603
                                inst->instToSleep = TRUE ;
604
                    instDone = INST_DONE_WAIT_FOR_NEXT_EVENT; //will use RX FWTO to time out (set above)
605
            }
606
            break;
607

    
608

    
609
        case TA_TX_WAIT_CONF :
610
            {
611
                                event_data_t* dw_event = instance_getevent(11); //get and clear this event
612

    
613
                if(dw_event->type != DWT_SIG_TX_DONE) //wait for TX done confirmation
614
                {
615
                                    instDone = INST_DONE_WAIT_FOR_NEXT_EVENT;
616
                    break;
617
                }
618

    
619
                instDone = INST_NOT_DONE_YET;
620

    
621
                if(inst->previousState == TA_TXFINAL_WAIT_SEND)
622
                {
623
                           inst->testAppState = TA_TXE_WAIT ;
624
                           inst->nextState = TA_TXPOLL_WAIT_SEND ;
625
                    break;
626
                }
627
                else
628
                {
629
                                        inst->txu.txTimeStamp = dw_event->timeStamp;
630
                                        inst->tagPollTxTime32h = dw_event->timeStamp32h;
631

    
632
                                        if(inst->previousState == TA_TXPOLL_WAIT_SEND)
633
                                        {
634
                        uint64_t tagCalculatedFinalTxTime ;
635
                                // Embed into Final message: 40-bit pollTXTime,  40-bit respRxTime,  40-bit finalTxTime
636
                                tagCalculatedFinalTxTime =  (inst->txu.txTimeStamp + inst->pollTx2FinalTxDelay) & MASK_TXDTS;
637

    
638
                                inst->delayedTRXTime32h = tagCalculatedFinalTxTime >> 8; //high 32-bits
639
                                // Calculate Time Final message will be sent and write this field of Final message
640
                                // Sending time will be delayedReplyTime, snapped to ~125MHz or ~250MHz boundary by
641
                                // zeroing its low 9 bits, and then having the TX antenna delay added
642
                                // getting antenna delay from the device and add it to the Calculated TX Time
643
                                tagCalculatedFinalTxTime = tagCalculatedFinalTxTime + inst->txAntennaDelay;
644
                                tagCalculatedFinalTxTime &= MASK_40BIT;
645

    
646
                                // Write Calculated TX time field of Final message
647
                        memcpy(&(inst->msg_f.messageData[FTXT]), (uint8_t *)&tagCalculatedFinalTxTime, 5);
648
                                // Write Poll TX time field of Final message
649
                        memcpy(&(inst->msg_f.messageData[PTXT]), (uint8_t *)&inst->txu.tagPollTxTime, 5);
650

    
651
                                        }
652

    
653
                    inst->testAppState = TA_RX_WAIT_DATA ;                      // After sending, tag expects response/report, anchor waits to receive a final/new poll
654

    
655
                    message = 0;
656
                    //fall into the next case (turn on the RX)
657
                    __attribute__ ((fallthrough));
658
                }
659

    
660
            }
661

    
662
            //break ; // end case TA_TX_WAIT_CONF
663

    
664
        case TA_RX_WAIT_DATA :                                                                     // Wait RX data
665
                   //printf("TA_RX_WAIT_DATA %d", message) ;
666

    
667
            switch (message)
668
            {
669

    
670
                                //if we have received a DWT_SIG_RX_OKAY event - this means that the message is IEEE data type - need to check frame control to know which addressing mode is used
671
                case DWT_SIG_RX_OKAY :
672
                {
673
                                        event_data_t* dw_event = instance_getevent(15); //get and clear this event
674
                    uint8_t  srcAddr[8] = {0,0,0,0,0,0,0,0};
675
                    uint8_t  dstAddr[8] = {0,0,0,0,0,0,0,0};
676
                    int fcode = 0;
677
                    uint8_t tof_idx  = 0;
678
                    uint8_t *messageData;
679

    
680
                                        memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ss.sourceAddr[0]), ADDR_BYTE_SIZE_S);
681
                                        memcpy(&dstAddr[0], &(dw_event->msgu.rxmsg_ss.destAddr[0]), ADDR_BYTE_SIZE_S);
682
                                        fcode = dw_event->msgu.rxmsg_ss.messageData[FCODE];
683
                                        messageData = &dw_event->msgu.rxmsg_ss.messageData[0];
684

    
685
                                        tof_idx = srcAddr[0] & 0x3 ;
686
                                        //process ranging messages
687
                                        switch(fcode)
688
                                        {
689
                                                case RTLS_DEMO_MSG_ANCH_RESP:
690
                                                {
691
                            uint8_t currentRangeNum = (messageData[TOFRN] + 1); //current = previous + 1
692

    
693
                            if(GATEWAY_ANCHOR_ADDR == (srcAddr[0] | ((uint32_t)(srcAddr[1] << 8)))) //if response from gateway then use the correction factor
694
                                                        {
695
                                // int sleepCorrection = (int16_t) (((uint16_t) messageData[RES_TAG_SLP1] << 8) + messageData[RES_TAG_SLP0]);
696
                                                                // casting received bytes to int because this is a signed correction -0.5 periods to +1.5 periods
697
                                inst->tagSleepCorrection_ms = (int16_t) (((uint16_t) messageData[RES_TAG_SLP1] << 8) + messageData[RES_TAG_SLP0]);
698
                                                                inst->tagSleepRnd_ms = 0; // once we have initial response from Anchor #0 the slot correction acts and we don't need this anymore
699
                                                        }
700

    
701
                                                        if(dw_event->typePend == DWT_SIG_RX_PENDING)
702
                                                        {
703
                                                                // stay in TA_RX_WAIT_DATA - receiver is already enabled, waiting for next response.
704
                                                        }
705
                                                        //DW1000 idle - send the final
706
                                                        else //if(dw_event->type_pend == DWT_SIG_DW_IDLE)
707
                                                        {
708
#if (TAG_HASTO_RANGETO_A0 == 1)
709
                                                                if(inst->rxResponseMask & 0x1)//if this is tag and A0's response received send the final
710
#endif
711
                                                                {
712
                                                                        inst->testAppState = TA_TXFINAL_WAIT_SEND ; // send our response / the final
713
                                                                }
714
#if (TAG_HASTO_RANGETO_A0 == 1)
715
                                                                else //go to sleep
716
                                                                {
717
                                                                        inst->testAppState = TA_TXE_WAIT ; //go to TA_TXE_WAIT first to check if it's sleep time
718
                                                                        inst->nextState = TA_TXPOLL_WAIT_SEND ;
719
                                                                        inst->instToSleep = TRUE;
720
                                                                }
721
#endif
722
                                                        }
723

    
724
                                                        if(currentRangeNum == inst->rangeNum) //these are the previous ranges...
725
                                                        {
726
                                                                //copy the ToF and put into array (array holds last 4 ToFs)
727
                                                                memcpy(&inst->tofArray[tof_idx], &(messageData[TOFR]), 4);
728

    
729
                                                                //check if the ToF is valid, this makes sure we only report valid ToFs
730
                                                                //e.g. consider the case of reception of response from anchor a1 (we are anchor a2)
731
                                                                //if a1 got a Poll with previous Range number but got no Final, then the response will have
732
                                                                //the correct range number but the range will be INVALID_TOF
733
                                                                if(inst->tofArray[tof_idx] != INVALID_TOF)
734
                                                                {
735
                                                                        inst->rxResponseMask |= (0x1 << tof_idx);
736
                                                                }
737

    
738
                                                        }
739
                                                        else
740
                                                        {
741
                                                                if(inst->tofArray[tof_idx] != INVALID_TOF)
742
                                                                {
743
                                                                        inst->tofArray[tof_idx] = INVALID_TOF;
744
                                                                }
745
                                                        }
746

    
747

    
748
                                                }
749
                                                break; //RTLS_DEMO_MSG_ANCH_RESP
750

    
751
                                                default:
752
                                                {
753
                                                        tag_process_rx_timeout(inst); //if unknown message process as timeout
754
                                                }
755
                                                break;
756
                                        } //end switch (fcode)
757

    
758
                }
759
                                break ; //end of DWT_SIG_RX_OKAY
760

    
761
                case RTLS_DEMO_MSG_RNG_INIT :
762
                {
763
                        event_data_t* dw_event = instance_getevent(16); //get and clear this event
764
                    uint8_t  srcAddr[8] = {0,0,0,0,0,0,0,0};
765

    
766
                    uint8_t* messageData = &dw_event->msgu.rxmsg_ls.messageData[0];
767
                        memcpy(&srcAddr[0], &(dw_event->msgu.rxmsg_ls.sourceAddr[0]), ADDR_BYTE_SIZE_S);
768

    
769
                    if(GATEWAY_ANCHOR_ADDR == (srcAddr[0] | ((uint32_t)(srcAddr[1] << 8)))) //if response from gateway then use the correction factor
770
                                        {
771
                                                // casting received bytes to int because this is a signed correction -0.5 periods to +1.5 periods
772
                        inst->tagSleepCorrection_ms = (int16_t) (((uint16_t) messageData[RES_TAG_SLP1] << 8) + messageData[RES_TAG_SLP0]);
773
                                                inst->tagSleepRnd_ms = 0; // once we have initial response from Anchor #0 the slot correction acts and we don't need this anymore
774
                                        }
775

    
776
                                        //get short address from anchor
777
                    inst->instanceAddress16 = (int16_t) (((uint16_t) messageData[RES_TAG_ADD1] << 8) + messageData[RES_TAG_ADD0]);
778

    
779
                                        //set source address
780
                                        inst->newRangeTagAddress = inst->instanceAddress16 ;
781
                                        dwt_setaddress16(inst->instanceAddress16);
782

    
783
                                        inst->nextState = TA_TXPOLL_WAIT_SEND;
784
                                        inst->testAppState = TA_TXE_WAIT;
785
                                        inst->instToSleep = TRUE ;
786

    
787
                                        inst->tagSleepTime_ms = inst->tagPeriod_ms ;
788

    
789
                                        //inst->twrMode = INITIATOR;
790

    
791
                                        break; //RTLS_DEMO_MSG_RNG_INIT
792
                }
793

    
794
                case DWT_SIG_RX_TIMEOUT :
795
                        {
796
                                event_data_t* dw_event = instance_getevent(17); //get and clear this event
797

    
798
                                                //printf("PD_DATA_TIMEOUT %d\n", inst->previousState) ;
799

    
800
                                //Anchor can time out and then need to send response - so will be in TX pending
801
                                if(dw_event->typePend == DWT_SIG_TX_PENDING)
802
                                {
803
                                        inst->testAppState = TA_TX_WAIT_CONF;                                               // wait confirmation
804
                                        inst->previousState = TA_TXRESPONSE_SENT_TORX ;    //wait for TX confirmation of sent response
805
                                }
806
                                else if(dw_event->typePend == DWT_SIG_DW_IDLE) //if timed out and back in receive then don't process as timeout
807
                                                {
808
                                        tag_process_rx_timeout(inst);
809
                                                }
810
                                //else if RX_PENDING then wait for next RX event...
811
                                                message = 0; //clear the message as we have processed the event
812
                        }
813
                break ;
814

    
815
                default :
816
                {
817
                    if(message) // == DWT_SIG_TX_DONE)
818
                    {
819
                            instDone = INST_DONE_WAIT_FOR_NEXT_EVENT;
820
                    }
821

    
822
                        if(instDone == INST_NOT_DONE_YET) instDone = INST_DONE_WAIT_FOR_NEXT_EVENT;
823
                }
824
                break;
825

    
826
            }
827
            break ; // end case TA_RX_WAIT_DATA
828
            default:
829
                //printf("\nERROR - invalid state %d - what is going on??\n", inst->testAppState) ;
830
            break;
831
    } // end switch on testAppState
832

    
833
    return instDone;
834
} // end testapprun_tag()
835

    
836
// -------------------------------------------------------------------------------------------------------------------
837
int tag_run(void)
838
{
839
        instance_data_t* inst = instance_get_local_structure_ptr(0);
840
    int done = INST_NOT_DONE_YET;
841

    
842
    while(done == INST_NOT_DONE_YET)
843
        {
844
                done = tag_app_run(inst) ; // run the communications application
845
        }
846

    
847
    if(done == INST_DONE_WAIT_FOR_NEXT_EVENT_TO) //tag has finished the ranging exchange and needs to configure sleep time
848
    {
849
        int32_t nextPeriod ;
850

    
851
                // next period will be a positive number because correction is -0.5 to +1.5 periods, (and tagSleepTime_ms is the period)
852
                nextPeriod = inst->tagSleepRnd_ms + inst->tagSleepTime_ms + inst->tagSleepCorrection_ms;
853

    
854
        inst->nextWakeUpTime_ms = (uint32_t) nextPeriod ; //set timeout time, CAST the positive period to UINT for correct wrapping.
855
                inst->tagSleepCorrection_ms = 0; //clear the correction
856
                inst->instanceTimerEn = 1; //start timer
857
    }
858

    
859
    //check if timer has expired
860
    if(inst->instanceTimerEn == 1)
861
    {
862
                if((portGetTickCnt() - inst->instanceWakeTime_ms) > inst->nextWakeUpTime_ms)
863
                {
864
                        event_data_t dw_event;
865
                        inst->instanceTimerEn = 0;
866
                        dw_event.rxLength = 0;
867
                        dw_event.type = 0;
868
                        //dw_event.typeSave = 0x80 | DWT_SIG_RX_TIMEOUT;
869
                        instance_putevent(dw_event, DWT_SIG_RX_TIMEOUT);
870
                }
871
    }
872
    return 0 ;
873
}
874

    
875
/* ==========================================================
876

877
Notes:
878

879
Previously code handled multiple instances in a single console application
880

881
Now have changed it to do a single instance only. With minimal code changes...(i.e. kept [instance] index but it is always 0.
882

883
Windows application should call instance_init() once and then in the "main loop" call instance_run().
884

885
*/
886

    
887

    
888
#endif /* defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 0) */
889