def wait_for_boot():
boot_deadline = time.monotonic() + BOOT_TIMEOUT
bootloader_detected = False
while boot_deadline > time.monotonic():
target_serial_symlink = get_target_serial_port_symlink()
if not target_serial_symlink:
time.sleep(0.5)
continue
if 'bootloader' in target_serial_symlink.lower():
if not bootloader_detected:
bootloader_detected = True
info(
'Bootloader port detected, waiting for the application to boot...'
)
else:
info('Boot successful')
return
time.sleep(1)
abort(
'The serial port of the device did not appear in the system.'
'This may indicate that the device could not boot or that the USB connection to it is not working properly.'
"Please double check the cabling. If it still doesn't work, the device must be broken."
)
def load_and_start_firmware(bootloader_interface, firmware_image):
while True:
try:
info('Flashing the firmware [%d bytes]...', len(firmware_image))
bootloader_interface.unlock()
bootloader_interface.load_firmware(firmware_image)
except Exception as ex:
error('Flashing failed: %r', ex)
if not input('Try harder?', yes_no=True):
abort('Flashing failed')
else:
input('Set PIO0_1 high (J4 open), then press ENTER')
info('Starting the firmware...')
bootloader_interface.reset()
break
def load_and_start_firmware(bootloader_interface, firmware_image):
while True:
try:
info('Flashing the firmware [%d bytes]...', len(firmware_image))
bootloader_interface.unlock()
bootloader_interface.load_firmware(firmware_image)
except Exception as ex:
error('Flashing failed: %r', ex)
if not input('Try harder?', yes_no=True):
abort('Flashing failed')
else:
input('Set PIO0_1 high (J4 open), then press ENTER')
info('Starting the firmware...')
bootloader_interface.reset()
break
def wait_for_boot():
def handle_serial_port_hanging():
fatal('DRWATSON HAS DETECTED A PROBLEM WITH CONNECTED HARDWARE AND NEEDS TO TERMINATE.\n'
'A serial port operation has timed out. This usually indicates a problem with the connected '
'hardware or its drivers. Please disconnect all USB devices currently connected to this computer, '
"then connect them back and restart Drwatson. If you're using a virtual machine, please reboot it.",
use_abort=True)
with BackgroundDelay(BOOT_TIMEOUT * 5, handle_serial_port_hanging):
with open_serial_port(DEBUGGER_PORT_CLI_GLOB) as p:
try:
serial_cli = SerialCLI(p)
boot_deadline = time.monotonic() + BOOT_TIMEOUT
boot_notification_received = False
failure_notification_received = False
while boot_deadline > time.monotonic():
timed_out, line = serial_cli.read_line(END_OF_BOOT_LOG_TIMEOUT)
if not timed_out:
cli_logger.info(repr(line))
if PRODUCT_NAME.lower() in line.lower() and 'bootloader' not in line.lower():
info('Boot confirmed')
boot_notification_received = True
if 'error' in line.lower() or 'fail' in line.lower():
failure_notification_received = True
error('Boot error: %r', line)
else:
if failure_notification_received:
abort('Device failed to start up normally; see the log for details')
if boot_notification_received:
return
except IOError:
logging.info('Boot error', exc_info=True)
finally:
p.flushInput()
abort("The device did not report to CLI with a correct boot message. Possible reasons for this error:\n"
'1. The device could not boot properly (however it was flashed successfully).\n'
'2. The debug connector is not soldered properly.\n'
'3. The serial port is open by another application.\n'
'4. Either USB-UART adapter or VM are malfunctioning. Try to re-connect the '
'adapter (disconnect from USB and from the device!) or reboot the VM.')
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)
def cb(e):
nonlocal response_event
if not e:
abort('Request has timed out: %r', what)
response_event = e
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 check_everything():
check_status()
try:
m = col_pressure[node_id].message
except KeyError:
abort(
'Pressure measurements are not available. Check the sensor.'
)
else:
if not 50000 < m.static_pressure < 150000:
abort(
'Invalid pressure reading: %d Pascal. Check the sensor.',
m.static_pressure)
try:
m = col_temp[node_id].message
except KeyError:
abort(
'Temperature measurements are not available. Check the sensor.'
)
else:
if not 10 < (m.static_temperature - 273.15) < 50:
abort(
'Invalid temperature reading: %d Kelvin. Check the sensor.',
m.static_temperature)
try:
m = col_mag[node_id].message
except KeyError:
abort(
'Magnetic field measurements are not available. Check the sensor.'
)
else:
magnetic_field_scalar = numpy.linalg.norm(
m.magnetic_field_ga)
if not 0.01 < magnetic_field_scalar < 2:
abort(
'Invalid magnetic field strength reading: %d Gauss. Check the sensor.',
magnetic_field_scalar)
def test_uavcan():
node_info = uavcan.protocol.GetNodeInfo.Response()
node_info.name = 'com.zubax.drwatson.zubax_gnss'
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.transport.TransferError:
logger.debug(
'Transfer error while spinning the node. '
'Reporting at the DEBUG level because of https://github.com/UAVCAN/pyuavcan/issues/14',
exc_info=True)
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()
# Dynamic node ID allocation
try:
nsmon = uavcan.app.node_monitor.NodeMonitor(n)
alloc = uavcan.app.dynamic_node_id.CentralizedServer(n, nsmon)
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, fire_and_forget=False, timeout=2):
response_event = None
def cb(e):
nonlocal response_event
if not e:
abort('Request has timed out: %r', what)
response_event = e
if fire_and_forget:
n.request(what, node_id, lambda _: None, timeout=timeout)
safe_spin(0.1)
else:
n.request(what, node_id, cb, timeout=timeout)
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(
12,
'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('Reconfiguring the node...')
def log_all_params():
for index in range(10000):
r = request(
uavcan.protocol.param.GetSet.Request(index=index))
if not r.name:
break
logger.info(
'Param %-30r %r' %
(r.name.decode(),
getattr(r.value, uavcan.get_active_union_field(
r.value))))
def set_param(name, value, union_field=None):
union_field = union_field or {
int: 'integer_value',
float: 'real_value',
bool: 'boolean_value',
str: 'string_value'
}[type(value)]
logger.info('Setting parameter %r field %r value %r', name,
union_field, value)
req = uavcan.protocol.param.GetSet.Request()
req.name.encode(name)
setattr(req.value, union_field, value)
r = request(req)
enforce(
uavcan.get_active_union_field(r.value) == union_field,
'Union field mismatch in param set response for %r', name)
enforce(
getattr(r.value, union_field) == value,
'The node refused to set parameter %r', name)
set_param('uavcan.pubp-time', 10000)
set_param('uavcan.pubp-stat', 2000)
set_param('uavcan.pubp-pres', 10000)
set_param('uavcan.pubp-mag', 20000)
set_param('uavcan.pubp-fix', 66666)
set_param('uavcan.pubp-aux', 100000)
enforce(
request(
uavcan.protocol.param.ExecuteOpcode.Request(
opcode=uavcan.protocol.param.ExecuteOpcode.Request(
).OPCODE_SAVE)).ok, 'Could not save configuration')
request(uavcan.protocol.RestartNode.Request(
magic_number=uavcan.protocol.RestartNode.Request(
).MAGIC_NUMBER),
fire_and_forget=True)
# Don't tell anybody I wrote this. I'm ashamed of this shit and too tired to redesign it. :(
with BackgroundSpinner(safe_spin, 0.1):
wait_for_boot()
wait_for_init()
check_status()
log_all_params()
def make_collector(data_type, timeout=0.5):
return uavcan.app.message_collector.MessageCollector(
n, data_type, timeout=timeout)
col_fix = make_collector(uavcan.equipment.gnss.Fix2)
col_aux = make_collector(uavcan.equipment.gnss.Auxiliary)
col_mag = make_collector(
uavcan.equipment.ahrs.MagneticFieldStrength)
col_pressure = make_collector(
uavcan.equipment.air_data.StaticPressure)
col_temp = make_collector(
uavcan.equipment.air_data.StaticTemperature)
def check_everything():
check_status()
try:
m = col_pressure[node_id].message
except KeyError:
abort(
'Pressure measurements are not available. Check the sensor.'
)
else:
if not 50000 < m.static_pressure < 150000:
abort(
'Invalid pressure reading: %d Pascal. Check the sensor.',
m.static_pressure)
try:
m = col_temp[node_id].message
except KeyError:
abort(
'Temperature measurements are not available. Check the sensor.'
)
else:
if not 10 < (m.static_temperature - 273.15) < 50:
abort(
'Invalid temperature reading: %d Kelvin. Check the sensor.',
m.static_temperature)
try:
m = col_mag[node_id].message
except KeyError:
abort(
'Magnetic field measurements are not available. Check the sensor.'
)
else:
magnetic_field_scalar = numpy.linalg.norm(
m.magnetic_field_ga)
if not 0.01 < magnetic_field_scalar < 2:
abort(
'Invalid magnetic field strength reading: %d Gauss. Check the sensor.',
magnetic_field_scalar)
imperative(
'Testing GNSS performance. Place the device close to a window to ensure decent GNSS reception. '
'Please note that this test is very crude, it can only detect whether GNSS circuit is working '
'at all or not. If GNSS performance is degraded due to improper manufacturing process, '
'this test may fail to detect it, so please double check that your manufacturing process '
'adheres to the documentation.')
info('Waiting for GNSS fix...')
with time_limit(
GNSS_FIX_TIMEOUT,
'GNSS fix timeout. Check the RF circuit, AFE, antenna, and receiver'
):
while True:
safe_spin(1)
check_everything()
sats_visible = col_aux[node_id].message.sats_visible
sats_used = col_aux[node_id].message.sats_used
sys.stdout.write('\rsat stats: visible %d, used %d \r' %
(sats_visible, sats_used))
sys.stdout.flush()
if col_fix[node_id].message.status >= 3:
break
info('Waiting for %d satellites...', GNSS_MIN_SAT_NUM)
with time_limit(
GNSS_MIN_SAT_TIMEOUT,
'GNSS performance is degraded. Could be caused by incorrectly assembled RF circuit.'
):
while True:
safe_spin(0.5)
check_everything()
num = col_fix[node_id].message.sats_used
cov = list(col_fix[node_id].message.covariance)
sys.stdout.write('\r%d sats, covariance: %r \r' %
(num, cov))
sys.stdout.flush()
if num >= GNSS_MIN_SAT_NUM:
break
check_everything()
info(
'Last sampled sensor measurements are provided below. They appear to be correct.'
)
info('GNSS fix: %r', col_fix[node_id].message)
info('GNSS aux: %r', col_aux[node_id].message)
info('Magnetic field [Ga]: %r', col_mag[node_id].message)
info('Pressure [Pa]: %r', col_pressure[node_id].message)
info('Temperature [K]: %r', col_temp[node_id].message)
# Finalizing the test
info('Resetting the configuration to factory default...')
enforce(
request(
uavcan.protocol.param.ExecuteOpcode.Request(
opcode=uavcan.protocol.param.ExecuteOpcode.Request(
).OPCODE_ERASE)).ok, 'Could not erase configuration')
request(uavcan.protocol.RestartNode.Request(
magic_number=uavcan.protocol.RestartNode.Request(
).MAGIC_NUMBER),
fire_and_forget=True)
with BackgroundSpinner(safe_spin, 0.1):
wait_for_boot()
wait_for_init()
check_status()
log_all_params()
except Exception:
for nid in nsmon.get_all_node_id():
print('Node state: %r' % nsmon.get(nid))
raise
# Blocking questions are moved out of the node scope because blocking breaks CAN communications
# Note that we must instantiate the driver in order to ensure proper traffic LED behavior
# Periodic calls to receive() are needed to avoid RX queue overflow
with closing(uavcan.make_driver(iface, bitrate=CAN_BITRATE)) as d:
with BackgroundSpinner(lambda: d.receive(0.01)):
if not input('Is the PPS LED blinking once per second?',
yes_no=True,
default_answer=True):
abort('PPS LED is not working')
if not input('Is the CAN1 LED blinking or glowing solid?',
yes_no=True,
default_answer=True):
abort(
'CAN1 LED is not working (however the interface is fine)')
if not input('Is the STATUS LED blinking once per second?',
yes_no=True,
default_answer=True):
abort('STATUS LED is not working')
# Testing CAN2
input('1. Disconnect CAN1 and connect to CAN2\n'
'2. Terminate CAN2\n'
'3. Press ENTER')
if not input('Is the CAN2 LED blinking or glowing solid?',
yes_no=True,
default_answer=True):
abort('Either CAN2 or its LED are not working')
def check_everything():
check_status()
try:
m = col_pressure[node_id].message
except KeyError:
abort('Pressure measurements are not available. Check the sensor.')
else:
if not 50000 < m.static_pressure < 150000:
abort('Invalid pressure reading: %d Pascal. Check the sensor.', m.static_pressure)
try:
m = col_temp[node_id].message
except KeyError:
abort('Temperature measurements are not available. Check the sensor.')
else:
if not 10 < (m.static_temperature - 273.15) < 50:
abort('Invalid temperature reading: %d Kelvin. Check the sensor.', m.static_temperature)
try:
m = col_mag[node_id].message
except KeyError:
abort('Magnetic field measurements are not available. Check the sensor.')
else:
magnetic_field_scalar = numpy.linalg.norm(m.magnetic_field_ga)
if not 0.01 < magnetic_field_scalar < 2:
abort('Invalid magnetic field strength reading: %d Gauss. Check the sensor.',
magnetic_field_scalar)
def test_uavcan():
node_info = uavcan.protocol.GetNodeInfo.Response()
node_info.name = 'com.zubax.drwatson.zubax_gnss'
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.transport.TransferError:
logger.debug('Transfer error while spinning the node. '
'Reporting at the DEBUG level because of https://github.com/UAVCAN/pyuavcan/issues/14',
exc_info=True)
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()
# Dynamic node ID allocation
try:
nsmon = uavcan.app.node_monitor.NodeMonitor(n)
alloc = uavcan.app.dynamic_node_id.CentralizedServer(n, nsmon)
with time_limit(10,
'The node did not show up in time. Check the CAN interface and the 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, fire_and_forget=False, timeout=2):
response_event = None
def cb(e):
nonlocal response_event
if not e:
abort('Request has timed out: %r', what)
response_event = e
if fire_and_forget:
n.request(what, node_id, lambda _: None, timeout=timeout)
safe_spin(0.1)
else:
n.request(what, node_id, cb, timeout=timeout)
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(12, '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('Reconfiguring the node...')
def log_all_params():
for index in range(10000):
r = request(uavcan.protocol.param.GetSet.Request(index=index))
if not r.name:
break
logger.info('Param %-30r %r' % (r.name.decode(),
getattr(r.value, uavcan.get_active_union_field(r.value))))
def set_param(name, value, union_field=None):
union_field = union_field or {
int: 'integer_value',
float: 'real_value',
bool: 'boolean_value',
str: 'string_value'
}[type(value)]
logger.info('Setting parameter %r field %r value %r', name, union_field, value)
req = uavcan.protocol.param.GetSet.Request()
req.name.encode(name)
setattr(req.value, union_field, value)
r = request(req)
enforce(uavcan.get_active_union_field(r.value) == union_field,
'Union field mismatch in param set response for %r', name)
enforce(getattr(r.value, union_field) == value,
'The node refused to set parameter %r', name)
set_param('uavcan.pubp-time', 10000)
set_param('uavcan.pubp-stat', 2000)
set_param('uavcan.pubp-pres', 10000)
set_param('uavcan.pubp-mag', 20000)
set_param('uavcan.pubp-fix', 66666)
set_param('uavcan.pubp-aux', 100000)
enforce(request(uavcan.protocol.param.ExecuteOpcode.Request(
opcode=uavcan.protocol.param.ExecuteOpcode.Request().OPCODE_SAVE)).ok,
'Could not save configuration')
request(uavcan.protocol.RestartNode.Request(
magic_number=uavcan.protocol.RestartNode.Request().MAGIC_NUMBER),
fire_and_forget=True)
# Don't tell anybody I wrote this. I'm ashamed of this shit and too tired to redesign it. :(
with BackgroundSpinner(safe_spin, 0.1):
wait_for_boot()
wait_for_init()
check_status()
log_all_params()
def make_collector(data_type, timeout=0.5):
return uavcan.app.message_collector.MessageCollector(n, data_type, timeout=timeout)
col_fix = make_collector(uavcan.equipment.gnss.Fix2)
col_aux = make_collector(uavcan.equipment.gnss.Auxiliary)
col_mag = make_collector(uavcan.equipment.ahrs.MagneticFieldStrength)
col_pressure = make_collector(uavcan.equipment.air_data.StaticPressure)
col_temp = make_collector(uavcan.equipment.air_data.StaticTemperature)
def check_everything():
check_status()
try:
m = col_pressure[node_id].message
except KeyError:
abort('Pressure measurements are not available. Check the sensor.')
else:
if not 50000 < m.static_pressure < 150000:
abort('Invalid pressure reading: %d Pascal. Check the sensor.', m.static_pressure)
try:
m = col_temp[node_id].message
except KeyError:
abort('Temperature measurements are not available. Check the sensor.')
else:
if not 10 < (m.static_temperature - 273.15) < 50:
abort('Invalid temperature reading: %d Kelvin. Check the sensor.', m.static_temperature)
try:
m = col_mag[node_id].message
except KeyError:
abort('Magnetic field measurements are not available. Check the sensor.')
else:
magnetic_field_scalar = numpy.linalg.norm(m.magnetic_field_ga)
if not 0.01 < magnetic_field_scalar < 2:
abort('Invalid magnetic field strength reading: %d Gauss. Check the sensor.',
magnetic_field_scalar)
imperative('Testing GNSS performance.')
info('Waiting for GNSS fix...')
with time_limit(GNSS_FIX_TIMEOUT, 'GNSS fix timeout. Check the RF circuit, AFE, antenna, and receiver'):
while True:
safe_spin(1)
check_everything()
sats_visible = col_aux[node_id].message.sats_visible
sats_used = col_aux[node_id].message.sats_used
sys.stdout.write('\rsat stats: visible %d, used %d \r' % (sats_visible, sats_used))
sys.stdout.flush()
if col_fix[node_id].message.status >= 3:
break
info('Waiting for %d satellites...', GNSS_MIN_SAT_NUM)
with time_limit(GNSS_MIN_SAT_TIMEOUT,
'GNSS performance is degraded. Could be caused by incorrectly assembled RF circuit.'):
while True:
safe_spin(0.5)
check_everything()
num = col_fix[node_id].message.sats_used
cov = list(col_fix[node_id].message.covariance)
sys.stdout.write('\r%d sats, covariance: %r \r' % (num, cov))
sys.stdout.flush()
if num >= GNSS_MIN_SAT_NUM:
break
check_everything()
info('The last sampled sensor measurements are provided below. They appear to be correct.')
info('GNSS fix: %r', col_fix[node_id].message)
info('GNSS aux: %r', col_aux[node_id].message)
info('Magnetic field [Ga]: %r', col_mag[node_id].message)
info('Pressure [Pa]: %r', col_pressure[node_id].message)
info('Temperature [K]: %r', col_temp[node_id].message)
# Finalizing the test
info('Resetting the configuration to the factory defaults...')
enforce(request(uavcan.protocol.param.ExecuteOpcode.Request(
opcode=uavcan.protocol.param.ExecuteOpcode.Request().OPCODE_ERASE)).ok,
'Could not erase configuration')
request(uavcan.protocol.RestartNode.Request(
magic_number=uavcan.protocol.RestartNode.Request().MAGIC_NUMBER),
fire_and_forget=True)
with BackgroundSpinner(safe_spin, 0.1):
wait_for_boot()
wait_for_init()
check_status()
log_all_params()
except Exception:
for nid in nsmon.get_all_node_id():
print('Node state: %r' % nsmon.get(nid))
raise
# Blocking questions are moved out of the node scope because blocking breaks CAN communications
# Note that we must instantiate the driver in order to ensure proper traffic LED behavior
# Periodic calls to receive() are needed to avoid RX queue overflow
with closing(uavcan.make_driver(iface, bitrate=CAN_BITRATE)) as d:
with BackgroundSpinner(lambda: d.receive(0.01)):
if not input('Is the PPS LED blinking once per second?', yes_no=True, default_answer=True):
abort('PPS LED is not working')
if not input('Is the CAN1 LED blinking or glowing solid?', yes_no=True, default_answer=True):
abort('CAN1 LED is not working (however the interface is fine)')
if not input('Is the STATUS LED blinking once per second?', yes_no=True, default_answer=True):
abort('STATUS LED is not working')
# Testing CAN2
input('1. Disconnect the cable from CAN1 and connect it to CAN2\n'
'2. Terminate CAN2\n'
'3. Press ENTER')
if not input('Is the CAN2 LED blinking or glowing solid?', yes_no=True, default_answer=True):
abort('Either CAN2 or its LED are not working')
def cb(e):
nonlocal response_event
if not e:
abort("Request has timed out: %r", what)
response_event = e # @UnusedVariable
def test_uavcan():
node_info = uavcan.protocol.GetNodeInfo.Response() # @UndefinedVariable
node_info.name.encode("com.zubax.drwatson.zubax_gnss")
iface = init_can_iface()
with closing(
uavcan.make_node(iface, bitrate=CAN_BITRATE, node_id=127, mode=uavcan.protocol.NodeStatus().MODE_OPERATIONAL)
) as n: # @UndefinedVariable
def safe_spin(timeout):
try:
n.spin(timeout)
except uavcan.transport.TransferError:
logger.debug(
"Transfer error while spinning the node. "
"Reporting at the DEBUG level because of https://github.com/UAVCAN/pyuavcan/issues/14",
exc_info=True,
)
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()
# Dynamic node ID allocation
try:
nsmon = uavcan.monitors.NodeStatusMonitor(n)
alloc = uavcan.monitors.DynamicNodeIDServer(n, nsmon) # @UnusedVariable
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, fire_and_forget=False):
response_event = None
def cb(e):
nonlocal response_event
if not e:
abort("Request has timed out: %r", what)
response_event = e # @UnusedVariable
if fire_and_forget:
n.request(what, node_id, lambda _: None)
safe_spin(0.1)
else:
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
): # @UndefinedVariable
break
def check_status():
status = nsmon.get(node_id).status
enforce(
status.mode == uavcan.protocol.NodeStatus().MODE_OPERATIONAL, # @UndefinedVariable
"Unexpected operating mode",
)
enforce(
status.health == uavcan.protocol.NodeStatus().HEALTH_OK, "Bad node health" # @UndefinedVariable
)
info("Waiting for the node to complete initialization...")
wait_for_init()
check_status()
info("Reconfiguring the node...")
def log_all_params():
for index in range(10000):
r = request(uavcan.protocol.param.GetSet.Request(index=index)) # @UndefinedVariable
if not r.name:
break
logger.info("Param %-30r %r" % (r.name.decode(), getattr(r.value, r.value.union_field)))
def set_param(name, value, union_field=None):
union_field = (
union_field
or {int: "integer_value", float: "real_value", bool: "boolean_value", str: "string_value"}[
type(value)
]
)
logger.info("Setting parameter %r field %r value %r", name, union_field, value)
req = uavcan.protocol.param.GetSet.Request() # @UndefinedVariable
req.name.encode(name)
setattr(req.value, union_field, value)
r = request(req)
enforce(r.value.union_field == union_field, "Union field mismatch in param set response for %r", name)
enforce(getattr(r.value, union_field) == value, "The node refused to set parameter %r", name)
set_param("uavcan.pubp-time", 10000)
set_param("uavcan.pubp-stat", 2000)
set_param("uavcan.pubp-pres", 10000)
set_param("uavcan.pubp-mag", 20000)
set_param("uavcan.pubp-fix", 66666)
set_param("uavcan.pubp-aux", 100000)
enforce(
request(
uavcan.protocol.param.ExecuteOpcode.Request( # @UndefinedVariable
opcode=uavcan.protocol.param.ExecuteOpcode.Request().OPCODE_SAVE
)
).ok, # @UndefinedVariable
"Could not save configuration",
)
request(
uavcan.protocol.RestartNode.Request( # @UndefinedVariable
magic_number=uavcan.protocol.RestartNode.Request().MAGIC_NUMBER
), # @UndefinedVariable
fire_and_forget=True,
)
# Don't tell anybody I wrote this. I'm ashamed of this shit and too tired to redesign it. :(
with BackgroundSpinner(safe_spin, 0.1):
wait_for_boot()
wait_for_init()
check_status()
log_all_params()
def make_collector(data_type, timeout=0.1):
return uavcan.monitors.MessageCollector(n, data_type, timeout=timeout)
col_fix = make_collector(uavcan.equipment.gnss.Fix, 0.2) # @UndefinedVariable
col_aux = make_collector(uavcan.equipment.gnss.Auxiliary, 0.2) # @UndefinedVariable
col_mag = make_collector(uavcan.equipment.ahrs.MagneticFieldStrength) # @UndefinedVariable
col_pressure = make_collector(uavcan.equipment.air_data.StaticPressure) # @UndefinedVariable
col_temp = make_collector(uavcan.equipment.air_data.StaticTemperature) # @UndefinedVariable
def check_everything():
check_status()
try:
m = col_pressure[node_id].message
except KeyError:
abort("Pressure measurements are not available. Check the sensor.")
else:
if not 50000 < m.static_pressure < 150000:
abort("Invalid pressure reading: %d Pascal. Check the sensor.", m.static_pressure)
try:
m = col_temp[node_id].message
except KeyError:
abort("Temperature measurements are not available. Check the sensor.")
else:
if not 10 < (m.static_temperature - 273.15) < 50:
abort("Invalid temperature reading: %d Kelvin. Check the sensor.", m.static_temperature)
try:
m = col_mag[node_id].message
except KeyError:
abort("Magnetic field measurements are not available. Check the sensor.")
else:
magnetic_field_scalar = numpy.linalg.norm(m.magnetic_field_ga) # @UndefinedVariable
if not 0.01 < magnetic_field_scalar < 2:
abort(
"Invalid magnetic field strength reading: %d Gauss. Check the sensor.",
magnetic_field_scalar,
)
imperative(
"Testing GNSS performance. Place the device close to a window to ensure decent GNSS reception. "
"Please note that this test is very crude, it can only detect whether GNSS circuit is working "
"at all or not. If GNSS performance is degraded due to improper manufacturing process, "
"this test may fail to detect it, so please double check that your manufacturing process "
"adheres to the documentation."
)
info("Waiting for GNSS fix...")
with time_limit(GNSS_FIX_TIMEOUT, "GNSS fix timeout. Check the RF circuit, AFE, antenna, and receiver"):
while True:
safe_spin(1)
check_everything()
sats_visible = col_aux[node_id].message.sats_visible
sats_used = col_aux[node_id].message.sats_used
sys.stdout.write("\rsat stats: visible %d, used %d \r" % (sats_visible, sats_used))
sys.stdout.flush()
if col_fix[node_id].message.status >= 3:
break
info("Waiting for %d satellites...", GNSS_MIN_SAT_NUM)
with time_limit(
GNSS_MIN_SAT_TIMEOUT,
"GNSS performance is degraded. Could be caused by incorrectly assembled RF circuit.",
):
while True:
safe_spin(0.5)
check_everything()
num = col_fix[node_id].message.sats_used
pos_cov = list(col_fix[node_id].message.position_covariance)
sys.stdout.write("\r%d sats, pos covariance: %r \r" % (num, pos_cov))
sys.stdout.flush()
if num >= GNSS_MIN_SAT_NUM:
break
check_everything()
info("Last sampled sensor measurements are provided below. They appear to be correct.")
info("GNSS fix: %r", col_fix[node_id].message)
info("GNSS aux: %r", col_aux[node_id].message)
info("Magnetic field [Ga]: %r", col_mag[node_id].message)
info("Pressure [Pa]: %r", col_pressure[node_id].message)
info("Temperature [K]: %r", col_temp[node_id].message)
# Finalizing the test
info("Resetting the configuration to factory default...")
enforce(
request(
uavcan.protocol.param.ExecuteOpcode.Request( # @UndefinedVariable
opcode=uavcan.protocol.param.ExecuteOpcode.Request().OPCODE_ERASE
)
).ok, # @UndefinedVariable
"Could not erase configuration",
)
request(
uavcan.protocol.RestartNode.Request( # @UndefinedVariable
magic_number=uavcan.protocol.RestartNode.Request().MAGIC_NUMBER
), # @UndefinedVariable
fire_and_forget=True,
)
with BackgroundSpinner(safe_spin, 0.1):
wait_for_boot()
wait_for_init()
check_status()
log_all_params()
except Exception:
for nid in nsmon.get_all_node_id():
print("Node state: %r" % nsmon.get(nid))
raise
# Blocking questions are moved out of the node scope because blocking breaks CAN communications
if not input("Is the PPS LED blinking once per second?", yes_no=True, default_answer=True):
abort("PPS LED is not working")
if not input("Is the CAN1 LED blinking or glowing solid?", yes_no=True, default_answer=True):
abort("CAN1 LED is not working (however the interface is fine)")
if not input("Is the STATUS LED blinking once per second?", yes_no=True, default_answer=True):
abort("STATUS LED is not working")
# Testing CAN2
input("1. Disconnect CAN1 and connect to CAN2\n" "2. Terminate CAN2\n" "3. Press ENTER")
if not input("Is the CAN2 LED blinking or glowing solid?", yes_no=True, default_answer=True):
abort("Either CAN2 or its LED are not working")
