def computeRangeAsymmetric(self):
"""
This is the function which calculates the timestamp used to determine the range between the devices.
"""
#global self.timeComputedRangeTS
self.round1 = DW1000.wrapTimestamp(self.timePollAckReceivedTS - self.timePollSentTS)
self.reply1 = DW1000.wrapTimestamp(self.timePollAckSentTS - self.timePollReceivedTS)
self.round2 = DW1000.wrapTimestamp(self.timeRangeReceivedTS - self.timePollAckSentTS)
self.reply2 = DW1000.wrapTimestamp(self.timeRangeSentTS - self.timePollAckReceivedTS)
self.timeComputedRangeTS = (self.round1 * self.round2 - self.reply1 * self.reply2) / (self.round1 + self.round2 + self.reply1 + self.reply2)
def computeRangeAsymmetric():
"""
This is the function which calculates the timestamp used to determine the range between the devices.
"""
global timeComputedRangeTS
round1 = DW1000.wrapTimestamp(timePollAckReceivedTS - timePollSentTS)
reply1 = DW1000.wrapTimestamp(timePollAckSentTS - timePollReceivedTS)
round2 = DW1000.wrapTimestamp(timeRangeReceivedTS - timePollAckSentTS)
reply2 = DW1000.wrapTimestamp(timeRangeSentTS - timePollAckReceivedTS)
timeComputedRangeTS = (round1 * round2 - reply1 * reply2) / (round1 + round2 + reply1 + reply2)
def getRange(self):
assert self.type == TAG, "Tags are not equipped to find distance from anchors"
round1 = DW1000.wrapTimestamp(self.timePollAckReceived -
self.timePollSent)
reply1 = DW1000.wrapTimestamp(self.timePollAckSent -
self.timePollReceived)
round2 = DW1000.wrapTimestamp(self.timeRangeReceived -
self.timePollAckSent)
reply2 = DW1000.wrapTimestamp(self.timeRangeSent - timePollAckReceived)
return (round1 * round2 - reply1 * reply2) / (round1 + round2 +
reply1 + reply2)
def loop():
global sentAck, receivedAck, data, timePollAckReceivedTS, timePollSentTS, timeRangeSentTS, expectedMsgId
if (sentAck == False and receivedAck == False):
if ((millis() - lastActivity) > C.RESET_PERIOD):
resetInactive()
return False
if sentAck:
sentAck = False
msgID = data[0]
if msgID == C.POLL:
timePollSentTS = DW1000.getTransmitTimestamp()
elif msgID == C.RANGE:
timeRangeSentTS = DW1000.getTransmitTimestamp()
noteActivity()
if receivedAck:
receivedAck = False
data = DW1000.getData(LEN_DATA)
# Dont accept data if sender is a Tag or if data is not for me (TAG ID's starts from 21)
if data[17] > 20 or data[18] != tagID:
return
msgID = data[0]
# print "Sender : ", data[17]
# print "Receiver : ",data[18]
if msgID != expectedMsgId:
print "Unexpected Messege"
expectedMsgId = C.POLL_ACK
transmitPoll()
return
if msgID == C.POLL_ACK:
# print "Received poll ack from : ", data[17]
timePollAckReceivedTS = DW1000.getReceiveTimestamp()
expectedMsgId = C.RANGE_REPORT
transmitRange()
noteActivity()
elif msgID == C.RANGE_REPORT:
print "Received range report from : ", data[17]
expectedMsgId = C.POLL_ACK
# time.sleep(0.03) #introduced so that it wont poll immediately after sending range
# transmitPoll()
noteActivity()
return True
elif msgID == C.RANGE_FAILED:
# print "Received range failed from : ", data[17]
expectedMsgId = C.POLL_ACK
transmitPoll()
noteActivity()
return False
def transmitter():
"""
This function configures the chip to prepare for a transmission.
It modifies the data that will be sent and start the transmission with the chosen delay.
"""
DW1000.newTransmit()
DW1000.setDataStr(msg)
DW1000.setDelay(100, C.MILLISECONDS)
DW1000.startTransmit()
def transmitPollAck(sequence):
global lastSignalServiced
print "Transmitting POLLACK"
print "Data: ", [C.POLL_ACK, MY_ADDRESS, sequence, NODE_TYPE]
DW1000.newTransmit()
DW1000.setDelay(REPLY_DELAY_TIME_US, C.MICROSECONDS)
DW1000.setData([C.POLL_ACK, MY_ADDRESS, sequence, NODE_TYPE], 4)
DW1000.startTransmit()
lastSignalServiced = SEND_POLL_ACK
def transmitPollAck(self, data, LEN_DATA):
"""
This function sends the polling acknowledge message which is used to confirm the reception of the polling message.
"""
DW1000.newTransmit()
data[0] = C.POLL_ACK
DW1000.setDelay(self.REPLY_DELAY_TIME_US, C.MICROSECONDS)
DW1000.setData(data, LEN_DATA)
DW1000.startTransmit()
def loop(self):
#global self.sentAck, self.receivedAck, self.timePollAckSentTS, self.timePollReceivedTS, self.timePollSentTS, self.timePollAckReceivedTS, self.timeRangeReceivedTS, self.protocolFailed, self.data, self.expectedMsgId, self.timeRangeSentTS
#current_time = millis()
if (self.sentAck == False and self.receivedAck == False):
if ((self.millis() - self.lastActivity) > C.RESET_PERIOD):
self.resetInactive()
return
if self.sentAck:
self.sentAck = False
self.msgId = self.data[0]
if self.msgId == C.POLL_ACK:
self.timePollAckSentTS = DW1000.getTransmitTimestamp()
self.noteActivity()
if self.receivedAck:
self.receivedAck = False
self.data = DW1000.getData(self.LEN_DATA)
#data.reverse()
#print('Data vector is: ')
#print(data)
self.msgId = self.data[0]
#print('Message id is: ')
#print(msgId)
if self.msgId != self.expectedMsgId:
#print('protocolFailed')
self.protocolFailed = True
if self.msgId == C.POLL:
self.protocolFailed = False
self.timePollReceivedTS = DW1000.getReceiveTimestamp()
self.expectedMsgId = C.RANGE
self.transmitPollAck()
self.noteActivity()
#print('POLL')
elif self.msgId == C.RANGE:
self.timeRangeReceivedTS = DW1000.getReceiveTimestamp()
self.expectedMsgId = C.POLL
#print('msgId = Range')
if self.protocolFailed == False:
self.timePollSentTS = DW1000.getTimeStamp(self.data, 1)
self.timePollAckReceivedTS = DW1000.getTimeStamp(self.data, 6)
self.timeRangeSentTS = DW1000.getTimeStamp(self.data, 11)
self.computeRangeAsymmetric()
self.transmitRangeAcknowledge()
self.distance = (self.timeComputedRangeTS % C.TIME_OVERFLOW) * C.DISTANCE_OF_RADIO
#print("Distance: %.2f m" %(self.distance))
return self.distance
#Sample rate
#if millis() - rangingCountPeriod > 1000:
# samplingRate = (1000.0 * successRangingCount) / (millis() - rangingCountPeriod)
# rangingCountPeriod = millis()
# successRangingCount = 0
else:
self.transmitRangeFailed()
self.noteActivity()
def loop():
global sentAck, receivedAck, data, timePollAckReceivedTS, timePollSentTS, timeRangeSentTS, expectedMsgId
if (sentAck == False and receivedAck == False):
if ((millis() - lastActivity) > C.RESET_PERIOD):
resetInactive()
return
if sentAck:
sentAck = False
msgID = data[0]
if msgID == C.POLL:
timePollSentTS = DW1000.getTransmitTimestamp()
elif msgID == C.RANGE:
timeRangeSentTS = DW1000.getTransmitTimestamp()
noteActivity()
if receivedAck:
receivedAck = False
data = DW1000.getData(LEN_DATA)
if data[17] > 20 or data[18] != tagID:
return
msgID = data[0]
print "Sender : ", data[17]
print "Receiver : ",data[18]
if msgID != expectedMsgId:
expectedMsgId = C.POLL_ACK
transmitPoll()
return
if msgID == C.POLL_ACK:
print "Received poll ack from : ", data[17]
timePollAckReceivedTS = DW1000.getReceiveTimestamp()
expectedMsgId = C.RANGE_REPORT
transmitRange()
noteActivity()
elif msgID == C.RANGE_REPORT:
print "Received range ack from : ", data[17]
expectedMsgId = C.POLL_ACK
transmitPoll()
noteActivity()
elif msgID == C.RANGE_FAILED:
print "Received range failed from : ", data[17]
expectedMsgId = C.POLL_ACK
transmitPoll()
noteActivity()
def getRange(self):
assert self.type == DW1000Device.ANCHOR, "Anchors are not equipped to find distance from anchors"
round1 = DW1000.wrapTimestamp(
self.timePollAckReceived[self.sequenceNumber] -
self.timePollSent[self.sequenceNumber])
reply1 = DW1000.wrapTimestamp(
self.timePollAckSent[self.sequenceNumber] -
self.timePollReceived[self.sequenceNumber])
round2 = DW1000.wrapTimestamp(
self.timeRangeReceived[self.sequenceNumber] -
self.timePollAckSent[self.sequenceNumber])
reply2 = DW1000.wrapTimestamp(
self.timeRangeSent[self.sequenceNumber] -
self.timePollAckReceived[self.sequenceNumber])
# print "round1 {} reply1 {}".format(round1, reply1)
# print "round2 {} reply2 {}".format(round2, reply2)
self.deletePreviousSequenceData()
return (round1 * round2 - reply1 * reply2) / (round1 + round2 +
reply1 + reply2)
def loop():
global sentAck, receivedAck, timePollAckSentTS, timePollReceivedTS, timePollSentTS, timePollAckReceivedTS, timeRangeReceivedTS, protocolFailed, data, expectedMsgId, timeRangeSentTS, successRangingCount, rangingCountPeriod, samplingRate, lastsampletime
#current_time = millis()
if (sentAck == False and receivedAck == False):
if ((millis() - lastActivity) > C.RESET_PERIOD):
resetInactive()
return
if sentAck:
sentAck = False
msgId = data[0]
if msgId == C.POLL_ACK:
timePollAckSentTS = DW1000.getTransmitTimestamp()
noteActivity()
if receivedAck:
receivedAck = False
data = DW1000.getData(LEN_DATA)
#data.reverse()
#print('Data vector is: ')
#print(data)
msgId = data[0]
#print('Message id is: ')
#print(msgId)
if msgId != expectedMsgId:
#print('protocolFailed')
protocolFailed = True
if msgId == C.POLL:
protocolFailed = False
timePollReceivedTS = DW1000.getReceiveTimestamp()
expectedMsgId = C.RANGE
transmitPollAck()
noteActivity()
#print('POLL')
elif msgId == C.RANGE:
timeRangeReceivedTS = DW1000.getReceiveTimestamp()
expectedMsgId = C.POLL
#print('msgId = Range')
if protocolFailed == False:
timePollSentTS = DW1000.getTimeStamp(data, 1)
timePollAckReceivedTS = DW1000.getTimeStamp(data, 6)
timeRangeSentTS = DW1000.getTimeStamp(data, 11)
computeRangeAsymmetric()
transmitRangeAcknowledge()
distance = (timeComputedRangeTS %
C.TIME_OVERFLOW) * C.DISTANCE_OF_RADIO
print("Distance: %.2f m" % (distance))
#Sample rate
#if millis() - rangingCountPeriod > 1000:
# samplingRate = (1000.0 * successRangingCount) / (millis() - rangingCountPeriod)
# rangingCountPeriod = millis()
# successRangingCount = 0
else:
transmitRangeFailed()
noteActivity()
def handleSent():
global timePollAckSent
reply.sender = MY_ADDRESS
reply.sequence = sequence
reply.signal = lastSignalServiced
if lastSignalServiced == SEND_POLL_ACK:
timePollAckSent[sequence] = DW1000.getTransmitTimestamp()
print "Poll Ack sent for {} with timestamp {}".format(
sequence, timePollAckSent[sequence])
replyPub.publish(reply)
def loop(currentTag):
global sentAck, receivedAck, timePollAckSentTS, timePollReceivedTS, timePollSentTS, timePollAckReceivedTS, timeRangeReceivedTS, protocolFailed, data, expectedMsgId, timeRangeSentTS, anchorID, tagID
tagID = currentTag
if (sentAck == False and receivedAck == False):
if ((millis() - lastActivity) > C.RESET_PERIOD):
# print tagID
resetInactive()
return False, 0
if sentAck:
sentAck = False
msgId = data[0]
if msgId == C.POLL_ACK:
timePollAckSentTS = DW1000.getTransmitTimestamp()
noteActivity()
if receivedAck:
receivedAck = False
if (DW1000.getData(LEN_DATA)[18] == anchorID) and (DW1000.getData(LEN_DATA)[17] == currentTag):
data = DW1000.getData(LEN_DATA)
msgId = data[0]
if msgId != expectedMsgId:
protocolFailed = True
if msgId == C.POLL:
protocolFailed = False
timePollReceivedTS = DW1000.getReceiveTimestamp()
expectedMsgId = C.RANGE
transmitPollAck()
noteActivity()
elif msgId == C.RANGE:
timeRangeReceivedTS = DW1000.getReceiveTimestamp()
expectedMsgId = C.POLL
if protocolFailed == False:
timePollSentTS = DW1000.getTimeStamp(data, 1)
timePollAckReceivedTS = DW1000.getTimeStamp(data, 6)
timeRangeSentTS = DW1000.getTimeStamp(data, 11)
computeRangeAsymmetric()
distance = (timeComputedRangeTS % C.TIME_OVERFLOW) * C.DISTANCE_OF_RADIO
# print("Distance: %.2f m from tag %d" %(distance, tagID))
# print(tagID)
transmitRangeAcknowledge()
return True, distance
else:
transmitRangeFailed()
noteActivity()
return False, 0
def transmitPollAck():
"""
This function sends the polling acknowledge message which is used to confirm the reception of the polling message.
"""
global data
DW1000.newTransmit()
data[0] = C.POLL_ACK
data[17] = anchorID #data[17] is tag Id data[18] is anchor Id
data[18] = tagID #data[17] is tag Id data[18] is anchor Id
DW1000.setDelay(REPLY_DELAY_TIME_US, C.MICROSECONDS)
DW1000.setData(data, LEN_DATA)
DW1000.startTransmit()
def Anchor_resetInactive(self):
"""
This function restarts the default polling operation when the device is deemed inactive.
"""
DW1000.generalConfiguration("82:17:5B:D5:A9:9A:E2:9B",
C.MODE_LONGDATA_RANGE_ACCURACY)
DW1000.registerCallback("handleSent", self.handleSent)
DW1000.registerCallback("handleReceived", self.handleReceived)
DW1000.setAntennaDelay(C.ANTENNA_DELAY_RASPI)
self.receiver()
lastActivity = self.noteActivity()
return lastActivity
def transmitter():
"""
This function configures the chip to prepare for a transmission.
It modifies the data that will be sent and start the transmission with the chosen delay.
"""
global number
DW1000.newTransmit()
msg = "Hello Raspi3, it's #" + str(number)
DW1000.setDataStr(msg)
DW1000.setDelay(SEND_DELAY, C.MILLISECONDS)
DW1000.startTransmit()
number += 1
def main():
# initialize DWM settings
DW1000.registerCallback("handleSent", handleSent)
DW1000.registerCallback("handleReceived", handleReceived)
receiver()
noteActivity()
queue = Queue()
imu_queue = Get_IMU_Data('IMU.', queue)
uwb_queue = Get_UWB_Data('UWB.', queue)
# euler_queue = EKF_Cal_Euler('Euler.',queue)
data_queue = Save_Data('Save Data', queue)
imu_queue.start()
uwb_queue.start()
# euler_queue.start()
data_queue.start()
imu_queue.join()
uwb_queue.join()
# euler_queue.join()
data_queue.join()
print('All threads terminate!')
def loop():
global sentAck, receivedAck, data, timePollAckReceivedTS, timePollSentTS, timeRangeSentTS, expectedMsgId
if (sentAck == False and receivedAck == False):
if ((millis() - lastActivity) > C.RESET_PERIOD):
resetInactive()
return
if sentAck:
sentAck = False
msgID = data[0]
#print(data[16],1)
#os.system("python ./DW1000RangingAnchor.py")
if data[16]==28:
if msgID == C.POLL:
timePollSentTS = DW1000.getTransmitTimestamp()
elif msgID == C.RANGE:
timeRangeSentTS = DW1000.getTransmitTimestamp()
noteActivity()
if receivedAck:
receivedAck = False
data = DW1000.getData(LEN_DATA)
msgID = data[0]
print(data[16],2)
if data[16]==28:
print(data[16])
if msgID == C.POLL_ACK:
timePollAckReceivedTS = DW1000.getReceiveTimestamp()
expectedMsgID = C.RANGE_REPORT
transmitRange()
noteActivity()
elif msgID == C.RANGE_REPORT:
expectedMsgId = C.POLL_ACK
transmitPoll()
noteActivity()
elif msgID == C.RANGE_FAILED:
expectedMsgId = C.POLL_ACK
transmitPoll()
noteActivity()
def receiver():
"""
This function configures the chip to prepare for a message reception.
"""
DW1000.newReceive()
DW1000.receivePermanently()
DW1000.startReceive()
def Anchor_resetInactive():
"""
This function restarts the default polling operation when the device is deemed inactive.
"""
global expectedMsgId
DW1000.generalConfiguration("82:17:5B:D5:A9:9A:E2:9B",
C.MODE_LONGDATA_RANGE_ACCURACY)
DW1000.registerCallback("handleSent", handleSent)
DW1000.registerCallback("handleReceived", handleReceived)
DW1000.setAntennaDelay(C.ANTENNA_DELAY_RASPI)
expectedMsgId = C.POLL
print("run")
receiver()
noteActivity()
def handleReceived():
global timePollReceived, timeRangeReceived
msgType, sender, sequence, node_type = DW1000.getData(4)
print "Received Data: ", [msgType, sender, sequence, node_type]
if node_type == NODE_TYPE:
return
if msgType == C.POLL:
timePollReceived[sender] = {sequence: DW1000.getReceiveTimestamp()}
print "Poll received from {} for seq {} with timestamp {}"\
.format(sender, sequence, timePollReceived[sender][sequence])
elif msgType == C.RANGE \
and sender in timePollReceived.keys() \
and sequence in timePollReceived[sender].keys() \
and sequence in timePollAckSent.keys():
timeRangeReceived[sender] = {sequence: DW1000.getReceiveTimestamp()}
print "Range received from {} for seq {} with timestamp {}"\
.format(sender, sequence, timeRangeReceived[sender][sequence])
timestampPub.publish(getTimeStampForSequence(sequence, sender))
else:
print "TIME POLL RECEIVED", timePollReceived
print "TIME POLL ACK SENT", timePollAckSent
print "TIME RANGE RECEIVED", timeRangeReceived
def loop():
global sentAck, receivedAck, data, timePollAckReceivedTS, timePollSentTS, timeRangeSentTS, expectedMsgId
if (sentAck == False and receivedAck == False):
if ((millis() - lastActivity) > C.RESET_PERIOD):
resetInactive()
return
if sentAck:
sentAck = False
msgID = data[0]
if msgID == C.POLL:
timePollSentTS = DW1000.getTransmitTimestamp()
#print "timePollSentTS : {}".format(timePollSentTS)
elif msgID == C.RANGE:
timeRangeSentTS = DW1000.getTransmitTimestamp()
noteActivity()
if receivedAck:
receivedAck = False
data = DW1000.getData(LEN_DATA)
msgID = data[0]
if msgID != expectedMsgId:
expectedMsgId = C.POLL_ACK
transmitPoll()
return
if msgID == C.POLL_ACK:
timePollAckReceivedTS = DW1000.getReceiveTimestamp()
#print "timePollAckReceivedTS : {}".format(timePollAckReceivedTS)
expectedMsgId = C.RANGE_REPORT
transmitRange()
noteActivity()
elif msgID == C.RANGE_REPORT:
expectedMsgId = C.POLL_ACK
transmitPoll()
noteActivity()
elif msgID == C.RANGE_FAILED:
expectedMsgId = C.POLL_ACK
transmitPoll()
noteActivity()
def transmitRangeAcknowledge(self, data, LEN_DATA):
"""
This functions sends the range acknowledge message which tells the tag that the ranging function was successful and another ranging transmission can begin.
"""
DW1000.newTransmit()
data[0] = C.RANGE_REPORT
DW1000.setData(data, LEN_DATA)
DW1000.startTransmit()
def transmitRangeFailed(self, data, LEN_DATA):
"""
This functions sends the range failed message which tells the tag that the ranging function has failed and to start another ranging transmission.
"""
DW1000.newTransmit()
data[0] = C.RANGE_FAILED
DW1000.setData(data, LEN_DATA)
DW1000.startTransmit()
def transmitPollAck():
"""
This function sends the polling acknowledge message which is used to confirm the reception of the polling message.
"""
global data, SEQ_NO
##print "transmitPollAck"
DW1000.newTransmit()
data[0] = C.POLL_ACK
data[19] = SEQ_NO
SEQ_NO += 1
if SEQ_NO == 256:
SEQ_NO = 0
DW1000.setDelay(REPLY_DELAY_TIME_US, C.MICROSECONDS)
DW1000.setData(data, LEN_DATA)
DW1000.startTransmit()
def transmitPollAck(address, anchor_init_over=0):
"""
This function sends the polling acknowledge message which is used to confirm the reception of the polling message.
"""
global data, myAddress, nodeType, tag_list
print "transmitPollAck"
DW1000.newTransmit()
data[0] = C.POLL_ACK
data[16] = myAddress
data[17] = address
data[19] = nodeType
data[20] = anchor_init_over
DW1000.setDelay(REPLY_DELAY_TIME_US, C.MICROSECONDS)
DW1000.setData(data, LEN_DATA)
DW1000.startTransmit()
noteActivity()
def receiver():
"""
This function configures the chip to prepare for a message reception.
"""
global data
print "receiver"
DW1000.newReceive()
DW1000.receivePermanently()
DW1000.startReceive()
noteActivity()
def loop():
global sentAck, receivedAck, timePollAckSentTS, timePollReceivedTS, timePollSentTS, timePollAckReceivedTS, timeRangeReceivedTS, protocolFailed, data, expectedMsgId, timeRangeSentTS, SEQ_NO
if (sentAck == False and receivedAck == False):
if ((millis() - lastActivity) > C.RESET_PERIOD):
resetInactive()
return
if sentAck:
sentAck = False
msgId = data[0]
##print msgId
if msgId == C.POLL_ACK:
##print "pollack"
timePollAckSentTS = DW1000.getTransmitTimestamp()
#print "timePollAckSentTS for SEQ_NO {} : {}".format(SEQ_NO, timePollAckSentTS)
noteActivity()
if receivedAck:
receivedAck = False
data = DW1000.getData(LEN_DATA)
#print "getting data ", data
msgId = data[0]
if msgId != expectedMsgId:
#print "protocolFailed"
protocolFailed = True
if msgId == C.POLL:
protocolFailed = False
timePollReceivedTS = DW1000.getReceiveTimestamp()
#print "timePollReceivedTS for SEQ_NO {} : {}".format(SEQ_NO, timePollReceivedTS)
expectedMsgId = C.RANGE
transmitPollAck()
noteActivity()
elif msgId == C.RANGE:
timeRangeReceivedTS = DW1000.getReceiveTimestamp()
expectedMsgId = C.POLL
if protocolFailed == False:
timePollSentTS = DW1000.getTimeStamp(data, 1)
timePollAckReceivedTS = DW1000.getTimeStamp(data, 6)
timeRangeSentTS = DW1000.getTimeStamp(data, 11)
computeRangeAsymmetric()
transmitRangeAcknowledge()
distance = (timeComputedRangeTS % C.TIME_OVERFLOW) * C.DISTANCE_OF_RADIO
# distance = (timeComputedRangeTS % C.TIME_OVERFLOW) * C.SPEED_OF_LIGHT / 1000
#print timeComputedRangeTS
print("Distance: %.2f m" %(distance))
else:
transmitRangeFailed()
noteActivity()
def transmitRangeAcknowledge():
"""
This functions sends the range acknowledge message which tells the tag that the ranging function was successful and another ranging transmission can begin.
"""
global data
DW1000.newTransmit()
data[0] = C.RANGE_REPORT
data[17] = anchorID #data[17] is tag Id data[18] is anchor Id
data[18] = tagID #data[17] is tag Id data[18] is anchor Id
DW1000.setData(data, LEN_DATA)
DW1000.startTransmit()
def transmitRangeFailed():
"""
This functions sends the range failed message which tells the tag that the ranging function has failed and to start another ranging transmission.
"""
global data
DW1000.newTransmit()
data[0] = C.RANGE_FAILED
data[17] = anchorID #data[17] is tag Id data[18] is anchor Id
data[18] = tagID #data[17] is tag Id data[18] is anchor Id
DW1000.setData(data, LEN_DATA)
DW1000.startTransmit()