def param_opcode_response(event): nonlocal configuration_finished if not event: raise Exception('Request timed out') print(dronecan.to_yaml(event)) if not event.response.ok: raise Exception('Param opcode execution rejected\n' + dronecan.to_yaml(event)) else: configuration_finished = True
def publish_throttle_setpoint(): if args.send_safety: # optionally send safety off messages. These are needed for some ESCs message = dronecan.ardupilot.indication.SafetyState() message.status = message.STATUS_SAFETY_OFF node.broadcast(message) print(dronecan.to_yaml(message)) # Generating a sine wave setpoint = int(512 * (math.sin(time.time()) + 2)) # Commanding ESC with indices 0, 1, 2, 3 only commands = [setpoint, setpoint, setpoint, setpoint] message = dronecan.uavcan.equipment.esc.RawCommand(cmd=commands) node.broadcast(message) # display the message on the console in human readable format print(dronecan.to_yaml(message))
def indication_callback(event): nonlocal received_indication if event.transfer.source_node_id in enumerated_nodes: print( 'Indication callback from node %d ignored - already enumerated' % event.transfer.source_node_id) else: print(dronecan.to_yaml(event)) received_indication = event
def begin_response_checker(event): nonlocal begin_responses_succeeded if not event: raise Exception('Request timed out') if event.response.error != event.response.ERROR_OK: raise Exception('Enumeration rejected\n' + dronecan.to_yaml(event)) begin_responses_succeeded += 1
def param_set_response(event): if not event: raise Exception('Request timed out') assert event.response.name == received_indication.message.parameter_name assert event.response.value.integer_value == next_index print(dronecan.to_yaml(event)) node.request( uavcan.protocol.param.ExecuteOpcode.Request( opcode=uavcan.protocol.param.ExecuteOpcode.Request( ).OPCODE_SAVE), target_node_id, param_opcode_response)
help="CAN bit rate") parser.add_argument("--node-id", default=100, type=int, help="CAN node ID") parser.add_argument("--dna-server", action='store_true', default=False, help="run DNA server") parser.add_argument("port", default=None, type=str, help="serial port") args = parser.parse_args() # Initializing a DroneCAN node instance. node = dronecan.make_node(args.port, node_id=args.node_id, bitrate=args.bitrate) # Initializing a node monitor, so we can see what nodes are online node_monitor = dronecan.app.node_monitor.NodeMonitor(node) if args.dna_server: # optionally start a DNA server dynamic_node_id_allocator = dronecan.app.dynamic_node_id.CentralizedServer( node, node_monitor) # callback for printing all messages in human-readable YAML format. node.add_handler(None, lambda msg: print(dronecan.to_yaml(msg))) # Running the node until the application is terminated or until first error. try: node.spin() except KeyboardInterrupt: pass
print(dronecan.to_yaml(message)) # Initializing a DroneCAN node instance. node = dronecan.make_node(args.port, node_id=args.node_id, bitrate=args.bitrate) # Initializing a node monitor, so we can see what nodes are online node_monitor = dronecan.app.node_monitor.NodeMonitor(node) if args.dna_server: # optionally start a DNA server dynamic_node_id_allocator = dronecan.app.dynamic_node_id.CentralizedServer(node, node_monitor) # Waiting for at least one other node to appear online while len(node_monitor.get_all_node_id()) <= 1: print('Waiting for other nodes to become online...') node.spin(timeout=1) print("There are %u nodes online" % len(node_monitor.get_all_node_id())) # setup to publish ESC RawCommand at 20Hz node.periodic(0.05, publish_throttle_setpoint) # callback for printing ESC status message to stdout in human-readable YAML format. node.add_handler(dronecan.uavcan.equipment.esc.Status, lambda msg: print(dronecan.to_yaml(msg))) # Running the node until the application is terminated or until first error. try: node.spin() except KeyboardInterrupt: pass