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