Revision 26dead12 source/DW1000/v1/deca_instance_tag_anchor_v1.c
source/DW1000/v1/deca_instance_tag_anchor_v1.c | ||
---|---|---|
17 | 17 |
|
18 | 18 |
#include <v1/deca_instance_v1.h> |
19 | 19 |
#include <string.h> |
20 |
#include<math.h> |
|
20 |
#include <math.h>
|
|
21 | 21 |
#include "module.h" |
22 | 22 |
|
23 | 23 |
// ------------------------------------------------------------------------------------------------------------------- |
24 |
// Data Definitions |
|
25 |
// ------------------------------------------------------------------------------------------------------------------- |
|
26 |
|
|
27 |
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! |
|
28 |
// NOTE: the maximum RX timeout is ~ 65ms |
|
29 |
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! |
|
30 |
|
|
31 |
|
|
32 |
// ------------------------------------------------------------------------------------------------------------------- |
|
33 |
// Functions |
|
34 |
// ------------------------------------------------------------------------------------------------------------------- |
|
35 |
|
|
36 |
// ------------------------------------------------------------------------------------------------------------------- |
|
37 | 24 |
// |
38 | 25 |
// function to construct the message/frame header bytes |
39 | 26 |
// |
... | ... | |
61 | 48 |
} |
62 | 49 |
|
63 | 50 |
//begin delayed TX of frame |
64 |
if (dwt_starttx(delayedTx | inst->wait4ack)){ // delayed start was too late
|
|
51 |
if (dwt_starttx((uint8_t)(delayedTx | inst->wait4ack))){ // delayed start was too late
|
|
65 | 52 |
result = 1; //late/error |
66 | 53 |
inst->lateTX++; |
67 | 54 |
} |
... | ... | |
72 | 59 |
return result; // state changes |
73 | 60 |
} |
74 | 61 |
|
75 |
|
|
76 | 62 |
int instance_calcranges(uint32_t *array, uint16_t size, int reportRange, uint8_t* mask){ |
77 | 63 |
int i; |
78 | 64 |
int newRange = TOF_REPORT_NUL; |
... | ... | |
135 | 121 |
|
136 | 122 |
sleep_mode = (DWT_LOADUCODE|DWT_PRESRV_SLEEP|DWT_CONFIG|DWT_TANDV); |
137 | 123 |
|
138 |
/* TODO: No need anymore since the driver handle itself */ |
|
139 |
// if((dwt_getldotune() != 0)) //if we need to use LDO tune value from OTP kick it after sleep |
|
140 |
// sleep_mode |= DWT_LOADOPSET; |
|
141 |
|
|
142 | 124 |
if(inst->configData.txPreambLength == DWT_PLEN_64) //if using 64 length preamble then use the corresponding OPSet |
143 | 125 |
sleep_mode |= DWT_LOADOPSET; |
144 | 126 |
|
... | ... | |
255 | 237 |
dwt_seteui(inst->eui64); |
256 | 238 |
} |
257 | 239 |
#else |
258 |
Sleep(3); //to approximate match the time spent in the #if above //TODO: 3ms default
|
|
240 |
Sleep(3); //to approximate match the time spent in the #if above |
|
259 | 241 |
#endif |
260 | 242 |
|
261 | 243 |
instancesetantennadelays(); //this will update the antenna delay if it has changed |
... | ... | |
426 | 408 |
else { //for anchor make the final half the delay ..... (this is ok, as A0 awaits 2 responses) |
427 | 409 |
tagCalculatedFinalTxTime = (inst->txu.txTimeStamp + inst->pollTx2FinalTxDelayAnc) & MASK_TXDTS; |
428 | 410 |
} |
429 |
inst->delayedReplyTime = tagCalculatedFinalTxTime >> 8; //high 32-bits
|
|
411 |
inst->delayedReplyTime = (uint32_t)(tagCalculatedFinalTxTime >> 8); //high 32-bits
|
|
430 | 412 |
// Calculate Time Final message will be sent and write this field of Final message |
431 | 413 |
// Sending time will be delayedReplyTime, snapped to ~125MHz or ~250MHz boundary by |
432 | 414 |
// zeroing its low 9 bits, and then having the TX antenna delay added |
... | ... | |
451 | 433 |
message = 0; |
452 | 434 |
//fall into the next case (turn on the RX) |
453 | 435 |
} |
454 |
|
|
455 | 436 |
} |
456 |
// TODO: fall through to the next case statement in case of debugging |
|
457 | 437 |
break ; // end case TA_TX_WAIT_CONF |
458 | 438 |
|
459 | 439 |
|
... | ... | |
476 | 456 |
// end case TA_RXE_WAIT, don't break, but fall through into the TA_RX_WAIT_DATA state to process it immediately. |
477 | 457 |
if(message == 0) break; |
478 | 458 |
} |
479 |
// TODO: fall through to the next case statement in case of debugging |
|
480 | 459 |
break; |
481 | 460 |
|
482 |
case TA_RX_WAIT_DATA : // Wait RX data
|
|
461 |
case TA_RX_WAIT_DATA : // Wait RX data |
|
483 | 462 |
//printf("TA_RX_WAIT_DATA %d", message) ; |
484 | 463 |
switch (message){ |
485 | 464 |
|
... | ... | |
493 | 472 |
//int srclen = 0; |
494 | 473 |
//int fctrladdr_len; |
495 | 474 |
uint8_t tof_idx = 0; |
496 |
uint8_t *messageData; |
|
475 |
uint8_t *messageData = NULL;
|
|
497 | 476 |
|
498 | 477 |
inst->stopTimer = 0; //clear the flag, as we have received a message |
499 | 478 |
|
... | ... | |
629 | 608 |
} |
630 | 609 |
} |
631 | 610 |
} |
632 |
/*else |
|
633 |
{ |
|
634 |
//stay in RX wait for next frame... |
|
635 |
inst->testAppState = TA_RXE_WAIT ; // wait for next frame |
|
636 |
}*/ |
|
637 | 611 |
|
638 | 612 |
if(fcode == RTLS_DEMO_MSG_ANCH_RESP) { //tag to anchor mode |
639 | 613 |
if(currentRangeNum == inst->rangeNum) { //these are the previous ranges... |
... | ... | |
692 | 666 |
uint64_t tagFinalRxTime = 0; |
693 | 667 |
uint64_t tagPollTxTime = 0; |
694 | 668 |
uint64_t anchorRespRxTime = 0; |
695 |
uint64_t tof = INVALID_TOF;
|
|
669 |
int32_t tof = (int32_t)INVALID_TOF;
|
|
696 | 670 |
|
697 | 671 |
double RaRbxDaDb = 0; |
698 | 672 |
double RbyDb = 0; |
... | ... | |
758 | 732 |
|
759 | 733 |
RayDa = ((double)Ra + (double)Da); |
760 | 734 |
|
761 |
tof = (int32_t) ( RaRbxDaDb/(RbyDb + RayDa) );
|
|
735 |
tof = (int32_t)(RaRbxDaDb/(RbyDb + RayDa));
|
|
762 | 736 |
} |
763 | 737 |
|
764 | 738 |
//tag to anchor ranging |
765 | 739 |
if(RTLS_DEMO_MSG_TAG_FINAL == fcode) { |
766 | 740 |
inst->newRangeTagAddress = srcAddr[0] + ((uint16_t) srcAddr[1] << 8); |
767 | 741 |
//time-of-flight |
768 |
inst->tof[inst->newRangeTagAddress & 0x7] = tof; |
|
742 |
inst->tof[inst->newRangeTagAddress & 0x7] = (uint32_t)tof;
|
|
769 | 743 |
//calculate all tag - anchor ranges... and report |
770 | 744 |
inst->newRange = instance_calcranges(&inst->tofArray[0], MAX_ANCHOR_LIST_SIZE, TOF_REPORT_T2A, &inst->rxResponseMask); |
771 | 745 |
inst->rxResponseMaskReport = inst->rxResponseMask; //copy the valid mask to report |
772 | 746 |
inst->rxResponseMask = 0; |
773 | 747 |
//we have our range - update the own mask entry... |
774 |
if(tof != INVALID_TOF) { //check the last ToF entry is valid and copy into the current array |
|
748 |
if(tof != (int32_t)INVALID_TOF) { //check the last ToF entry is valid and copy into the current array
|
|
775 | 749 |
setTagDist(srcAddr[0], inst->shortAdd_idx); //copy distance from this anchor to the tag into array |
776 | 750 |
|
777 |
inst->rxResponseMask = (0x1 << inst->shortAdd_idx); |
|
778 |
inst->tofArray[inst->shortAdd_idx] = tof; |
|
751 |
inst->rxResponseMask = (uint8_t)(0x1 << inst->shortAdd_idx);
|
|
752 |
inst->tofArray[inst->shortAdd_idx] = (uint32_t)tof;
|
|
779 | 753 |
} |
780 | 754 |
inst->newRangeTime = dw_event->uTimeStamp ; |
781 | 755 |
} |
782 | 756 |
else { //anchor to anchor ranging |
783 | 757 |
inst->newRangeTagAddress = srcAddr[0] + ((uint16_t) srcAddr[1] << 8); |
784 | 758 |
//time-of-flight |
785 |
inst->tofAnc[tof_idx] = tof; |
|
759 |
inst->tofAnc[tof_idx] = (uint32_t)tof;
|
|
786 | 760 |
} |
787 | 761 |
|
788 | 762 |
//reset the response count |
... | ... | |
884 | 858 |
float msgdatalen = 0; |
885 | 859 |
float preamblelen = 0; |
886 | 860 |
int sfdlen = 0; |
887 |
int x = 0;
|
|
861 |
float x = 0;
|
|
888 | 862 |
|
889 | 863 |
//Set the RX timeouts based on the longest expected message - the Final message |
890 | 864 |
//Poll = 13, Response = 20, Final = 44 bytes |
891 | 865 |
//msgdatalen = TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC; |
892 | 866 |
msgdatalen = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC; |
893 | 867 |
|
894 |
x = (int) ceil(msgdatalen*8/330.0f);
|
|
868 |
x = (float)ceil((double)(msgdatalen*8)/(double)330.0f);
|
|
895 | 869 |
|
896 | 870 |
msgdatalen = msgdatalen*8 + x*48; |
897 | 871 |
|
898 | 872 |
//add some margin so we don't timeout too soon |
899 | 873 |
margin = 0; //(TAG_FINAL_MSG_LEN - TAG_POLL_MSG_LEN); |
900 | 874 |
|
901 |
x = (int) ceil(margin*8/330.0f);
|
|
875 |
x = (float) ceil((double)(margin*8)/(double)330.0f);
|
|
902 | 876 |
|
903 |
margin = margin*8 + x*48;
|
|
877 |
margin = (int) (margin*8 + x*48);
|
|
904 | 878 |
|
905 | 879 |
//assume PHR length is 172308ns for 110k and 21539ns for 850k/6.81M |
906 | 880 |
if(instance_data[instance].configData.dataRate == DWT_BR_110K) { |
... | ... | |
946 | 920 |
preamblelen = (sfdlen + preamblelen) * 1.01763f; |
947 | 921 |
} |
948 | 922 |
|
949 |
respframe_sy = (16 + (int)((preamblelen + ((msgdatalen + margin)/1000.0))/ 1.0256)) ;
|
|
923 |
respframe_sy = (16 + (int)((double)((double)preamblelen + ((double)(msgdatalen + margin)/1000.0))/ 1.0256)) ;
|
|
950 | 924 |
|
951 | 925 |
//this is the delay used for the delayed transmit (when sending the response, and final messages) |
952 | 926 |
instance_data[instance].pollTx2FinalTxDelay = convertmicrosectodevicetimeu (delayus); |
... | ... | |
959 | 933 |
//andhor 3 will have the delay set to 3 * fixedReplyDelayAnc and so on... |
960 | 934 |
//this delay depends on how quickly the tag can receive and process the message from previous anchor |
961 | 935 |
//(and also the frame length of course) |
962 |
respframe = (int)(preamblelen + (msgdatalen/1000.0)); //length of response frame (micro seconds)
|
|
936 |
respframe = (int)((double)preamblelen + ((double)msgdatalen/1000.0)); //length of response frame (micro seconds)
|
|
963 | 937 |
if(instance_data[instance].configData.dataRate == DWT_BR_110K) { |
964 | 938 |
|
965 | 939 |
//set the frame wait timeout time - total time the frame takes in symbols |
... | ... | |
967 | 941 |
|
968 | 942 |
instance_data[instance].fwtoTimeAnc_sy = respframe_sy; //add some margin so we don't timeout too soon |
969 | 943 |
instance_data[instance].fixedReplyDelayAnc = convertmicrosectodevicetimeu (respframe + RX_RESPONSE1_TURNAROUND_110K); |
970 |
instance_data[instance].fixedReplyDelayAncP = (uint32_t) (((uint64_t) convertmicrosectodevicetimeu (preamblelen)) >> 8) + 16; |
|
944 |
instance_data[instance].fixedReplyDelayAncP = (uint32_t) (((uint64_t) convertmicrosectodevicetimeu ((double)preamblelen)) >> 8) + 16;
|
|
971 | 945 |
|
972 | 946 |
instance_data[instance].ancRespRxDelay = RX_RESPONSE1_TURNAROUND_110K ; |
973 | 947 |
} |
... | ... | |
978 | 952 |
|
979 | 953 |
instance_data[instance].fwtoTimeAnc_sy = respframe_sy; |
980 | 954 |
instance_data[instance].fixedReplyDelayAnc = convertmicrosectodevicetimeu (respframe + RX_RESPONSE1_TURNAROUND_6M81); |
981 |
instance_data[instance].fixedReplyDelayAncP = (uint32_t) (((uint64_t) convertmicrosectodevicetimeu (preamblelen)) >> 8) + 16; |
|
955 |
instance_data[instance].fixedReplyDelayAncP = (uint32_t) (((uint64_t) convertmicrosectodevicetimeu ((double)preamblelen)) >> 8) + 16;
|
|
982 | 956 |
|
983 | 957 |
instance_data[instance].ancRespRxDelay = RX_RESPONSE1_TURNAROUND_6M81 ; |
984 | 958 |
} |
985 |
|
|
986 | 959 |
} |
987 | 960 |
|
988 | 961 |
// ------------------------------------------------------------------------------------------------------------------- |
Also available in: Unified diff