Beispiel #1
0
                         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))
Beispiel #2
0
    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),
Beispiel #3
0
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)
Beispiel #4
0
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)