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()