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