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 process_one_device(set_device_info):
execute_shell_command('ifconfig %s down && ifconfig %s up', args.iface,
args.iface)
if not args.only_sign:
# Flashing firmware without signature
with closing(bootloader.BootloaderInterface(args.iface)) as bli:
input('\n'.join([
'1. Set PIO0_3 low, PIO0_1 low (J4 closed, J3 open)',
'2. Power on the device and connect it to CAN bus',
'3. Press ENTER'
]))
with CLIWaitCursor():
load_and_start_firmware(bli, firmware_base)
# Testing the device
input('\n'.join([
'1. Make sure that LED indicators are blinking',
'2. Test button operation', '3. Press ENTER'
]))
# Installing signature
with closing(bootloader.BootloaderInterface(args.iface)) as bli:
info("Now we're going to sign the device")
input('\n'.join([
'1. Set PIO0_3 low, PIO0_1 low (J4 closed, J3 open)',
'2. Reset the device (e.g. cycle power)', '3. Press ENTER'
]))
with CLIWaitCursor():
info('Reading unique ID...')
unique_id = bli.read_unique_id()
set_device_info(PRODUCT_NAME, unique_id)
info('Requesting signature for unique ID %s',
' '.join(['%02x' % x for x in unique_id]))
gensign_response = api.generate_signature(unique_id, PRODUCT_NAME)
info(
'Signature has been generated successfully [%s], patching the firmware...',
['existing', 'NEW'][gensign_response.new])
firmware_with_signature = firmware_base.ljust(
SIGNATURE_OFFSET, b'\xFF') + gensign_response.signature
load_and_start_firmware(bli, firmware_with_signature)
def process_one_device(set_device_info):
execute_shell_command('ifconfig %s down && ifconfig %s up', args.iface, args.iface)
if not args.only_sign:
# Flashing firmware without signature
with closing(bootloader.BootloaderInterface(args.iface)) as bli:
input('\n'.join(['1. Set PIO0_3 low, PIO0_1 low (J4 closed, J3 open)',
'2. Power on the device and connect it to CAN bus',
'3. Press ENTER']))
with CLIWaitCursor():
load_and_start_firmware(bli, firmware_base)
# Testing the device
input('\n'.join(['1. Make sure that LED indicators are blinking',
'2. Test button operation',
'3. Press ENTER']))
# Installing signature
with closing(bootloader.BootloaderInterface(args.iface)) as bli:
info("Now we're going to sign the device")
input('\n'.join(['1. Set PIO0_3 low, PIO0_1 low (J4 closed, J3 open)',
'2. Reset the device (e.g. cycle power)',
'3. Press ENTER']))
with CLIWaitCursor():
info('Reading unique ID...')
unique_id = bli.read_unique_id()
set_device_info(PRODUCT_NAME, unique_id)
info('Requesting signature for unique ID %s', ' '.join(['%02x' % x for x in unique_id]))
gensign_response = api.generate_signature(unique_id, PRODUCT_NAME)
info('Signature has been generated successfully [%s], patching the firmware...',
['existing', 'NEW'][gensign_response.new])
firmware_with_signature = firmware_base.ljust(SIGNATURE_OFFSET, b'\xFF') + gensign_response.signature
load_and_start_firmware(bli, firmware_with_signature)
def process_one_device(set_device_info):
out = input('1. Connect DroneCode Probe to the debug connector.\n'
'2. Connect CAN to the first CAN1 connector on the device; terminate the other CAN1 connector.\n'
'4. Connect an appropriate power supply (see the hardware specs for requirements).\n'
' Make sure the motor leads are NOT CONNECTED to anything.\n'
'5. If you want to skip firmware upload, type F.\n'
'6. Press ENTER.')
skip_fw_upload = 'f' in out.lower()
if not skip_fw_upload:
info('Loading the firmware')
with CLIWaitCursor():
load_firmware_via_gdb(firmware_data,
toolchain_prefix=TOOLCHAIN_PREFIX,
load_offset=FLASH_OFFSET,
gdb_port=glob_one(DEBUGGER_PORT_GDB_GLOB),
gdb_monitor_scan_command='swdp_scan')
else:
info('Firmware upload skipped, rebooting the device')
with open_serial_port(DEBUGGER_PORT_CLI_GLOB) as io:
SerialCLI(io, 0.1).write_line_and_read_output_lines_until_timeout('reboot')
info('Waiting for the device to boot...')
wait_for_boot()
with open_serial_port(DEBUGGER_PORT_CLI_GLOB) as io:
info('Identifying the connected device...')
cli = SerialCLI(io, 0.1)
cli.flush_input(0.5)
zubax_id = read_zubax_id(cli)
unique_id = b64decode(zubax_id['hw_unique_id'])
product_id = zubax_id['product_id']
set_device_info(product_id, unique_id)
with BackgroundCLIListener(io, lambda line: cli_logger.info(repr(line))):
input('Connect a motor WITHOUT ANY LOAD ATTACHED to the ESC, then press ENTER.\n'
'CAUTION: THE MOTOR WILL SPIN')
test_uavcan()
try:
# Using first command to get rid of any garbage lingering in the buffers
cli.write_line_and_read_output_lines_until_timeout('systime')
except Exception:
pass
# Getting the signature
info('Requesting signature for unique ID %s', binascii.hexlify(unique_id).decode())
gensign_response = licensing_api.generate_signature(unique_id, PRODUCT_NAME)
if gensign_response.new:
info('New signature has been generated')
else:
info('This particular device has been signed earlier, reusing existing signature')
base64_signature = b64encode(gensign_response.signature).decode()
logger.info('Generated signature in Base64: %s', base64_signature)
# Installing the signature; this may fail if the device has been signed earlier - the failure will be ignored
out = cli.write_line_and_read_output_lines_until_timeout('zubax_id %s', base64_signature)
logger.debug('Signature installation response (may fail, which is OK): %r', out)
# Reading the signature back and verifying it
installed_signature = read_zubax_id(cli)['hw_signature']
logger.info('Installed signature in Base64: %s', installed_signature)
enforce(b64decode(installed_signature) == gensign_response.signature,
'Written signature does not match the generated signature')
info('Signature has been installed and verified')
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 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 process_one_device(set_device_info):
out = input(
'1. Connect DroneCode Probe to the debug connector\n'
'2. Connect CAN to the first CAN1 connector on the device; terminate the other CAN1 connector\n'
'3. Connect USB to the device, and make sure that no other Zubax GNSS is connected\n'
'4. If you want to skip firmware upload, type F\n'
'5. Press ENTER')
skip_fw_upload = 'f' in out.lower()
if not skip_fw_upload:
info('Loading the firmware')
with CLIWaitCursor():
load_firmware(firmware_data)
info('Waiting for the device to boot...')
wait_for_boot()
else:
info('Firmware upload skipped')
info('Identifying the connected device...')
with open_serial_port(USB_CDC_ACM_GLOB, wait_for_port=5) as io:
logger.info('USB CLI is on %r', io.port)
zubax_id = SerialCLI(io, 0.1).read_zubax_id()
product_id = zubax_id['product_id']
unique_id = b64decode(zubax_id['hw_unique_id'])
set_device_info(product_id, unique_id)
info('Testing UAVCAN interface...')
test_uavcan()
info('Connecting via USB...')
with open_serial_port(USB_CDC_ACM_GLOB, wait_for_port=5) as io:
logger.info('USB CLI is on %r', io.port)
cli = SerialCLI(io, 0.1)
enforce(unique_id == b64decode(cli.read_zubax_id()['hw_unique_id']),
'UID changed!')
try:
# Using first command to get rid of any garbage lingering in the buffers
cli.write_line_and_read_output_lines_until_timeout('systime')
except Exception:
pass
# Getting the signature
info('Requesting signature for unique ID %s',
binascii.hexlify(unique_id).decode())
gensign_response = licensing_api.generate_signature(
unique_id, PRODUCT_NAME)
if gensign_response.new:
info('New signature has been generated')
else:
info(
'This particular device has been signed earlier, reusing existing signature'
)
base64_signature = b64encode(gensign_response.signature).decode()
logger.info('Generated signature in Base64: %s', base64_signature)
# Installing the signature; this may fail if the device has been signed earlier - the failure will be ignored
out = cli.write_line_and_read_output_lines_until_timeout(
'signature %s', base64_signature)
logger.debug(
'Signature installation response (may fail, which is OK): %r', out)
# Reading the signature back and verifying it
out = cli.write_line_and_read_output_lines_until_timeout('signature')
enforce(
len(out) == 1,
'Could not read the signature back. Returned lines: %r', out)
logger.info('Installed signature in Base64: %s', out[0])
enforce(
b64decode(out[0]) == gensign_response.signature,
'Written signature does not match the generated signature')
info('Signature has been installed and verified')
def process_one_device(set_device_info):
out = input('1. Connect DroneCode Probe to the debug connector.\n'
'2. Connect CAN to the first CAN1 connector on the device; terminate the other CAN1 connector.\n'
'3. Connect USB to the device, and make sure that no other Zubax GNSS is connected.\n'
'4. If you want to skip firmware upload, type F.\n'
'5. Press ENTER.')
skip_fw_upload = 'f' in out.lower()
if not skip_fw_upload:
info('Loading the firmware')
with CLIWaitCursor():
load_firmware(firmware_data)
info('Waiting for the device to boot...')
wait_for_boot()
else:
info('Firmware upload skipped')
info('Identifying the connected device...')
with open_serial_port(USB_CDC_ACM_GLOB, wait_for_port=5) as io:
logger.info('USB CLI is on %r', io.port)
zubax_id = SerialCLI(io, 0.1).read_zubax_id()
product_id = zubax_id['product_id']
unique_id = b64decode(zubax_id['hw_unique_id'])
set_device_info(product_id, unique_id)
info('Testing the UAVCAN interface...')
test_uavcan()
info('Connecting via USB...')
with open_serial_port(USB_CDC_ACM_GLOB, wait_for_port=5) as io:
logger.info('USB CLI is on %r', io.port)
cli = SerialCLI(io, 0.1)
enforce(unique_id == b64decode(cli.read_zubax_id()['hw_unique_id']), 'UID changed!')
try:
# Using first command to get rid of any garbage lingering in the buffers
cli.write_line_and_read_output_lines_until_timeout('systime')
except Exception:
pass
# Getting the signature
info('Requesting signature for unique ID %s', binascii.hexlify(unique_id).decode())
gensign_response = licensing_api.generate_signature(unique_id, PRODUCT_NAME)
if gensign_response.new:
info('New signature has been generated')
else:
info('This particular device was signed earlier, reusing the existing signature')
base64_signature = b64encode(gensign_response.signature).decode()
logger.info('Generated signature in Base64: %s', base64_signature)
# Installing the signature; this may fail if the device has been signed earlier - the failure will be ignored
out = cli.write_line_and_read_output_lines_until_timeout('signature %s', base64_signature)
logger.debug('Signature installation response (may fail, which is OK): %r', out)
# Reading the signature back and verifying it
out = cli.write_line_and_read_output_lines_until_timeout('signature')
enforce(len(out) == 1, 'Could not read the signature back. Returned lines: %r', out)
logger.info('Installed signature in Base64: %s', out[0])
enforce(b64decode(out[0]) == gensign_response.signature,
'Written signature does not match the generated signature')
info('Signature has been installed and verified')
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 process_one_device(set_device_info):
out = input(
'1. Connect DroneCode Probe to the debug connector.\n'
'2. Connect CAN to the first CAN connector on the Babel; leave the other CAN connector empty.\n'
'3. Connect USB to the Micro USB port on the Babel.\n'
'4. If you want to skip firmware upload, type F now.\n'
'5. Press ENTER.')
skip_fw_upload = 'f' in out.lower()
if not skip_fw_upload:
info('Loading the firmware')
with CLIWaitCursor():
try:
load_firmware_via_gdb(
firmware_data,
toolchain_prefix=TOOLCHAIN_PREFIX,
load_offset=FLASH_OFFSET,
gdb_port=glob_one(DEBUGGER_PORT_GDB_GLOB),
gdb_monitor_scan_command='swdp_scan')
except Exception as ex:
logging.info('Firmware load error', exc_info=True)
fatal(
'Could not load firmware; check the debug connector; error: %r',
ex)
else:
info('Firmware upload skipped')
info('Waiting for the device to boot...')
wait_for_boot()
with closing(
drwatson.can.SLCAN(get_target_serial_port_symlink(),
bitrate=CAN_BITRATE,
default_timeout=1)) as drv_target:
info('Reading Zubax ID...')
zubax_id = read_zubax_id(drv_target)
unique_id = b64decode(zubax_id['hw_unique_id'])
product_id = zubax_id['product_id']
assert PRODUCT_NAME == product_id
set_device_info(product_id, unique_id)
info('Configuring the adapter...')
drv_target.execute_cli_command(
'cfg set can.terminator_on 1') # Terminator ON
logger.info('Adapter state:\n%s',
drv_target.execute_cli_command('stat'))
with closing(uavcan.driver.make_driver(
args.iface, bitrate=CAN_BITRATE)) as drv_test:
random_frames = [
make_random_can_frame() for _ in range(NUM_TEST_FRAMES)
]
# If we're using another Zubax Babel as a CAN adapter, this command will disable its CAN power output.
# Otherwise the command will silently fail. The CAN power output must be disabled because it interferes
# with the power supply delivery testing.
try:
drv_test.execute_cli_command('cfg set can.power_on 0',
lambda _: None)
except Exception as ex:
logger.debug('CAN adapter CLI command failed: %s', ex)
info('Testing CAN bus exchange: target --> test')
for idx, rf in enumerate(random_frames):
drv_target.send(rf.id, rf.data, rf.extended)
received = drv_test.receive(1)
enforce(received is not None,
'Timeout when trying to receive frame %d', idx + 1)
enforce(
received.extended == rf.extended
and received.data == rf.data and received.id == rf.id,
'Received frame %d [%r] does not match the reference [%r]',
idx + 1, received, rf)
info('Testing CAN bus exchange: test --> target')
for idx, rf in enumerate(random_frames):
drv_test.send(rf.id, rf.data, rf.extended)
try:
received = drv_target.receive(1)
except TimeoutError:
fatal('Timeout when trying to receive frame %d', idx + 1)
enforce(
received['ext'] == rf.extended
and received['data'] == rf.data
and received['id'] == rf.id,
'Received frame %d [%r] does not match the reference [%r]',
idx + 1, received, rf)
info('Test exchange OK (2x%d frames)', len(random_frames))
info('Testing power supply...')
drv_target.execute_cli_command(
'cfg set can.power_on 0') # Bus power OFF
time.sleep(2)
stat = yaml.load(drv_target.execute_cli_command('stat'))
enforce(
BUS_VOLTAGE_RANGE_OFF[0] <= stat['bus_voltage'] <=
BUS_VOLTAGE_RANGE_OFF[1],
'Invalid voltage on the bus (power is turned OFF): %r volts; '
'there may be a short circuit on the board',
stat['bus_voltage'])
info('Bus voltage: %r', stat['bus_voltage'])
drv_target.execute_cli_command(
'cfg set can.power_on 1') # Bus power ON
time.sleep(2)
stat = yaml.load(drv_target.execute_cli_command('stat'))
enforce(
BUS_VOLTAGE_RANGE_ON[0] <= stat['bus_voltage'] <=
BUS_VOLTAGE_RANGE_ON[1],
'Invalid voltage on the bus (power is turned ON): %r volts; '
'the power supply circuit is malfunctioning',
stat['bus_voltage'])
info('Bus voltage: %r', stat['bus_voltage'])
info('Power supply is OK')
info('Testing LED indicators...')
# LED1 - CAN Power - Red
# LED2 - Terminator ON - Orange
# LED3 - Status - Blue
# LED4 - CAN Activity - Green
enforce(input('Is LED1 (CAN power, RED) turned on?', yes_no=True),
'CAN Power LED is not working')
enforce(
input('Is LED2 (terminator, ORANGE) turned on?', yes_no=True),
'Terminator and/or its LED are not working')
enforce(
input('Is LED3 (status, BLUE) blinking about once a second?',
yes_no=True), 'Status LED is not working')
def generate_traffic():
drv_target.send(0, b'', False)
time.sleep(0.2)
with BackgroundSpinner(generate_traffic):
enforce(
input('Is LED4 (activity, GREEN) blinking quickly?',
yes_no=True),
'Activity LED is not working, or the bus has been disconnected'
)
info('Resetting configuration to factory defaults...')
drv_target.execute_cli_command('cfg erase')
info('Installing signature...')
# Getting the signature
info('Requesting signature for unique ID %s',
binascii.hexlify(unique_id).decode())
gen_sign_response = licensing_api.generate_signature(
unique_id, PRODUCT_NAME)
if gen_sign_response.new:
info('New signature has been generated')
else:
info(
'This particular device was signed earlier, reusing the existing signature'
)
base64_signature = b64encode(gen_sign_response.signature).decode()
logger.info('Generated signature in Base64: %s', base64_signature)
# Installing the signature; this may fail if the device has been signed earlier - the failure will be ignored
out = drv_target.execute_cli_command('zubax_id %s' % base64_signature)
logger.debug(
'Signature installation response (may fail, which is OK): %r', out)
# Reading the signature back and verifying it
installed_signature = read_zubax_id(drv_target)['hw_signature']
logger.info('Installed signature in Base64: %s', installed_signature)
enforce(
b64decode(installed_signature) == gen_sign_response.signature,
'Written signature does not match the generated signature')
info('Signature has been installed and verified')
def process_one_device():
out = input(
"1. Connect DroneCode Probe to the debug connector\n"
"2. Connect CAN to the first CAN1 connector on the device; terminate the other CAN1 connector\n"
"3. Connect USB to the device, and make sure that no other Zubax GNSS is connected\n"
"4. If you want to skip firmware upload, type F\n"
"5. Press ENTER"
)
skip_fw_upload = "f" in out.lower()
if not skip_fw_upload:
info("Loading the firmware")
with CLIWaitCursor():
load_firmware(firmware_data)
info("Waiting for the board to boot...")
wait_for_boot()
else:
info("Firmware upload skipped")
info("Testing UAVCAN interface...")
test_uavcan()
input(
"Now we're going to test USB. If this application is running on a virtual "
"machine, make sure that the corresponsing USB device is made available for "
"the VM, then press ENTER."
)
info("Connecting via USB...")
with open_serial_port(USB_CDC_ACM_GLOB) as io:
logger.info("USB CLI is on %r", io.port)
cli = SerialCLI(io, 0.1)
cli.flush_input(0.5)
try:
# Using first command to get rid of any garbage lingering in the buffers
cli.write_line_and_read_output_lines_until_timeout("systime")
except Exception:
pass
zubax_id = cli.write_line_and_read_output_lines_until_timeout("zubax_id")
zubax_id = yaml.load("\n".join(zubax_id))
logger.info("Zubax ID: %r", zubax_id)
unique_id = b64decode(zubax_id["hw_unique_id"])
# Getting the signature
info("Requesting signature for unique ID %s", binascii.hexlify(unique_id).decode())
gensign_response = licensing_api.generate_signature(unique_id, PRODUCT_NAME)
if gensign_response.new:
info("New signature has been generated")
else:
info("This particular device has been signed earlier, reusing existing signature")
base64_signature = b64encode(gensign_response.signature).decode()
logger.info("Generated signature in Base64: %s", base64_signature)
# Installing the signature; this may fail if the device has been signed earlier - the failure will be ignored
out = cli.write_line_and_read_output_lines_until_timeout("signature %s", base64_signature)
logger.debug("Signature installation response (may fail, which is OK): %r", out)
# Reading the signature back and verifying it
out = cli.write_line_and_read_output_lines_until_timeout("signature")
enforce(len(out) == 1, "Could not read the signature back. Returned lines: %r", out)
logger.info("Installed signature in Base64: %s", out[0])
enforce(
b64decode(out[0]) == gensign_response.signature, "Written signature does not match the generated signature"
)
info("Signature has been installed and verified")
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")