def main():
args = parse_args()
level = logging.INFO
if args.verbose:
level = logging.DEBUG
logging.basicConfig(level=level)
if os.getuid() != 0:
logging.error("must run as root.")
sys.exit(1)
uavcan.load_dsdl(args.dsdl)
tun_fd = open_tun_interface(args.ip_address)
node = uavcan.make_node(args.interface, node_id=42)
tap_to_can = Queue()
can_to_tap = Queue()
logging.info("waiting for packets, press 3x Ctrl-C to stop...")
rx_thd = threading.Thread(target=rx_thread,
args=(tun_fd, tap_to_can,
args.packets_per_second))
tx_thd = threading.Thread(target=tx_thread, args=(tun_fd, can_to_tap))
node_thd = threading.Thread(target=node_thread,
args=(tun_fd, node, can_to_tap, tap_to_can))
rx_thd.start()
tx_thd.start()
node_thd.start()
node_thd.join()
rx_thd.join()
tx_thd.join()
def __init__(self) -> None:
super().__init__()
self.last_message = ""
self.speed = 0
self.node = node = uavcan.make_node(
'/dev/serial/by-id/usb-Zubax_Robotics_Zubax_Babel_32002E0018514D563935392000000000-if00',
node_id=10,
bitrate=1000000,
)
# setup
node_monitor = uavcan.app.node_monitor.NodeMonitor(node)
dynamic_node_id_allocator = uavcan.app.dynamic_node_id.CentralizedServer(
node, node_monitor)
# Waiting for at least one other node to appear online (our local node is already online).
while len(dynamic_node_id_allocator.get_allocation_table()) <= 1:
print('Waiting for other nodes to become online...')
node.spin(timeout=1)
node.periodic(0.05, self.update)
node.add_handler(uavcan.equipment.esc.Status, self.listen)
self.thread = threading.Thread(target=node.spin, daemon=True).start()
def __init__(self, can_iface_name: str, bitrate: int,
can_dump_directory: str, file_server_lookup_paths: list,
self_node_id: int):
os.makedirs(can_dump_directory, exist_ok=True)
can_dump_base_name = 'can_%s.dump' % time.strftime('%Y-%m-%d-%H-%M-%S')
can_dump_file = os.path.join(can_dump_directory, can_dump_base_name)
self._can_dump_file = open(can_dump_file, 'w', encoding='utf8')
self._node = uavcan.make_node(can_iface_name,
bitrate=bitrate,
mode=NODE_STATUS_CONST.MODE_OPERATIONAL)
self._node.node_info.name = 'com.zubax.bootloader_tester'
self._node.node_id = self_node_id
self._node.can_driver.add_io_hook(self._can_io_hook)
self.file_server = uavcan.app.file_server.FileServer(
self._node, file_server_lookup_paths)
self.monitor = uavcan.app.node_monitor.NodeMonitor(self._node)
self.node_id_allocator = uavcan.app.dynamic_node_id.CentralizedServer(
self._node, self.monitor)
def logging_callback(event):
logger.info('uavcan.protocol.debug.LogMessage\n%s',
uavcan.to_yaml(event))
self._node.add_handler(uavcan.protocol.debug.LogMessage,
logging_callback)
def __init__(self, can_interface, node_id):
uavcan.load_dsdl(DSDL_PATH)
node_info = (
uavcan.protocol.GetNodeInfo.Response()
)
node_info.name = "com.wibotic.ros_connector"
node_info.software_version.major = 1
try:
self.uavcan_node = uavcan.make_node(
can_interface,
node_id=node_id,
node_info=node_info,
mode=uavcan.protocol.NodeStatus().MODE_OPERATIONAL,
)
except OSError:
rospy.logerr(
"ERROR: Device not found. "
"Please confirm the device name is correctly set!"
)
else:
self.monitor = uavcan.app.node_monitor.NodeMonitor(
self.uavcan_node
)
self.uavcan_node.add_handler(
uavcan.thirdparty.wibotic.WiBoticInfo,
self.wibotic_info_callback,
)
def main():
args = parse_args()
if os.getuid() != 0:
print("must run as root.")
sys.exit(1)
uavcan.load_dsdl(args.dsdl)
tun_fd = open_tun_interface(args.ip_address)
node = uavcan.make_node(args.interface, node_id=42)
tap_to_can = Queue()
can_to_tap = Queue()
print("waiting for packets, press 3x Ctrl-C to stop...")
rx_thd = threading.Thread(target=rx_thread, args=(tun_fd, tap_to_can))
tx_thd = threading.Thread(target=tx_thread, args=(tun_fd, can_to_tap))
node_thd = threading.Thread(
target=node_thread, args=(tun_fd, node, can_to_tap, tap_to_can)
)
rx_thd.start()
tx_thd.start()
node_thd.start()
node_thd.join()
rx_thd.join()
tx_thd.join()
def main():
args = parse_args()
with open(args.config) as file:
config = yaml.load(file)
config = config['actuator'][args.board]
config = config_fill_missing(config)
# start UAVCAN node
uavcan.load_dsdl(args.dsdl)
node = uavcan.make_node(args.port, node_id=127)
# set up board discovery
disc = NodeDiscovery(node, args.board)
while disc.get_id() is None:
node.spin(0.1)
print('board "{}" has ID {}'.format(args.board, disc.get_id()))
print('Load config...')
config_send(node, config, disc.get_id())
node.spin(0.1)
def main():
args = parse_args()
if args.output:
output_file = csv.DictWriter(args.output, ['ts', 'X', 'Y', 'Z'])
output_file.writeheader()
def orientation_cb(event):
msg = event.message
print(msg.timestamp.usec)
x, y, z, w = tuple(msg.orientation_xyzw)
x, y, z = quaternion_to_euler_angle(w, x, y, z)
if args.output:
output_file.writerow({
'ts': msg.timestamp.usec,
'X': x,
'Y': y,
'Z': z
})
else:
x = math.degrees(x)
y = math.degrees(y)
z = math.degrees(z)
print("{:.1f}° {:.1f}° {:.1f}°".format(x, y, z))
node = uavcan.make_node(args.port, node_id=127)
node.add_handler(uavcan.equipment.ahrs.Solution, orientation_cb)
node.spin()
def __init__(self, interface):
self.node_lock = threading.RLock()
self.rpc_answered = threading.Semaphore(value=0)
self.node = uavcan.make_node(interface, node_id=127)
threading.Thread(target=self._uavcan_thread).start()
self.node.add_handler(uavcan.protocol.NodeStatus,
self._node_status_callback)
def __init__(self,
can_iface_name: str,
bitrate: int,
can_dump_directory: str,
file_server_lookup_paths: list,
self_node_id: int):
os.makedirs(can_dump_directory, exist_ok=True)
can_dump_base_name = 'can_%s.dump' % time.strftime('%Y-%m-%d-%H-%M-%S')
can_dump_file = os.path.join(can_dump_directory, can_dump_base_name)
self._can_dump_file = open(can_dump_file, 'w', encoding='utf8')
self._node = uavcan.make_node(can_iface_name,
bitrate=bitrate,
mode=NODE_STATUS_CONST.MODE_OPERATIONAL)
self._node.node_info.name = 'com.zubax.bootloader_tester'
self._node.node_id = self_node_id
self._node.can_driver.add_io_hook(self._can_io_hook)
self.file_server = uavcan.app.file_server.FileServer(self._node, file_server_lookup_paths)
self.monitor = uavcan.app.node_monitor.NodeMonitor(self._node)
self.node_id_allocator = uavcan.app.dynamic_node_id.CentralizedServer(self._node, self.monitor)
def logging_callback(event):
logger.info('uavcan.protocol.debug.LogMessage\n%s', uavcan.to_yaml(event))
self._node.add_handler(uavcan.protocol.debug.LogMessage, logging_callback)
def main():
args = parse_args()
if args.output:
output_file = csv.DictWriter(args.output,
['ts', 'anchor_addr', 'range'])
output_file.writeheader()
def range_cb(event):
msg = event.message
if args.anchor and msg.anchor_addr != args.anchor:
return
print("Received a range from {}: {:.3f}".format(
msg.anchor_addr, msg.range))
if args.output:
output_file.writerow({
'ts': msg.timestamp.usec,
'range': msg.range,
'anchor_addr': msg.anchor_addr
})
args.output.flush()
node = uavcan.make_node(args.port, node_id=127)
# TODO This path is a bit too hardcoded
dsdl_path = os.path.join(os.path.dirname(__file__), '..', '..',
'uavcan_data_types', 'cvra', 'uwb_beacon')
uavcan.load_dsdl(dsdl_path)
node.add_handler(uavcan.thirdparty.uwb_beacon.RadioRange, range_cb)
node.spin()
def createNode(com):
node = uavcan.make_node(com,
node_id=126,
bitrate=1000000,
baudrate=1000000)
node_monitor = uavcan.app.node_monitor.NodeMonitor(node)
# dynamic_node_id_allocator = uavcan.app.dynamic_node_id.CentralizedServer(node, node_monitor)
# while len(dynamic_node_id_allocator.get_allocation_table()) <= 1:
# print('Waiting for other nodes to become online...')
# node.spin(timeout=1)
while len(node_monitor.get_all_node_id()) < 1:
print('Waiting for other nodes to become online...')
node.spin(timeout=1)
all_node_ids = list(node_monitor.get_all_node_id())
print('\nDetected Node IDs', all_node_ids)
print(
'Node ID in use',
all_node_ids[0],
)
node_dict = node_monitor.get(
all_node_ids[0]
) #momentarily, always use the first, one shouldk use an ESC detection scheme as in the examples
return node
def main():
args = parse_args()
node = uavcan.make_node(args.port)
uavcan.load_dsdl(DSDL_DIR)
handle = node.add_handler(uavcan.thirdparty.cvra.sensor.ColorRGBC, color_cb)
node.spin()
def createNodeDynamicId(com):
node = uavcan.make_node(com, node_id=126, bitrate=1000000, baudrate=1000000)
node_monitor = uavcan.app.node_monitor.NodeMonitor(node)
dynamic_node_id_allocator = uavcan.app.dynamic_node_id.CentralizedServer(node, node_monitor)
waitForAllNodesDynamicId(dynamic_node_id_allocator,node)
return node_monitor, node, dynamic_node_id_allocator
def main(config_filename, *args):
node_ids = set() # we keep a cache of them so we can request node info
config = read_config(config_filename)
influxdb_client = influxdb.InfluxDBClient(
config.get('influxdb', 'host'),
config.getint('influxdb', 'port'),
config.get('influxdb', 'username'),
config.get('influxdb', 'password'),
config.get('influxdb', 'database'),
)
influxdb_client.create_database(config.get('influxdb', 'database'))
node_info = uavcan.protocol.GetNodeInfo.Response()
node_info.name = config.get('node', 'name')
node = uavcan.make_node(
config.get('canbus', 'ifname'),
node_id=config.getint('node', 'id'),
node_info=node_info,
)
uavcan_types = [
uavcan.protocol.NodeStatus,
uavcan.thirdparty.homeautomation.EventCount,
uavcan.thirdparty.homeautomation.Environment,
uavcan.thirdparty.homeautomation.ConductionSensor,
uavcan.thirdparty.homeautomation.PumpStatus,
]
for uavcan_type in uavcan_types:
node.add_handler(
uavcan_type,
record_event_data,
influxdb_client=influxdb_client,
)
node.add_handler(uavcan.protocol.NodeStatus,
lambda event: node_ids.add(event.transfer.source_node_id))
node.add_handler(uavcan.protocol.RestartNode, restart_request)
node.mode = uavcan.protocol.NodeStatus().MODE_OPERATIONAL
node.health = uavcan.protocol.NodeStatus().HEALTH_OK
print('started')
while True:
try:
node.spin(1)
get_node_infos(node, node_ids, influxdb_client)
receive_node_info(config.getint('node', 'id'), node_info,
influxdb_client)
except uavcan.UAVCANException as ex:
print('Node error:', ex)
def main():
logger.info('Starting the application')
app = QApplication(sys.argv)
while True:
# Asking the user to specify which interface to work with
try:
iface, iface_kwargs = run_iface_config_window(get_app_icon())
if not iface:
sys.exit(0)
except Exception as ex:
show_error('Fatal error',
'Could not list available interfaces',
ex,
blocking=True)
sys.exit(1)
# Trying to start the node on the specified interface
try:
node_info = uavcan.protocol.GetNodeInfo.Response()
node_info.name = NODE_NAME
node_info.software_version.major = __version__[0]
node_info.software_version.minor = __version__[1]
node = uavcan.make_node(
iface,
node_info=node_info,
mode=uavcan.protocol.NodeStatus().MODE_OPERATIONAL,
**iface_kwargs)
# Making sure the interface is alright
node.spin(0.1)
except Exception as ex:
logger.error('UAVCAN node init failed', exc_info=True)
show_error('Fatal error',
'Could not initialize UAVCAN node',
ex,
blocking=True)
else:
break
logger.info('Creating main window; iface %r', iface)
window = MainWindow(node, iface)
window.show()
try:
update_checker.begin_async_check(window)
except Exception:
logger.error('Could not start update checker', exc_info=True)
logger.info('Init complete, invoking the Qt event loop')
exit_code = app.exec_()
node.close()
sys.exit(exit_code)
def main():
args = parse_args()
node = uavcan.make_node(args.port)
uavcan.load_dsdl(DSDL_DIR)
handle = node.add_handler(uavcan.thirdparty.cvra.io.DigitalInput,
digital_input_cb)
node.spin()
def main():
args = parse_args()
node = uavcan.make_node(args.port)
uavcan.load_dsdl(DSDL_DIR)
handle = node.add_handler(uavcan.thirdparty.cvra.sensor.DistanceVL6180X,
distance_cb)
node.spin()
def main():
args = parse_args()
node = uavcan.make_node(args.port, node_id=42)
uavcan.load_dsdl(DSDL_DIR)
send_pwm(node, args.id, args.value)
# Spin node for 1 second
node.spin(1)
node.close()
def main():
args = parse_args()
node = uavcan.make_node(args.port, node_id=42)
uavcan.load_dsdl(DSDL_DIR)
set_servo(node, args.id,
servo_setpoint(args.servo, args.pos, args.vel, args.acc))
# Spin node for 1 second
node.spin(1)
node.close()
def main():
logger.info("Starting the application")
app = QApplication(sys.argv)
while True:
# Asking the user to specify which interface to work with
try:
iface, iface_kwargs = run_iface_config_window(get_app_icon())
if not iface:
sys.exit(0)
except Exception as ex:
show_error("Fatal error", "Could not list available interfaces", ex, blocking=True)
sys.exit(1)
# Trying to start the node on the specified interface
try:
node_info = uavcan.protocol.GetNodeInfo.Response()
node_info.name = NODE_NAME
node_info.software_version.major = __version__[0]
node_info.software_version.minor = __version__[1]
node = uavcan.make_node(
iface, node_info=node_info, mode=uavcan.protocol.NodeStatus().MODE_OPERATIONAL, **iface_kwargs
)
# Making sure the interface is alright
node.spin(0.1)
except uavcan.transport.TransferError:
# allow unrecognized messages on startup:
logger.warn("UAVCAN Transfer Error occured on startup", exc_info=True)
break
except Exception as ex:
logger.error("UAVCAN node init failed", exc_info=True)
show_error("Fatal error", "Could not initialize UAVCAN node", ex, blocking=True)
else:
break
logger.info("Creating main window; iface %r", iface)
window = MainWindow(node, iface)
window.show()
try:
update_checker.begin_async_check(window)
except Exception:
logger.error("Could not start update checker", exc_info=True)
logger.info("Init complete, invoking the Qt event loop")
exit_code = app.exec_()
node.close()
sys.exit(exit_code)
def createNode(com):
node = uavcan.make_node(com, node_id=126, bitrate=1000000, baudrate=1000000)
node_monitor = uavcan.app.node_monitor.NodeMonitor(node)
# dynamic_node_id_allocator = uavcan.app.dynamic_node_id.CentralizedServer(node, node_monitor)
# while len(dynamic_node_id_allocator.get_allocation_table()) <= 1:
# print('Waiting for other nodes to become online...')
# node.spin(timeout=1)
waitForAllNodes(node_monitor,node)
return node_monitor, node
def main():
args = parse_args()
uavcan.load_dsdl(args.dsdl)
node = uavcan.make_node(args.interface, node_id=42)
def do_publish():
msg = uavcan.thirdparty.cvra.uwb_beacon.DataPacket()
msg.dst_addr = 42
node.broadcast(msg, priority=uavcan.TRANSFER_PRIORITY_LOWEST)
handle = node.periodic(1, do_publish)
node.spin()
def main():
args = parse_args()
node = uavcan.make_node(args.port)
uavcan.load_dsdl(DSDL_DIR)
handle = node.add_handler(uavcan.thirdparty.cvra.proximity_beacon.Signal,
beacon_cb)
print("Listening for messages...")
# Spin node for 1 second
try:
node.spin()
except KeyboardInterrupt:
node.close()
def main(args):
if args.dsdl is not None:
uavcan.load_dsdl(args.dsdl)
node_info = uavcan.protocol.GetNodeInfo.Response(name=args.name)
node = uavcan.make_node(args.interface, node_id=args.id, node_info=node_info)
def publish(msg):
node.broadcast(msg, priority=uavcan.TRANSFER_PRIORITY_LOWEST)
def publish_current():
publish(
uavcan.thirdparty.cvra.motor.feedback.CurrentPID(
current_setpoint=step(scale=1, divider=0.5),
current=random.uniform(0, 1),
)
)
def publish_velocity():
publish(
uavcan.thirdparty.cvra.motor.feedback.VelocityPID(
velocity_setpoint=step(scale=1, divider=1),
velocity=random.uniform(0, 1),
)
)
def publish_position():
publish(
uavcan.thirdparty.cvra.motor.feedback.PositionPID(
position_setpoint=step(scale=1, divider=2),
position=random.uniform(0, 1),
)
)
if args.dsdl is not None:
handle_current = node.periodic(0.01, publish_current)
handle_velocity = node.periodic(0.03, publish_velocity)
handle_position = node.periodic(0.1, publish_position)
while True:
try:
node.spin(1)
except uavcan.UAVCANException as ex:
print("Node error:", ex)
def node_process(iface, iface_kwargs, on_transfer_callback):
logging.info('Starting the spinner process')
# Initializing the node
node = uavcan.make_node(iface, **iface_kwargs) # Passive mode
# Patching the node object to receive all messages from the bus
# TODO: Add required logic to the library
def call_handlers_replacement(transfer):
if not transfer.service_not_message:
on_transfer_callback(transfer)
node._handler_dispatcher.call_handlers = call_handlers_replacement
# Spinning the node
while True:
try:
node.spin()
except Exception:
logging.error('Node spin exception', exc_info=True)
def main():
app = QApplication(sys.argv)
# noinspection PyBroadException
try:
app_icon = QIcon(os.path.join(os.path.dirname(__file__), 'icon.png'))
except Exception:
logger.error('Could not load icon', exc_info=True)
app_icon = QIcon()
while True:
# Asking the user to specify which interface to work with
try:
iface, iface_kwargs = run_iface_config_window(app_icon)
if not iface:
exit(0)
except Exception as ex:
show_error('Fatal error', 'Could not list available interfaces',
ex)
exit(1)
# Trying to start the node on the specified interface
try:
node_info = uavcan.protocol.GetNodeInfo.Response()
node_info.name = NODE_NAME
node_info.software_version.major = 1 # TODO: share with setup.py
node_info.software_version.minor = 0
node = uavcan.make_node(
iface,
node_info=node_info,
mode=uavcan.protocol.NodeStatus().MODE_OPERATIONAL,
**iface_kwargs)
except Exception as ex:
show_error('Fatal error', 'Could not initialize UAVCAN node', ex)
else:
break
window = MainWindow(app_icon, node, iface)
window.show()
exit(app.exec_())
def node_process(iface, iface_kwargs, on_transfer_callback):
logging.info('Starting the spinner process')
# Initializing the node
node = uavcan.make_node(iface, **iface_kwargs) # Passive mode
# Patching the node object to receive all messages from the bus
# TODO: Add required logic to the library
def call_handlers_replacement(transfer):
if not transfer.service_not_message:
on_transfer_callback(transfer)
node._handler_dispatcher.call_handlers = call_handlers_replacement
# Spinning the node
while True:
try:
node.spin()
except Exception:
logging.error('Node spin exception', exc_info=True)
def main():
args = parse_args()
node = uavcan.make_node(args.port, node_id=123)
node.lock = threading.RLock()
# TODO This path is a bit too hardcoded
dsdl_path = os.path.join(os.path.dirname(__file__), '..', 'beacon_messages')
uavcan.load_dsdl(dsdl_path)
samples = []
def range_cb(event):
samples.append(event.message.range)
print(event.message.range)
if len(samples) >= 100:
bias = mean(samples) - args.distance
print("Estimated bias: {:.3f}".format(bias))
print("Delta to add in decawave units: {}".format(int((bias / 2) / SPEED_OF_LIGHT)))
node.add_handler(uavcan.thirdparty.beacon_messages.equipment.RadioRange, range_cb)
node.spin()
def main():
args = parse_args()
node = uavcan.make_node(args.port, node_id=123)
node.lock = threading.RLock()
uavcan.load_dsdl(args.dsdl)
samples = []
def range_cb(event):
samples.append(event.message.range)
print(event.message.range)
if len(samples) >= 100:
bias = mean(samples) - args.distance
print("Estimated bias: {:.3f}".format(bias))
print("Delta to add in decawave units: {}".format(
int((bias / 2) / SPEED_OF_LIGHT)))
node.add_handler(uavcan.thirdparty.cvra.uwb_beacon.RadioRange, range_cb)
node.spin()
def main():
app = QApplication(sys.argv)
# noinspection PyBroadException
try:
app_icon = QIcon(os.path.join(os.path.dirname(__file__), 'icon.png'))
except Exception:
logger.error('Could not load icon', exc_info=True)
app_icon = QIcon()
while True:
# Asking the user to specify which interface to work with
try:
iface, iface_kwargs = run_iface_config_window(app_icon)
if not iface:
exit(0)
except Exception as ex:
show_error('Fatal error', 'Could not list available interfaces', ex)
exit(1)
# Trying to start the node on the specified interface
try:
node_info = uavcan.protocol.GetNodeInfo.Response()
node_info.name = NODE_NAME
node_info.software_version.major = 1 # TODO: share with setup.py
node_info.software_version.minor = 0
node = uavcan.make_node(iface,
node_info=node_info,
mode=uavcan.protocol.NodeStatus().MODE_OPERATIONAL,
**iface_kwargs)
except Exception as ex:
show_error('Fatal error', 'Could not initialize UAVCAN node', ex)
else:
break
window = MainWindow(app_icon, node, iface)
window.show()
exit(app.exec_())
def run_uavcan(node_infos, request_queue):
if getattr(uavcan.thirdparty, 'homeautomation', None) is None:
uavcan.load_dsdl(
os.path.join(os.path.dirname(__file__), 'dsdl_files',
'homeautomation'))
node_info = uavcan.protocol.GetNodeInfo.Response()
node_info.name = os.environ.get('NODE__NAME', config.get('node', 'name'))
node = uavcan.make_node(
os.environ.get('CANBUS__IFNAME', config.get('canbus', 'ifname')),
node_id=int(os.environ.get('NODE__ID', config.getint('node', 'id'))),
node_info=node_info,
)
node.mode = uavcan.protocol.NodeStatus().MODE_OPERATIONAL
node.health = uavcan.protocol.NodeStatus().HEALTH_OK
def node_status_cb(event):
node_infos[event.transfer.source_node_id]['last_seen'] = datetime.now()
node.add_handler(uavcan.protocol.NodeStatus, node_status_cb)
while running:
try:
node.spin(0.2)
while not request_queue.empty():
request = request_queue.get_nowait()
response_queue = request['response_queue']
node.request(
request['request'],
request['node_id'],
lambda event: response_queue.put(event),
)
request_queue.task_done()
except uavcan.UAVCANException as ex:
print('Node error:', ex)
def reconnect(self):
while True:
zubax = set(find_serial('zubax').keys())
if len(zubax) != 1:
logger.error(f"Zubax controller not determined, {zubax}")
sleep(1)
continue
serial_device = next(iter(zubax), None)
print(serial_device)
logger.info(f"Zubax controller determined, {zubax}")
try:
self.node: Optional[uavcan.node.Node] = uavcan.make_node(
serial_device,
node_id=10,
bitrate=1000000,
)
# setup
node_monitor = uavcan.app.node_monitor.NodeMonitor(self.node)
dynamic_node_id_allocator = uavcan.app.dynamic_node_id.CentralizedServer(
self.node, node_monitor)
# Waiting for at least one other node to appear online (our local node is already online).
while len(
dynamic_node_id_allocator.get_allocation_table()) <= 1:
logger.error('Waiting for other nodes to become online...')
self.node.spin(timeout=1)
# how fast can we blast this?
self.node.periodic(0.05, self.update)
self.node.add_handler(uavcan.equipment.esc.Status, self.listen)
logger.info('New zubax node: %s', serial_device)
return
except Exception as e:
logger.error("Opening zubax failed %s", e)
sleep(1)
continue
def run(self):
self.node = uavcan.make_node(self.port, node_id=127)
self.node.add_handler(uavcan.protocol.NodeStatus,
self._node_status_callback)
self.node.add_handler(uavcan.thirdparty.cvra.motor.feedback.CurrentPID,
self._current_pid_callback)
self.node.add_handler(
uavcan.thirdparty.cvra.motor.feedback.VelocityPID,
self._velocity_pid_callback)
self.node.add_handler(
uavcan.thirdparty.cvra.motor.feedback.PositionPID,
self._position_pid_callback)
self.node.periodic(0.1, self._publish_setpoint)
while True:
with self.lock:
try:
self.node.spin(0.2)
except uavcan.transport.TransferError:
pass
def main():
# Init ROS node
rospy.init_node(canros.ros_node_name)
# Get can_interface parameter
try:
can_interface = rospy.get_param('~can_interface')
except KeyError:
print("'can_interface' ROS parameter must be set")
return
# Get uavcan_node_id parameter
try:
uavcan_node_id = int(rospy.get_param('~uavcan_id'))
if uavcan_node_id < 0 or uavcan_node_id > 127:
raise ValueError()
except KeyError:
print("'uavcan_id' ROS parameter must be set")
return
except ValueError:
print("'uavcan_id' must be an integer from 0-127")
return
try:
blacklist = list(rospy.get_param('~blacklist'))
except KeyError:
print("'blacklist' ROS parameter must be set")
except ValueError:
print("'blacklist' must be a list or strings")
try:
ignore_uavcan_msgs = rospy.get_param('~ignore_uavcan_msgs')
except KeyError:
print("'ignore_uavcan_msgs' ROS parameter must be set")
# Init UAVCAN logging
uavcan.driver.slcan.logger.addHandler(logging.StreamHandler())
uavcan.driver.slcan.logger.setLevel('DEBUG')
# Set UAVCAN node information
uavcan_node_info = uavcan.protocol.GetNodeInfo.Response()
uavcan_node_info.name = canros.uavcan_name
uavcan_node_info.software_version.major = 0
uavcan_node_info.software_version.minor = 1
uavcan_node_info.hardware_version.unique_id = hardware_id()
# Start UAVCAN node
global uavcan_node #pylint: disable=W0603
uavcan_node = uavcan.make_node(can_interface,
node_id=uavcan_node_id,
node_info=uavcan_node_info)
# Load types
for uavcan_name, typ in uavcan.TYPENAMES.iteritems():
if typ.default_dtid is None:
continue
if uavcan_name in blacklist:
continue
if ignore_uavcan_msgs and uavcan_name.startswith('uavcan'):
continue
_ = Message(typ) if typ.kind == typ.KIND_MESSAGE else Service(typ)
# GetInfo
def GetInfoHandler(_):
rosmsg = canros.srv.GetNodeInfoResponse()
rosmsg.node_info = canros.copy_uavcan_ros(rosmsg.node_info,
uavcan_node.node_info,
request=False)
setattr(rosmsg.node_info.status, canros.uavcan_id_field_name,
uavcan_node.node_id)
return rosmsg
rospy.Service(canros.get_info_topic, canros.srv.GetNodeInfo,
GetInfoHandler)
# Spin
uavcan_errors = 0
while not rospy.is_shutdown():
try:
uavcan_node.spin(0)
if uavcan_errors > 0:
uavcan_errors = 0
except uavcan.transport.TransferError:
uavcan_errors += 1
if uavcan_errors >= 1000:
print("Too many UAVCAN transport errors")
break
uavcan_node.close()
print("canros server exited successfully")
def test_uavcan():
node_info = uavcan.protocol.GetNodeInfo.Response()
node_info.name = 'com.zubax.drwatson.sapog'
iface = init_can_iface()
with closing(uavcan.make_node(iface,
bitrate=CAN_BITRATE,
node_id=127,
mode=uavcan.protocol.NodeStatus().MODE_OPERATIONAL,
node_info=node_info)) as n:
def safe_spin(timeout):
try:
n.spin(timeout)
except uavcan.UAVCANException:
logger.error('Node spin failure', exc_info=True)
@contextmanager
def time_limit(timeout, error_fmt, *args):
aborter = n.defer(timeout, partial(abort, error_fmt, *args))
yield
aborter.remove()
try:
# Dynamic node ID allocation
nsmon = uavcan.app.node_monitor.NodeMonitor(n)
alloc = uavcan.app.dynamic_node_id.CentralizedServer(n, nsmon)
info('Waiting for the node to show up on the CAN bus...')
with time_limit(10, 'The node did not show up in time. Check CAN interface and crystal oscillator.'):
while True:
safe_spin(1)
target_nodes = list(nsmon.find_all(lambda e: e.info and e.info.name.decode() == PRODUCT_NAME))
if len(target_nodes) == 1:
break
if len(target_nodes) > 1:
abort('Expected to find exactly one target node, found more: %r', target_nodes)
node_id = target_nodes[0].node_id
info('Node %r initialized', node_id)
for nd in target_nodes:
logger.info('Discovered node %r', nd)
def request(what):
response_event = None
def cb(e):
nonlocal response_event
if not e:
abort('Request has timed out: %r', what)
response_event = e
n.request(what, node_id, cb)
while response_event is None:
safe_spin(0.1)
return response_event.response
# Starting the node and checking its self-reported diag outputs
def wait_for_init():
with time_limit(10, 'The node did not complete initialization in time'):
while True:
safe_spin(1)
if nsmon.exists(node_id) and nsmon.get(node_id).status.mode == \
uavcan.protocol.NodeStatus().MODE_OPERATIONAL:
break
def check_status():
status = nsmon.get(node_id).status
enforce(status.mode == uavcan.protocol.NodeStatus().MODE_OPERATIONAL,
'Unexpected operating mode')
enforce(status.health == uavcan.protocol.NodeStatus().HEALTH_OK,
'Bad node health')
info('Waiting for the node to complete initialization...')
wait_for_init()
check_status()
info('Resetting the configuration to factory defaults...')
enforce(request(uavcan.protocol.param.ExecuteOpcode.Request(
opcode=uavcan.protocol.param.ExecuteOpcode.Request().OPCODE_ERASE)).ok,
'The node refused to reset configuration to factory defaults')
col_esc_status = uavcan.app.message_collector.MessageCollector(n, uavcan.equipment.esc.Status, timeout=10)
def check_everything(check_rotation=False):
check_status()
try:
m = col_esc_status[node_id].message
except KeyError:
abort('Rock is dead.')
else:
if check_rotation:
enforce(m.rpm > 100 and m.power_rating_pct > 0, 'Could not start the motor')
enforce(m.current > 0, 'Current is not positive')
enforce(m.error_count < ESC_ERROR_LIMIT, 'High error count: %r', m.error_count)
temp_degc = convert_units_from_to(m.temperature, 'Kelvin', 'Celsius')
enforce(TEMPERATURE_RANGE_DEGC[0] <= temp_degc <= TEMPERATURE_RANGE_DEGC[1],
'Invalid temperature: %r degC', temp_degc)
# Testing before the motor is started
imperative('CAUTION: THE MOTOR WILL START IN 2 SECONDS, KEEP CLEAR')
safe_spin(2)
check_everything()
# Starting the motor
esc_raw_command_bitlen = \
uavcan.get_uavcan_data_type(uavcan.get_fields(uavcan.equipment.esc.RawCommand())['cmd'])\
.value_type.bitlen # SO EASY TO USE
def do_publish(duty_cycle, check_rotation):
command_value = int(duty_cycle * (2 ** (esc_raw_command_bitlen - 1)))
n.broadcast(uavcan.equipment.esc.RawCommand(cmd=[command_value]))
check_everything(check_rotation)
info('Starting the motor')
publisher = n.periodic(0.01, partial(do_publish, STARTUP_DUTY_CYCLE, False))
safe_spin(5)
publisher.remove()
info('Checking stability...')
for dc in STABILITY_TEST_DUTY_CYCLES:
info('Setting duty cycle %d%%...', int(dc * 100))
publisher = n.periodic(0.01, partial(do_publish, dc, True))
safe_spin(5)
publisher.remove()
info('Stopping...')
latest_status = col_esc_status[node_id].message
safe_spin(1)
check_everything()
# Final results
info('Validate the latest ESC status variables (units are SI):\n%s', uavcan.to_yaml(latest_status))
# Testing CAN2
with BackgroundSpinner(safe_spin, 0.1):
input('1. Disconnect CAN1 and connect to CAN2\n'
'2. Terminate CAN2\n'
'3. Press ENTER')
safe_spin(1)
try:
check_status()
except Exception as ex:
logger.info('CAN2 test failed', exc_info=True)
abort('CAN2 test failed [%r]', ex)
# Testing LED
info('Testing LED')
def set_led():
rgb = uavcan.equipment.indication.RGB565(red=0b11111, green=0b111111, blue=0b11111)
slc = uavcan.equipment.indication.SingleLightCommand(light_id=0, color=rgb)
n.broadcast(uavcan.equipment.indication.LightsCommand(commands=[slc]))
check_everything()
publisher = n.periodic(0.1, set_led)
with BackgroundSpinner(safe_spin, 0.1):
if not input('Is the LED glowing bright white?', yes_no=True, default_answer=True):
abort('LED is not working properly')
publisher.remove()
except Exception:
for nid in nsmon.get_all_node_id():
logger.info('UAVCAN test failed; last known state of the device node: %r' % nsmon.get(nid))
raise
def test_uavcan():
node_info = uavcan.protocol.GetNodeInfo.Response() # @UndefinedVariable
node_info.name.encode("com.zubax.drwatson.zubax_gnss")
iface = init_can_iface()
with closing(
uavcan.make_node(iface, bitrate=CAN_BITRATE, node_id=127, mode=uavcan.protocol.NodeStatus().MODE_OPERATIONAL)
) as n: # @UndefinedVariable
def safe_spin(timeout):
try:
n.spin(timeout)
except uavcan.transport.TransferError:
logger.debug(
"Transfer error while spinning the node. "
"Reporting at the DEBUG level because of https://github.com/UAVCAN/pyuavcan/issues/14",
exc_info=True,
)
except uavcan.UAVCANException:
logger.error("Node spin failure", exc_info=True)
@contextmanager
def time_limit(timeout, error_fmt, *args):
aborter = n.defer(timeout, partial(abort, error_fmt, *args))
yield
aborter.remove()
# Dynamic node ID allocation
try:
nsmon = uavcan.monitors.NodeStatusMonitor(n)
alloc = uavcan.monitors.DynamicNodeIDServer(n, nsmon) # @UnusedVariable
with time_limit(10, "The node did not show up in time. Check CAN interface and crystal oscillator."):
while True:
safe_spin(1)
target_nodes = list(nsmon.find_all(lambda e: e.info and e.info.name.decode() == PRODUCT_NAME))
if len(target_nodes) == 1:
break
if len(target_nodes) > 1:
abort("Expected to find exactly one target node, found more: %r", target_nodes)
node_id = target_nodes[0].node_id
info("Node %r initialized", node_id)
for nd in target_nodes:
logger.info("Discovered node %r", nd)
def request(what, fire_and_forget=False):
response_event = None
def cb(e):
nonlocal response_event
if not e:
abort("Request has timed out: %r", what)
response_event = e # @UnusedVariable
if fire_and_forget:
n.request(what, node_id, lambda _: None)
safe_spin(0.1)
else:
n.request(what, node_id, cb)
while response_event is None:
safe_spin(0.1)
return response_event.response
# Starting the node and checking its self-reported diag outputs
def wait_for_init():
with time_limit(10, "The node did not complete initialization in time"):
while True:
safe_spin(1)
if (
nsmon.exists(node_id)
and nsmon.get(node_id).status.mode == uavcan.protocol.NodeStatus().MODE_OPERATIONAL
): # @UndefinedVariable
break
def check_status():
status = nsmon.get(node_id).status
enforce(
status.mode == uavcan.protocol.NodeStatus().MODE_OPERATIONAL, # @UndefinedVariable
"Unexpected operating mode",
)
enforce(
status.health == uavcan.protocol.NodeStatus().HEALTH_OK, "Bad node health" # @UndefinedVariable
)
info("Waiting for the node to complete initialization...")
wait_for_init()
check_status()
info("Reconfiguring the node...")
def log_all_params():
for index in range(10000):
r = request(uavcan.protocol.param.GetSet.Request(index=index)) # @UndefinedVariable
if not r.name:
break
logger.info("Param %-30r %r" % (r.name.decode(), getattr(r.value, r.value.union_field)))
def set_param(name, value, union_field=None):
union_field = (
union_field
or {int: "integer_value", float: "real_value", bool: "boolean_value", str: "string_value"}[
type(value)
]
)
logger.info("Setting parameter %r field %r value %r", name, union_field, value)
req = uavcan.protocol.param.GetSet.Request() # @UndefinedVariable
req.name.encode(name)
setattr(req.value, union_field, value)
r = request(req)
enforce(r.value.union_field == union_field, "Union field mismatch in param set response for %r", name)
enforce(getattr(r.value, union_field) == value, "The node refused to set parameter %r", name)
set_param("uavcan.pubp-time", 10000)
set_param("uavcan.pubp-stat", 2000)
set_param("uavcan.pubp-pres", 10000)
set_param("uavcan.pubp-mag", 20000)
set_param("uavcan.pubp-fix", 66666)
set_param("uavcan.pubp-aux", 100000)
enforce(
request(
uavcan.protocol.param.ExecuteOpcode.Request( # @UndefinedVariable
opcode=uavcan.protocol.param.ExecuteOpcode.Request().OPCODE_SAVE
)
).ok, # @UndefinedVariable
"Could not save configuration",
)
request(
uavcan.protocol.RestartNode.Request( # @UndefinedVariable
magic_number=uavcan.protocol.RestartNode.Request().MAGIC_NUMBER
), # @UndefinedVariable
fire_and_forget=True,
)
# Don't tell anybody I wrote this. I'm ashamed of this shit and too tired to redesign it. :(
with BackgroundSpinner(safe_spin, 0.1):
wait_for_boot()
wait_for_init()
check_status()
log_all_params()
def make_collector(data_type, timeout=0.1):
return uavcan.monitors.MessageCollector(n, data_type, timeout=timeout)
col_fix = make_collector(uavcan.equipment.gnss.Fix, 0.2) # @UndefinedVariable
col_aux = make_collector(uavcan.equipment.gnss.Auxiliary, 0.2) # @UndefinedVariable
col_mag = make_collector(uavcan.equipment.ahrs.MagneticFieldStrength) # @UndefinedVariable
col_pressure = make_collector(uavcan.equipment.air_data.StaticPressure) # @UndefinedVariable
col_temp = make_collector(uavcan.equipment.air_data.StaticTemperature) # @UndefinedVariable
def check_everything():
check_status()
try:
m = col_pressure[node_id].message
except KeyError:
abort("Pressure measurements are not available. Check the sensor.")
else:
if not 50000 < m.static_pressure < 150000:
abort("Invalid pressure reading: %d Pascal. Check the sensor.", m.static_pressure)
try:
m = col_temp[node_id].message
except KeyError:
abort("Temperature measurements are not available. Check the sensor.")
else:
if not 10 < (m.static_temperature - 273.15) < 50:
abort("Invalid temperature reading: %d Kelvin. Check the sensor.", m.static_temperature)
try:
m = col_mag[node_id].message
except KeyError:
abort("Magnetic field measurements are not available. Check the sensor.")
else:
magnetic_field_scalar = numpy.linalg.norm(m.magnetic_field_ga) # @UndefinedVariable
if not 0.01 < magnetic_field_scalar < 2:
abort(
"Invalid magnetic field strength reading: %d Gauss. Check the sensor.",
magnetic_field_scalar,
)
imperative(
"Testing GNSS performance. Place the device close to a window to ensure decent GNSS reception. "
"Please note that this test is very crude, it can only detect whether GNSS circuit is working "
"at all or not. If GNSS performance is degraded due to improper manufacturing process, "
"this test may fail to detect it, so please double check that your manufacturing process "
"adheres to the documentation."
)
info("Waiting for GNSS fix...")
with time_limit(GNSS_FIX_TIMEOUT, "GNSS fix timeout. Check the RF circuit, AFE, antenna, and receiver"):
while True:
safe_spin(1)
check_everything()
sats_visible = col_aux[node_id].message.sats_visible
sats_used = col_aux[node_id].message.sats_used
sys.stdout.write("\rsat stats: visible %d, used %d \r" % (sats_visible, sats_used))
sys.stdout.flush()
if col_fix[node_id].message.status >= 3:
break
info("Waiting for %d satellites...", GNSS_MIN_SAT_NUM)
with time_limit(
GNSS_MIN_SAT_TIMEOUT,
"GNSS performance is degraded. Could be caused by incorrectly assembled RF circuit.",
):
while True:
safe_spin(0.5)
check_everything()
num = col_fix[node_id].message.sats_used
pos_cov = list(col_fix[node_id].message.position_covariance)
sys.stdout.write("\r%d sats, pos covariance: %r \r" % (num, pos_cov))
sys.stdout.flush()
if num >= GNSS_MIN_SAT_NUM:
break
check_everything()
info("Last sampled sensor measurements are provided below. They appear to be correct.")
info("GNSS fix: %r", col_fix[node_id].message)
info("GNSS aux: %r", col_aux[node_id].message)
info("Magnetic field [Ga]: %r", col_mag[node_id].message)
info("Pressure [Pa]: %r", col_pressure[node_id].message)
info("Temperature [K]: %r", col_temp[node_id].message)
# Finalizing the test
info("Resetting the configuration to factory default...")
enforce(
request(
uavcan.protocol.param.ExecuteOpcode.Request( # @UndefinedVariable
opcode=uavcan.protocol.param.ExecuteOpcode.Request().OPCODE_ERASE
)
).ok, # @UndefinedVariable
"Could not erase configuration",
)
request(
uavcan.protocol.RestartNode.Request( # @UndefinedVariable
magic_number=uavcan.protocol.RestartNode.Request().MAGIC_NUMBER
), # @UndefinedVariable
fire_and_forget=True,
)
with BackgroundSpinner(safe_spin, 0.1):
wait_for_boot()
wait_for_init()
check_status()
log_all_params()
except Exception:
for nid in nsmon.get_all_node_id():
print("Node state: %r" % nsmon.get(nid))
raise
# Blocking questions are moved out of the node scope because blocking breaks CAN communications
if not input("Is the PPS LED blinking once per second?", yes_no=True, default_answer=True):
abort("PPS LED is not working")
if not input("Is the CAN1 LED blinking or glowing solid?", yes_no=True, default_answer=True):
abort("CAN1 LED is not working (however the interface is fine)")
if not input("Is the STATUS LED blinking once per second?", yes_no=True, default_answer=True):
abort("STATUS LED is not working")
# Testing CAN2
input("1. Disconnect CAN1 and connect to CAN2\n" "2. Terminate CAN2\n" "3. Press ENTER")
if not input("Is the CAN2 LED blinking or glowing solid?", yes_no=True, default_answer=True):
abort("Either CAN2 or its LED are not working")
))
print(value_to_constant_name(
uavcan.protocol.file.Error(value=uavcan.protocol.file.Error().ACCESS_DENIED),
'value'
))
print(value_to_constant_name(
uavcan.equipment.power.BatteryInfo(status_flags=
uavcan.equipment.power.BatteryInfo().STATUS_FLAG_NEED_SERVICE),
'status_flags'
))
print(value_to_constant_name(
uavcan.equipment.power.BatteryInfo(status_flags=
uavcan.equipment.power.BatteryInfo().STATUS_FLAG_NEED_SERVICE |
uavcan.equipment.power.BatteryInfo().STATUS_FLAG_TEMP_HOT |
uavcan.equipment.power.BatteryInfo().STATUS_FLAG_CHARGED),
'status_flags'
))
print(value_to_constant_name(
uavcan.protocol.AccessCommandShell.Response(flags=
uavcan.protocol.AccessCommandShell.Response().FLAG_SHELL_ERROR |
uavcan.protocol.AccessCommandShell.Response().
FLAG_HAS_PENDING_STDOUT),
'flags'
))
# Printing transfers
node = uavcan.make_node('vcan0', node_id=42)
node.request(uavcan.protocol.GetNodeInfo.Request(), 100, lambda e: print(to_yaml(e)))
node.add_handler(uavcan.protocol.NodeStatus, lambda e: print(to_yaml(e)))
node.spin()
def test_uavcan():
node_info = uavcan.protocol.GetNodeInfo.Response()
node_info.name = 'com.zubax.drwatson.zubax_gnss'
iface = init_can_iface()
with closing(uavcan.make_node(iface,
bitrate=CAN_BITRATE,
node_id=127,
mode=uavcan.protocol.NodeStatus().MODE_OPERATIONAL,
node_info=node_info)) as n:
def safe_spin(timeout):
try:
n.spin(timeout)
except uavcan.transport.TransferError:
logger.debug('Transfer error while spinning the node. '
'Reporting at the DEBUG level because of https://github.com/UAVCAN/pyuavcan/issues/14',
exc_info=True)
except uavcan.UAVCANException:
logger.error('Node spin failure', exc_info=True)
@contextmanager
def time_limit(timeout, error_fmt, *args):
aborter = n.defer(timeout, partial(abort, error_fmt, *args))
yield
aborter.remove()
# Dynamic node ID allocation
try:
nsmon = uavcan.app.node_monitor.NodeMonitor(n)
alloc = uavcan.app.dynamic_node_id.CentralizedServer(n, nsmon)
with time_limit(10,
'The node did not show up in time. Check the CAN interface and the crystal oscillator.'):
while True:
safe_spin(1)
target_nodes = list(nsmon.find_all(lambda e: e.info and e.info.name.decode() == PRODUCT_NAME))
if len(target_nodes) == 1:
break
if len(target_nodes) > 1:
abort('Expected to find exactly one target node, found more: %r', target_nodes)
node_id = target_nodes[0].node_id
info('Node %r initialized', node_id)
for nd in target_nodes:
logger.info('Discovered node %r', nd)
def request(what, fire_and_forget=False, timeout=2):
response_event = None
def cb(e):
nonlocal response_event
if not e:
abort('Request has timed out: %r', what)
response_event = e
if fire_and_forget:
n.request(what, node_id, lambda _: None, timeout=timeout)
safe_spin(0.1)
else:
n.request(what, node_id, cb, timeout=timeout)
while response_event is None:
safe_spin(0.1)
return response_event.response
# Starting the node and checking its self-reported diag outputs
def wait_for_init():
with time_limit(12, 'The node did not complete initialization in time'):
while True:
safe_spin(1)
if nsmon.exists(node_id) and nsmon.get(node_id).status.mode == \
uavcan.protocol.NodeStatus().MODE_OPERATIONAL:
break
def check_status():
status = nsmon.get(node_id).status
enforce(status.mode == uavcan.protocol.NodeStatus().MODE_OPERATIONAL,
'Unexpected operating mode')
enforce(status.health == uavcan.protocol.NodeStatus().HEALTH_OK,
'Bad node health')
info('Waiting for the node to complete initialization...')
wait_for_init()
check_status()
info('Reconfiguring the node...')
def log_all_params():
for index in range(10000):
r = request(uavcan.protocol.param.GetSet.Request(index=index))
if not r.name:
break
logger.info('Param %-30r %r' % (r.name.decode(),
getattr(r.value, uavcan.get_active_union_field(r.value))))
def set_param(name, value, union_field=None):
union_field = union_field or {
int: 'integer_value',
float: 'real_value',
bool: 'boolean_value',
str: 'string_value'
}[type(value)]
logger.info('Setting parameter %r field %r value %r', name, union_field, value)
req = uavcan.protocol.param.GetSet.Request()
req.name.encode(name)
setattr(req.value, union_field, value)
r = request(req)
enforce(uavcan.get_active_union_field(r.value) == union_field,
'Union field mismatch in param set response for %r', name)
enforce(getattr(r.value, union_field) == value,
'The node refused to set parameter %r', name)
set_param('uavcan.pubp-time', 10000)
set_param('uavcan.pubp-stat', 2000)
set_param('uavcan.pubp-pres', 10000)
set_param('uavcan.pubp-mag', 20000)
set_param('uavcan.pubp-fix', 66666)
set_param('uavcan.pubp-aux', 100000)
enforce(request(uavcan.protocol.param.ExecuteOpcode.Request(
opcode=uavcan.protocol.param.ExecuteOpcode.Request().OPCODE_SAVE)).ok,
'Could not save configuration')
request(uavcan.protocol.RestartNode.Request(
magic_number=uavcan.protocol.RestartNode.Request().MAGIC_NUMBER),
fire_and_forget=True)
# Don't tell anybody I wrote this. I'm ashamed of this shit and too tired to redesign it. :(
with BackgroundSpinner(safe_spin, 0.1):
wait_for_boot()
wait_for_init()
check_status()
log_all_params()
def make_collector(data_type, timeout=0.5):
return uavcan.app.message_collector.MessageCollector(n, data_type, timeout=timeout)
col_fix = make_collector(uavcan.equipment.gnss.Fix2)
col_aux = make_collector(uavcan.equipment.gnss.Auxiliary)
col_mag = make_collector(uavcan.equipment.ahrs.MagneticFieldStrength)
col_pressure = make_collector(uavcan.equipment.air_data.StaticPressure)
col_temp = make_collector(uavcan.equipment.air_data.StaticTemperature)
def check_everything():
check_status()
try:
m = col_pressure[node_id].message
except KeyError:
abort('Pressure measurements are not available. Check the sensor.')
else:
if not 50000 < m.static_pressure < 150000:
abort('Invalid pressure reading: %d Pascal. Check the sensor.', m.static_pressure)
try:
m = col_temp[node_id].message
except KeyError:
abort('Temperature measurements are not available. Check the sensor.')
else:
if not 10 < (m.static_temperature - 273.15) < 50:
abort('Invalid temperature reading: %d Kelvin. Check the sensor.', m.static_temperature)
try:
m = col_mag[node_id].message
except KeyError:
abort('Magnetic field measurements are not available. Check the sensor.')
else:
magnetic_field_scalar = numpy.linalg.norm(m.magnetic_field_ga)
if not 0.01 < magnetic_field_scalar < 2:
abort('Invalid magnetic field strength reading: %d Gauss. Check the sensor.',
magnetic_field_scalar)
imperative('Testing GNSS performance.')
info('Waiting for GNSS fix...')
with time_limit(GNSS_FIX_TIMEOUT, 'GNSS fix timeout. Check the RF circuit, AFE, antenna, and receiver'):
while True:
safe_spin(1)
check_everything()
sats_visible = col_aux[node_id].message.sats_visible
sats_used = col_aux[node_id].message.sats_used
sys.stdout.write('\rsat stats: visible %d, used %d \r' % (sats_visible, sats_used))
sys.stdout.flush()
if col_fix[node_id].message.status >= 3:
break
info('Waiting for %d satellites...', GNSS_MIN_SAT_NUM)
with time_limit(GNSS_MIN_SAT_TIMEOUT,
'GNSS performance is degraded. Could be caused by incorrectly assembled RF circuit.'):
while True:
safe_spin(0.5)
check_everything()
num = col_fix[node_id].message.sats_used
cov = list(col_fix[node_id].message.covariance)
sys.stdout.write('\r%d sats, covariance: %r \r' % (num, cov))
sys.stdout.flush()
if num >= GNSS_MIN_SAT_NUM:
break
check_everything()
info('The last sampled sensor measurements are provided below. They appear to be correct.')
info('GNSS fix: %r', col_fix[node_id].message)
info('GNSS aux: %r', col_aux[node_id].message)
info('Magnetic field [Ga]: %r', col_mag[node_id].message)
info('Pressure [Pa]: %r', col_pressure[node_id].message)
info('Temperature [K]: %r', col_temp[node_id].message)
# Finalizing the test
info('Resetting the configuration to the factory defaults...')
enforce(request(uavcan.protocol.param.ExecuteOpcode.Request(
opcode=uavcan.protocol.param.ExecuteOpcode.Request().OPCODE_ERASE)).ok,
'Could not erase configuration')
request(uavcan.protocol.RestartNode.Request(
magic_number=uavcan.protocol.RestartNode.Request().MAGIC_NUMBER),
fire_and_forget=True)
with BackgroundSpinner(safe_spin, 0.1):
wait_for_boot()
wait_for_init()
check_status()
log_all_params()
except Exception:
for nid in nsmon.get_all_node_id():
print('Node state: %r' % nsmon.get(nid))
raise
# Blocking questions are moved out of the node scope because blocking breaks CAN communications
# Note that we must instantiate the driver in order to ensure proper traffic LED behavior
# Periodic calls to receive() are needed to avoid RX queue overflow
with closing(uavcan.make_driver(iface, bitrate=CAN_BITRATE)) as d:
with BackgroundSpinner(lambda: d.receive(0.01)):
if not input('Is the PPS LED blinking once per second?', yes_no=True, default_answer=True):
abort('PPS LED is not working')
if not input('Is the CAN1 LED blinking or glowing solid?', yes_no=True, default_answer=True):
abort('CAN1 LED is not working (however the interface is fine)')
if not input('Is the STATUS LED blinking once per second?', yes_no=True, default_answer=True):
abort('STATUS LED is not working')
# Testing CAN2
input('1. Disconnect the cable from CAN1 and connect it to CAN2\n'
'2. Terminate CAN2\n'
'3. Press ENTER')
if not input('Is the CAN2 LED blinking or glowing solid?', yes_no=True, default_answer=True):
abort('Either CAN2 or its LED are not working')
