Statistics
| Branch: | Tag: | Revision:

amiro-lld / source / deca_instance_tag.c @ ef078306

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 <deca_instance.h>
15
#if defined(AMIROLLD_CFG_USE_DW1000) || defined(__DOXYGEN__)
16

    
17
#include <alld_dw1000.h>
18
#include <alld_dw1000_regs.h>
19
#include <string.h>
20

    
21

    
22
// -------------------------------------------------------------------------------------------------------------------
23
//      Data Definitions
24
// -------------------------------------------------------------------------------------------------------------------
25

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

    
30

    
31
// -------------------------------------------------------------------------------------------------------------------
32
// Functions
33
// -------------------------------------------------------------------------------------------------------------------
34

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

    
59
}
60

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

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

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

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

    
115
    }
116
}
117

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

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

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

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

    
176
        }
177

    
178
        return type_pend;
179
}
180

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

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

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

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

    
205
        instance_putevent(dw_event, DWT_SIG_RX_TIMEOUT);
206
}
207

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    
443
                    port_wakeup_dw1000_fast();
444

    
445
                //led_off(LED_PC9);
446

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

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

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

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

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

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

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

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

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

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

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

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

    
538
            }
539
            break ; // end case TA_TXBLINK_WAIT_SEND
540

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

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

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

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

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

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

    
566
                                inst->twrMode = INITIATOR;
567

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

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

    
574
            }
575
            break;
576

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

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

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

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

    
609

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

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

    
620
                instDone = INST_NOT_DONE_YET;
621

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

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

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

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

    
652
                                        }
653

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

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

    
661
            }
662

    
663
            //break ; // end case TA_TX_WAIT_CONF
664

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

    
668
            switch (message)
669
            {
670

    
671
                                //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
672
                case DWT_SIG_RX_OKAY :
673
                {
674
                                        event_data_t* dw_event = instance_getevent(15); //get and clear this event
675
                    uint8_t  srcAddr[8] = {0,0,0,0,0,0,0,0};
676
                    uint8_t  dstAddr[8] = {0,0,0,0,0,0,0,0};
677
                    int fcode = 0;
678
                    uint8_t tof_idx  = 0;
679
                    uint8_t *messageData;
680

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

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

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

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

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

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

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

    
748

    
749
                                                }
750
                                                break; //RTLS_DEMO_MSG_ANCH_RESP
751

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

    
759
                }
760
                                break ; //end of DWT_SIG_RX_OKAY
761

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

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

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

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

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

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

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

    
790
                                        //inst->twrMode = INITIATOR;
791

    
792
                                        break; //RTLS_DEMO_MSG_RNG_INIT
793
                }
794

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

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

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

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

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

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

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

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

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

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

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

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

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

    
876
/* ==========================================================
877

878
Notes:
879

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

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

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

886
*/
887

    
888

    
889
#endif /* defined(AMIROLLD_CFG_USE_DW1000) */