def indentify_devices(self, devices_setting):
self.logger.info("Setting nodes : ".format(json.dumps(devices_setting)))
nodes = [RTLSNode(node["com_port"], node["baud_rate"], node["name"]) for node in devices_setting]
_rtls_manager = RTLSManager(nodes, websocket_port=None)
_rtls_manager_subscriber = _rtls_manager.create_subscriber()
_rtls_manager.auto_params = False
_rtls_manager.start()
self.logger.info("RTLS Manager started")
time.sleep(2)
_master_node, _passive_nodes, failed = _rtls_manager.wait_identified()
_all_nodes = [pn for pn in _passive_nodes] ## deep copy
_all_nodes.extend([_master_node])
_all_nodes.extend([f for f in failed])
_rtls_manager.stop()
while not _rtls_manager.stopped:
time.sleep(0.1)
_rtls_manager_subscriber = None
_rtls_manager = None
return _all_nodes
def set_devices(self, devices_setting):
self.logger.info("Setting nodes : ".format(
json.dumps(devices_setting)))
nodes = [
RTLSNode(node["com_port"], node["baud_rate"], node["name"])
for node in devices_setting
]
self._rtls_manager = RTLSManager(nodes,
websocket_port=self.websocket_port)
self._rtls_manager_subscriber = self._rtls_manager.create_subscriber()
self._rtls_manager.auto_params = False
self._start_message_receiver()
self.logger.info("Message receiver started")
self._rtls_manager.start()
self.logger.info("RTLS Manager started")
time.sleep(2)
self._master_node, self._passive_nodes, failed = self._rtls_manager.wait_identified(
)
if self._master_node is None:
raise RtlsUtilMasterNotFoundException(
"No one of the nodes identified as RTLS MASTER")
# elif len(self._passive_nodes) == 0:
# raise RtlsUtilPassiveNotFoundException("No one of the nodes identified as RTLS PASSIVE")
elif len(failed) > 0:
raise RtlsUtilNodesNotIdentifiedException(
"{} nodes not identified at all".format(len(failed)), failed)
else:
pass
self._all_nodes = [pn for pn in self._passive_nodes] ## deep copy
self._all_nodes.extend([self._master_node])
for node in self._all_nodes:
node.cci_started = False
node.aoa_initialized = False
node.tof_calib_info = None
node.tof_calib_read_return = False
node.tof_initialized = False
node.seed_initialized = False
node.tof_calibration_configured = False
node.tof_calibrated = False
node.tof_started = False
node.ble_connected = False
node.device_resets = False
self.logger.info("Done setting node")
return self._master_node, self._passive_nodes, self._all_nodes
import time
from rtls import RTLSManager, RTLSNode
import numpy as np
import matplotlib.pyplot as plt
import statistics as stat
import json as js
# Un-comment the below to get raw serial transaction logs
# import logging, sys
# logging.basicConfig(stream=sys.stdout, level=logging.DEBUG,
# format='[%(asctime)s] {%(filename)s:%(lineno)d} %(levelname)s - %(message)s')
if __name__ == '__main__':
# Initialize, but don't start RTLS Nodes to give to the RTLSManager
my_nodes = [RTLSNode('COM11', 115200)]
# Initialize refer
# ences to the connected devices
master_node = None
passive_nodes = []
# Initialize references to the connected devices
address = None
address_type = None
# ToF related settings
samples_per_burst = 64 # Should be a power of 2. Hint: in a 100ms interval, there are about 300~ samples ES EL NUMERO DE SYNC WORDS
tof_freq_list = [2408, 2412, 2414, 2418, 2420,
2424] # Other options: 2414, 2420
tof_num_freq = len(tof_freq_list)
auto_tof_rssi = -55
import logging
import queue
import sys
import time
from rtls import RTLSNode, Subscriber
logging.basicConfig(
stream=sys.stdout,
level=logging.INFO,
format='[%(asctime)s] {%(filename)s:%(lineno)d} %(levelname)s - %(message)s'
)
try:
passivenode1 = RTLSNode('COM8', 115200, "Passive")
passivenode1.start()
masternode = RTLSNode('COM14', 115200, "Master")
masternode.start()
tofsub = Subscriber(queue=queue.PriorityQueue(),
interest=None,
transient=False,
eventloop=None)
masternode.add_subscriber(tofsub)
passivesub1 = Subscriber(queue=queue.PriorityQueue(),
interest=None,
transient=False,
eventloop=None)
def run_forever():
# Initialize, but don't start RTLS Nodes to give to the RTLSManager
my_nodes = [
RTLSNode('/dev/ttyACM0', 115200),
RTLSNode('/dev/ttyACM2', 115200)
]
# Initialize references to the connected devices
master_node = None
passive_nodes = []
# Initialize references to the connected devices
address = None
address_type = None
# AoA related settings
aoa_run_mode = 'AOA_MODE_ANGLE'
aoa_cte_scan_ovs = 4
aoa_cte_offset = 4
aoa_cte_time = 20
# Auto detect AoA or ToF support related
aoa_supported = False
# If slave addr is None, the script will connect to the first RTLS slave
# that it found. If you wish to connect to a specific device
# (in the case of multiple RTLS slaves) then you may specify the address
# explicitly as given in the comment to the right
slave_addr = None # '54:6C:0E:83:3F:3D'
# Scan list
scanResultList = list()
# connection interval in units of 1.25msec configured in connection request, default is 80 (100ms)
conn_interval = 80
# Initialize manager reference, because on Exception we need to stop the manager to stop all the threads.
manager = None
try:
# Start an RTLSManager instance without WebSocket server enabled
manager = RTLSManager(my_nodes, websocket_port=None)
# Create a subscriber object for RTLSManager messages
subscriber = manager.create_subscriber()
# Tell the manager to automatically distribute connection parameters
manager.auto_params = True
# Start RTLS Node threads, Serial threads, and manager thread
manager.start()
# Wait until nodes have responded to automatic identify command and get reference
# to single master RTLSNode and list of passive RTLSNode instances
master_node, passive_nodes, failed = manager.wait_identified()
if len(failed):
print(
f"ERROR: {len(failed)} nodes could not be identified. Are they programmed?"
)
# Exit if no master node exists
if not master_node:
raise RuntimeError("No RTLS Master node connected")
# Combined list for lookup
all_nodes = passive_nodes + [master_node]
# Initialize application variables on nodes
for node in all_nodes:
node.seed_initialized = False
node.aoa_initialized = False
#
# At this point the connected devices are initialized and ready
#
# Display list of connected devices and their capabilities
print(
f"{master_node.identifier} {', '.join([cap for cap, available in master_node.capabilities.items() if available])}"
)
# Iterate over Passives and detect their capabilities
for pn in passive_nodes:
print(
f"{pn.identifier} {', '.join([cap for cap, available in pn.capabilities.items() if available])}"
)
# Check over aggregated capabilities to see if they make sense
capabilities_per_node = [[
cap for cap, avail in node.capabilities.items() if avail
] for node in all_nodes]
# Assume AoA if all nodes are not ToF
aoa_supported = all(
not ('TOF_PASSIVE' in node_caps or 'TOF_MASTER' in node_caps)
for node_caps in capabilities_per_node)
# Check that Nodes all must be AoA
if not (aoa_supported):
raise RuntimeError("All nodes must be AoA")
# Need at least 1 passive for AoA
if aoa_supported and len(passive_nodes) == 0:
raise RuntimeError('Need at least 1 passive for AoA')
# Send an example command to each of them, from commands listed at the bottom of rtls/ss_rtls.py
for n in all_nodes:
n.rtls.identify()
while True:
# Get messages from manager
try:
identifier, msg_pri, msg = subscriber.pend(
block=True, timeout=0.05).as_tuple()
# Get reference to RTLSNode based on identifier in message
sending_node = manager[identifier]
if sending_node in passive_nodes:
print(f"PASSIVE: {identifier} --> {msg.as_json()}")
else:
print(f"MASTER: {identifier} --> {msg.as_json()}")
# If we received an error, print it.
if msg.command == 'RTLS_EVT_ERROR':
print(
f"Received RTLS_EVT_ERROR with status: {msg.payload.status}"
)
# If we received an assert, print it.
if msg.command == 'RTLS_EVT_ASSERT' and msg.type == 'AsyncReq':
raise RuntimeError(
f"Received HCI H/W Assert with code: {msg.payload.cause}"
)
# After identify is received, we start scanning
if msg.command == 'RTLS_CMD_IDENTIFY':
master_node.rtls.scan()
# Once we start scaning, we will save the address of the
# last scan response
if msg.command == 'RTLS_CMD_SCAN' and msg.type == 'AsyncReq':
# Slave address none means that we connect to any slave
if slave_addr is None:
address = msg.payload.addr
address_type = msg.payload.addrType
else:
scanResultList.append(msg.payload.addr)
scanResultList.append(msg.payload.addrType)
# Once the scan has stopped and we have a valid address, then
# connect
if msg.command == 'RTLS_CMD_SCAN_STOP':
if slave_addr is None:
if address is not None and address_type is not None:
master_node.rtls.connect(address_type, address,
conn_interval)
elif slave_addr in scanResultList:
i = scanResultList.index(slave_addr)
master_node.rtls.connect(scanResultList[i + 1],
scanResultList[i],
conn_interval)
scanResultList.clear()
else:
# If we didn't find the device, keep scanning.
master_node.rtls.scan()
# Once we are connected, then we can do stuff
if msg.command == 'RTLS_CMD_CONNECT' and msg.type == 'AsyncReq':
if msg.payload.status == 'RTLS_SUCCESS':
# Find the role based on capabilities of sending node
role = 'AOA_MASTER' if sending_node.capabilities.get(
'RTLS_MASTER', False) else 'AOA_PASSIVE'
# Send AoA params
sending_node.rtls.aoa_set_params(
role, aoa_run_mode, aoa_cte_scan_ovs,
aoa_cte_offset, aoa_cte_time)
else:
# If the connection failed, keep scanning
master_node.rtls.scan()
# Count the number of nodes that have ToF initialized
if msg.command == 'RTLS_CMD_AOA_SET_PARAMS' and msg.payload.status == 'RTLS_SUCCESS':
sending_node.aoa_initialized = True
if all([n.aoa_initialized for n in all_nodes]):
# Start AoA on the master and passive nodes
for node in all_nodes:
node.rtls.aoa_start(True)
# Wait for security seed
if msg.command == 'RTLS_CMD_TOF_GET_SEC_SEED' and msg.payload.seed is not 0:
seed = msg.payload.seed
for node in passive_nodes:
node.rtls.tof_set_sec_seed(seed)
# Wait until passives have security seed set
if msg.command == 'RTLS_CMD_TOF_SET_SEC_SEED' and msg.payload.status == 'RTLS_SUCCESS':
sending_node.seed_initialized = True
try:
with open("Output/test.json", "a") as fichier:
message = json.loads("[" + msg.as_json() + "]")
message[0]["payload"]["time"] = timestamp
message[0]["payload"]["distance"] = args.distance
message[0]["payload"]["position"] = args.position
json.dump(message[0]["payload"], fichier)
print(message[0]["payload"])
fichier.write('\n')
fichier.close
except:
print('\nProblem writing on JSON file')
break
except queue.Empty:
pass
finally:
if manager:
manager.stop()