metavar="NODE_ID") cmd_group.add_option("-s", "--bus-speed", dest="bus_speed", default=1000000, help="set CAN bus speed", metavar="NODE_ID") cmd_group.add_option("-l", "--log", dest="log_path", default=None, help="log UAVCAN messages to PATH", metavar="PATH") cmd_group = OptionGroup(parser, "UAVCAN options") cmd_group.add_option("--dsdl", dest="dsdl_paths", action="append", metavar="PATH", help="load DSDL files from PATH") cmd_group.add_option("--firmware", dest="firmware", metavar="PATH", help="use firmware images in PATH to update nodes") options, args = parser.parse_args() uavcan.load_dsdl("../dsdl/thiemar", *(options.dsdl_paths or [])) ioloop = tornado.ioloop.IOLoop.instance() if options.firmware: firmware_dir = options.firmware else: firmware_dir = None if options.log_path: JSON_LOG_FILE = options.log_path @gen.coroutine def enumerate_device(this_node, node_id, response): global UAVCAN_NODE_INFO, UAVCAN_NODE_CONFIG log.debug("enumerate_device({}, {!r})".format(node_id, response))
cmd_group.add_option("-n", "--node-id", dest="node_id", default=127, help="run master with NODE_ID", metavar="NODE_ID") cmd_group.add_option("-s", "--bus-speed", dest="bus_speed", default=1000000, help="set CAN bus speed", metavar="NODE_ID") cmd_group.add_option("-l", "--log", dest="log_path", default=None, help="log UAVCAN messages to PATH", metavar="PATH") cmd_group = OptionGroup(parser, "UAVCAN options") cmd_group.add_option("--dsdl", dest="dsdl_paths", action="append", metavar="PATH", help="load DSDL files from PATH") options, args = parser.parse_args() uavcan.load_dsdl(options.dsdl_paths + ["../dsdl/thiemar"]) ioloop = tornado.ioloop.IOLoop.instance() if options.log_path: JSON_LOG_FILE = options.log_path if len(args): node = uavcan.node.Node([ # Server implementation (uavcan.protocol.NodeStatus, uavcan.monitors.NodeStatusMonitor, {"new_node_callback": enumerate_device}), (uavcan.protocol.dynamic_node_id.Allocation, uavcan.monitors.DynamicNodeIDServer, {"dynamic_id_range": (2, 125)}), (uavcan.thirdparty.thiemar.equipment.esc.Status, ESCStatusMonitor),
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, dsdl_directory = run_setup_window(get_app_icon(), args.dsdl) 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) if not dsdl_directory: dsdl_directory = args.dsdl try: if dsdl_directory: logger.info('Loading custom DSDL from %r', dsdl_directory) uavcan.load_dsdl(dsdl_directory) logger.info('Custom DSDL loaded successfully') # setup an environment variable for sub-processes to know where to load custom DSDL from os.environ['UAVCAN_CUSTOM_DSDL_PATH'] = dsdl_directory except Exception as ex: logger.exception('No DSDL loaded from %r, only standard messages will be supported', dsdl_directory) show_error('DSDL not loaded', 'Could not load DSDL definitions from %r.\n' 'The application will continue to work without the custom DSDL definitions.' % dsdl_directory, ex, blocking=True) # 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.warning('UAVCAN Transfer Error occurred 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)
import struct import MMCCRC import uavcan from contextlib import closing import time uavcan.load_dsdl() MOUNT_MOVE_WORKING = True MOUNT_MOVE_STOP = False MOUNT_ZOOM_WORKING = True MOUNT_ZOOM_STOP = False class MountControl: def __init__(self, node): self.buf = [] self.__zoom = 1 self.node = node self.__yawAngle = 0 self.__pitchAngle = 0 self.__rollAngle = 0 self.mountMoveStatus = MOUNT_MOVE_STOP self.mountZoomStatus = MOUNT_ZOOM_STOP self.__tempBuf = [] self.__tempType = 0 self.__origin = {'pitch': 0, 'yaw': 0} self.table = { 'pitchAngle': { 'type': b'\x0e', 'len': b'\x03', 'dataType': 'b'
def main(): args = parse_args() config = yaml.load(args.config) config = config['actuator'][args.board] if args.dsdl: uavcan.load_dsdl(args.dsdl) global node node = uavcan.make_node(args.port, node_id=127) node.add_handler(uavcan.protocol.NodeStatus, node_status_callback) req = uavcan.thirdparty.cvra.motor.config.LoadConfiguration.Request() req.position_pid.kp = config['control']['position']['kp'] req.position_pid.ki = config['control']['position']['ki'] req.position_pid.kd = config['control']['position']['kd'] req.velocity_pid.kp = config['control']['velocity']['kp'] req.velocity_pid.ki = config['control']['velocity']['ki'] req.velocity_pid.kd = config['control']['velocity']['kd'] req.current_pid.kp = config['control']['current']['kp'] req.current_pid.ki = config['control']['current']['ki'] req.current_pid.kd = config['control']['current']['kd'] req.torque_limit = config['control']['torque_limit'] req.acceleration_limit = config['control']['acceleration_limit'] req.low_batt_th = config['control']['low_batt_th'] req.velocity_limit = config['control']['velocity_limit'] req.torque_constant = config['motor']['torque_constant'] req.motor_encoder_steps_per_revolution = config['motor'][ 'motor_encoder_steps_per_revolution'] try: req.second_encoder_steps_per_revolution = config['motor'][ 'second_encoder_steps_per_revolution'] except KeyError: pass req.transmission_ratio_p = config['motor']['transmission_ratio_p'] req.transmission_ratio_q = config['motor']['transmission_ratio_q'] req.mode = config['motor']['mode'] while args.board not in boards_id_by_name: print("Board not found, waiting...") try: node.spin(0.2) except uavcan.transport.TransferError: pass board_id = boards_id_by_name[args.board] print("Found board, ID={}".format(board_id)) node.request(req, board_id, request_cb) try: node.spin(1) except uavcan.transport.TransferError: pass print("Done")
if __name__ == "__main__": # decode options given to the script try: opts, args = getopt.getopt(sys.argv[1:],'hc:',['help','custom=']) except getopt.GetoptError: printUsage() sys.exit() # include/execute options given to the script for opt, arg in opts: if opt in ('-h','--help'): printUsage() sys.exit() elif opt in ('-c','--custom'): uavcan.load_dsdl(arg) longest_name = max(map(len, uavcan.TYPENAMES.keys())) header = 'Full Data Type Name'.ljust(longest_name) + ' | DDTID | Type Signature | Max Bit Len ' print(header) print('-' * len(header)) for typename, typedef in sorted(uavcan.TYPENAMES.items()): ddtid = typedef.default_dtid if typedef.default_dtid is not None else 'N/A' s = '%-*s % 5s 0x%016x' % (longest_name, typename, ddtid, typedef.get_data_type_signature()) try: s += ' % 5d' % typedef.get_max_bitlen() except Exception: s += ' % 5d / %-5d' % (typedef.get_max_bitlen_request(), typedef.get_max_bitlen_response()) print(s)