def field_to_string(self, name, keep_literal=False, fallback_format=None):
val = getattr(self.struct, name)
# Trying prefixed constants first
for name, const in self.type_inspector.list_enum_constants(name):
if const.value == val:
return ('%s (%r)' % (name, val)) if keep_literal else name
# If the struct contains only one field
if len(uavcan.get_fields(self.struct)) == 1:
for name, cvalue in uavcan.get_constants(self.struct).items():
if cvalue == val:
return name
return (fallback_format or '%r') % val
def field_to_string(self, name, keep_literal=False, fallback_format=None):
val = getattr(self.struct, name)
# Trying prefixed constants first
for name, const in self.type_inspector.list_enum_constants(name):
if const.value == val:
return ('%s (%r)' % (name, val)) if keep_literal else name
# If the struct contains only one field
if len(uavcan.get_fields(self.struct)) == 1:
for name, cvalue in uavcan.get_constants(self.struct).items():
if cvalue == val:
return name
return (fallback_format or '%r') % val
def _read(self, e):
logger.debug("[#{0:03d}:uavcan.protocol.file.Read] {1!r} @ offset {2:d}"
.format(e.transfer.source_node_id, e.request.path.path.decode(), e.request.offset))
try:
with open(self._resolve_path(e.request.path), "rb") as f:
f.seek(e.request.offset)
resp = uavcan.protocol.file.Read.Response()
read_size = uavcan.get_uavcan_data_type(uavcan.get_fields(resp)['data']).max_size
resp.data = bytearray(f.read(read_size))
resp.error.value = resp.error.OK
except Exception:
logger.exception("[#{0:03d}:uavcan.protocol.file.Read] error")
resp = uavcan.protocol.file.Read.Response()
resp.error.value = resp.error.UNKNOWN_ERROR
return resp
def _extract_struct_fields(m):
if isinstance(m, uavcan.transport.CompoundValue):
out = CompactMessage(uavcan.get_uavcan_data_type(m).full_name)
for field_name, field in uavcan.get_fields(m).items():
if uavcan.is_union(m) and uavcan.get_active_union_field(m) != field_name:
continue
val = _extract_struct_fields(field)
if val is not None:
out._add_field(field_name, val)
return out
elif isinstance(m, uavcan.transport.ArrayValue):
# cannot say I'm breaking the rules
container = bytes if uavcan.get_uavcan_data_type(m).is_string_like else list
# if I can glue them back together
return container(filter(lambda x: x is not None, (_extract_struct_fields(item) for item in m)))
elif isinstance(m, uavcan.transport.PrimitiveValue):
return m.value
elif isinstance(m, (int, float, bool)):
return m
elif isinstance(m, uavcan.transport.VoidValue):
pass
else:
raise ValueError(':(')
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 _to_yaml_impl(obj, indent_level=0, parent=None, name=None, uavcan_type=None):
buf = StringIO()
def write(fmt, *args):
buf.write((fmt % args) if len(args) else fmt)
def indent_newline():
buf.write(os.linesep + ' ' * 2 * indent_level)
# Decomposing PrimitiveValue to value and type. This is ugly but it's by design...
if isinstance(obj, PrimitiveValue):
uavcan_type = uavcan.get_uavcan_data_type(obj)
obj = obj.value
# CompoundValue
if isinstance(obj, CompoundValue):
first_field = True
# Rendering all fields than can be rendered
for field_name, field in uavcan.get_fields(obj).items():
if uavcan.is_union(obj) and uavcan.get_active_union_field(obj) != field_name:
continue
if isinstance(field, VoidValue):
continue
if (first_field and indent_level > 0) or not first_field:
indent_newline()
first_field = False
rendered_field = _to_yaml_impl(field, indent_level=indent_level + 1, parent=obj, name=field_name)
write('%s: %s', field_name, rendered_field)
# Special case - empty non-union struct is rendered as empty map
if first_field and not uavcan.is_union(obj):
if indent_level > 0:
indent_newline()
write('{}')
# ArrayValue
elif isinstance(obj, ArrayValue):
t = uavcan.get_uavcan_data_type(obj)
if t.value_type.category == t.value_type.CATEGORY_PRIMITIVE:
def is_nice_character(ch):
if 32 <= ch <= 126:
return True
if ch in b'\n\r\t':
return True
return False
as_bytes = '[%s]' % ', '.join([_to_yaml_impl(x, indent_level=indent_level + 1, uavcan_type=t.value_type)
for x in obj])
if t.is_string_like and all(map(is_nice_character, obj)):
write('%r # ', obj.decode())
write(as_bytes)
else:
if len(obj) == 0:
write('[]')
else:
for x in obj:
indent_newline()
write('- %s', _to_yaml_impl(x, indent_level=indent_level + 1, uavcan_type=t.value_type))
# Primitive types
elif isinstance(obj, float):
assert uavcan_type is not None
float_fmt = {
16: '%.4f',
32: '%.6f',
64: '%.9f',
}[uavcan_type.bitlen]
write(float_fmt, obj)
elif isinstance(obj, bool):
write('%s', 'true' if obj else 'false')
elif isinstance(obj, int):
write('%s', obj)
if parent is not None and name is not None:
resolved_name = value_to_constant_name(parent, name)
if isinstance(resolved_name, str):
write(' # %s', resolved_name)
# Non-printable types
elif isinstance(obj, VoidValue):
pass
# Unknown types
else:
raise ValueError('Cannot generate YAML representation for %r' % type(obj))
return buf.getvalue()