def check_latest_node_info(): curr_node_info = node.monitor.get(device_node_id).info logger.info( 'Comparing node info, original vs. current:\n%s\n---\n%s', uavcan.to_yaml(orig_node_info), uavcan.to_yaml(curr_node_info)) _enforce( orig_node_info.hardware_version.major == curr_node_info.hardware_version.major, 'HW version major') _enforce( orig_node_info.hardware_version.minor == curr_node_info.hardware_version.minor, 'HW version minor') _enforce( orig_node_info.hardware_version.unique_id == curr_node_info.hardware_version.unique_id, 'HW UID') _enforce( orig_node_info.hardware_version.certificate_of_authenticity == curr_node_info.hardware_version. certificate_of_authenticity, 'HW COA') _enforce(orig_node_info.name == curr_node_info.name, 'Node name')
def param_opcode_response(event): nonlocal configuration_finished if not event: raise Exception('Request timed out') print(uavcan.to_yaml(event)) if not event.response.ok: raise Exception('Param opcode execution rejected\n' + uavcan.to_yaml(event)) else: configuration_finished = True
def request(self, payload, dest_node_id, timeout=None, priority=None): """ Synchronous request. Throws a TimeoutException on timeout. """ result = None def callback(event): nonlocal result if event is not None: logger.debug('Response %r from %d\n%s', uavcan.get_uavcan_data_type(event.response), event.transfer.source_node_id, uavcan.to_yaml(event.response)) assert event.transfer.source_node_id == dest_node_id result = event.response else: raise Node.TimeoutException('Request to node %d with payload %r has timed out in %.3f seconds' % (dest_node_id, payload, timeout or uavcan.node.DEFAULT_SERVICE_TIMEOUT)) logger.debug('Synchronously requesting %r from %d\n%s', uavcan.get_uavcan_data_type(payload), dest_node_id, uavcan.to_yaml(payload)) self.request_async(payload, dest_node_id, callback, timeout, priority) while result is None: self.spin_for(0.1) return result
def request(self, payload, dest_node_id, timeout=None, priority=None): """ Synchronous request. Throws a TimeoutException on timeout. """ result = None def callback(event): nonlocal result if event is not None: logger.debug('Response %r from %d\n%s', uavcan.get_uavcan_data_type(event.response), event.transfer.source_node_id, uavcan.to_yaml(event.response)) assert event.transfer.source_node_id == dest_node_id result = event.response else: raise Node.TimeoutException( 'Request to node %d with payload %r has timed out in %.3f seconds' % (dest_node_id, payload, timeout or uavcan.node.DEFAULT_SERVICE_TIMEOUT)) logger.debug('Synchronously requesting %r from %d\n%s', uavcan.get_uavcan_data_type(payload), dest_node_id, uavcan.to_yaml(payload)) self.request_async(payload, dest_node_id, callback, timeout, priority) while result is None: self.spin_for(0.1) return result
def decode_transfer_from_frame(entry_row, row_to_frame): entry_frame = row_to_frame(entry_row) can_id = entry_frame.id transfer_id = _get_transfer_id(entry_frame) frames = [entry_frame] related_rows = [] # Scanning backward looking for the first frame row = entry_row - 1 while not _is_start_of_transfer(frames[0]): if row < 0 or entry_row - row > TABLE_TRAVERSING_RANGE: raise DecodingFailedException('SOT not found') f = row_to_frame(row) row -= 1 if f.id == can_id and _get_transfer_id(f) == transfer_id: frames.insert(0, f) related_rows.insert(0, row) # Scanning forward looking for the last frame row = entry_row + 1 while not _is_end_of_transfer(frames[-1]): f = row_to_frame(row) if f is None or row - entry_row > TABLE_TRAVERSING_RANGE: raise DecodingFailedException('EOT not found') row += 1 if f.id == can_id and _get_transfer_id(f) == transfer_id: frames.append(f) related_rows.append(row) # The transfer is now fully recovered tr = Transfer() tr.from_frames([Frame(x.id, x.data) for x in frames]) return related_rows, uavcan.to_yaml(tr.payload)
def indication_callback(event): nonlocal received_indication if event.transfer.source_node_id in enumerated_nodes: print( 'Indication callback from node %d ignored - already enumerated' % event.transfer.source_node_id) else: print(uavcan.to_yaml(event)) received_indication = event
def begin_response_checker(event): nonlocal begin_responses_succeeded if not event: raise Exception('Request timed out') if event.response.error != event.response.ERROR_OK: raise Exception('Enumeration rejected\n' + uavcan.to_yaml(event)) begin_responses_succeeded += 1
def response_func(event): nonlocal response_received nonlocal is_valid if not event: raise Exception('Request timed out') response_received = True if not event.response.ok: raise Exception('Param opcode execution rejected\n' + uavcan.to_yaml(event)) else: is_valid = True
def param_set_response(event): if not event: raise Exception('Request timed out') assert event.response.name == received_indication.message.parameter_name assert event.response.value.integer_value == next_index print(uavcan.to_yaml(event)) node.request( uavcan.protocol.param.ExecuteOpcode.Request( opcode=uavcan.protocol.param.ExecuteOpcode.Request( ).OPCODE_SAVE), target_node_id, param_opcode_response)
def callback(event): nonlocal result if event is not None: logger.debug('Response %r from %d\n%s', uavcan.get_uavcan_data_type(event.response), event.transfer.source_node_id, uavcan.to_yaml(event.response)) assert event.transfer.source_node_id == dest_node_id result = event.response else: raise Node.TimeoutException('Request to node %d with payload %r has timed out in %.3f seconds' % (dest_node_id, payload, timeout or uavcan.node.DEFAULT_SERVICE_TIMEOUT))
def callback(event): nonlocal result if event is not None: logger.debug('Response %r from %d\n%s', uavcan.get_uavcan_data_type(event.response), event.transfer.source_node_id, uavcan.to_yaml(event.response)) assert event.transfer.source_node_id == dest_node_id result = event.response else: raise Node.TimeoutException( 'Request to node %d with payload %r has timed out in %.3f seconds' % (dest_node_id, payload, timeout or uavcan.node.DEFAULT_SERVICE_TIMEOUT))
def check_latest_node_info(): curr_node_info = node.monitor.get(device_node_id).info logger.info('Comparing node info, original vs. current:\n%s\n---\n%s', uavcan.to_yaml(orig_node_info), uavcan.to_yaml(curr_node_info)) _enforce(orig_node_info.hardware_version.major == curr_node_info.hardware_version.major, 'HW version major') _enforce(orig_node_info.hardware_version.minor == curr_node_info.hardware_version.minor, 'HW version minor') _enforce(orig_node_info.hardware_version.unique_id == curr_node_info.hardware_version.unique_id, 'HW UID') _enforce(orig_node_info.hardware_version.certificate_of_authenticity == curr_node_info.hardware_version.certificate_of_authenticity, 'HW COA') _enforce(orig_node_info.name == curr_node_info.name, 'Node name')
def _do_broadcast(self): try: if not self._pause.isChecked(): msg = uavcan.equipment.esc.RawCommand() for sl in self._sliders: raw_value = sl.get_value() / 100 value = (-self.CMD_MIN if raw_value < 0 else self.CMD_MAX) * raw_value msg.cmd.append(int(value)) self._node.broadcast(msg) self._msg_viewer.setPlainText(uavcan.to_yaml(msg)) else: self._msg_viewer.setPlainText("Paused") except Exception as ex: self._msg_viewer.setPlainText("Publishing failed:\n" + str(ex))
def listen(self, msg): """ Transfer( id=4, source_node_id=125, dest_node_id=None, transfer_priority=7, payload=uavcan.equipment.esc.Status( error_count=0, voltage=12.9296875, current=-0.0, temperature=307.0, rpm=0, power_rating_pct=0, esc_index=0) ) """ self.last_raw = uavcan.to_yaml(msg) self.last_msg = yaml.load(self.last_raw) if self.last_msg.get('esc_index', None) == 0: self.rpm = (self.rpm + [self.last_msg.get('rpm', 0)])[-10:] self.last_rpm = round(sum(self.rpm) / len(self.rpm))
def _do_broadcast(self): try: if not self._pause.isChecked(): msg = uavcan.equipment.esc.RawCommand() for sl in self._sliders: raw_value = sl.get_value() / 100 value = (-self.CMD_MIN if raw_value < 0 else self.CMD_MAX) * raw_value msg.cmd.append(int(value)) self._node.broadcast(msg) self._msg_viewer.setPlainText(uavcan.to_yaml(msg)) else: self._msg_viewer.setPlainText('Paused') except Exception as ex: self._msg_viewer.setPlainText('Publishing failed:\n' + str(ex))
def callback(e): if e is None: self.window().show_message("Transport stats request timed out") else: text = uavcan.to_yaml(e.response) win = QDialog(self) view = QPlainTextEdit(win) view.setReadOnly(True) view.setFont(get_monospace_font()) view.setPlainText(text) view.setLineWrapMode(QPlainTextEdit.NoWrap) layout = QVBoxLayout(win) layout.addWidget(view) win.setModal(True) win.setWindowTitle("Transport stats of node %r" % e.transfer.source_node_id) win.setLayout(layout) win.show()
def callback(e): if e is None: self.window().show_message('Transport stats request timed out') else: text = uavcan.to_yaml(e.response) win = QDialog(self) view = QPlainTextEdit(win) view.setReadOnly(True) view.setFont(get_monospace_font()) view.setPlainText(text) view.setLineWrapMode(QPlainTextEdit.NoWrap) layout = QVBoxLayout(win) layout.addWidget(view) win.setModal(True) win.setWindowTitle('Transport stats of node %r' % e.transfer.source_node_id) win.setLayout(layout) win.show()
def param_getset_response(event): nonlocal response_received nonlocal is_valid nonlocal param_dict nonlocal return_yaml nonlocal param_yaml if not event: raise Exception('Request timed out') response_received = True is_valid = not hasattr(event.transfer.payload.value,'empty') if is_valid: #print(event) #print(uavcan.to_yaml(event)) if return_yaml: param_yaml = uavcan.to_yaml(event) else: param_dict['name'] = str(event.transfer.payload.name) if hasattr(event.transfer.payload.value,'integer_value'): param_dict['value'] = event.transfer.payload.value.integer_value if hasattr(event.transfer.payload.value,'real_value'): param_dict['value'] = event.transfer.payload.value.real_value
def _on_message(self, e): # Global statistics self._num_messages_total += 1 # Rendering and filtering try: text = uavcan.to_yaml(e) if not self._apply_filter(text): return except Exception as ex: self._num_errors += 1 text = '!!! [%d] MESSAGE PROCESSING FAILED: %s' % (self._num_errors, ex) else: self._num_messages_past_filter += 1 self._msgs_per_sec_estimator.register_event(e.transfer.ts_monotonic) # Sending the text for later rendering try: self._message_queue.put_nowait(text) except queue.Full: pass
def _on_message(self, e): # Global statistics self._num_messages_total += 1 # Rendering and filtering try: text = uavcan.to_yaml(e) if not self._apply_filter(text): return except Exception as ex: self._num_errors += 1 text = '!!! [%d] MESSAGE PROCESSING FAILED: %s' % ( self._num_errors, ex) else: self._num_messages_past_filter += 1 self._msgs_per_sec_estimator.register_event( e.transfer.ts_monotonic) # Sending the text for later rendering try: self._message_queue.put_nowait(text) except queue.Full: pass
def tunnel_callback(event): print("received uavcan.tunnel.Broadcast message:") print(uavcan.to_yaml(event))
def print_yaml(obj): """ Formats the argument as YAML structure using uavcan.to_yaml(), and prints the result into stdout. Use this function to print received UAVCAN structures. """ print(uavcan.to_yaml(obj))
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_once(self, bitrate): logger.info('Instantiating a new node with bitrate %d bps', bitrate) node = Node(self._can_iface_name, bitrate, self._can_dump_directory, [self._valid_firmware_dir, self._invalid_firmware_dir], self._self_node_id) with closing(node): node.monitor.add_update_handler(lambda e: logger.debug('Node monitor event: %s', e)) logger.info('Waiting for the device to appear online...') device_node_id = 0 def is_dut_online(): nonlocal device_node_id def predicate(e: uavcan.app.node_monitor.NodeMonitor.Entry): return (e.info is not None) and bytes(e.info.hardware_version.unique_id) == self._device_uid matches = list(node.monitor.find_all(predicate)) if len(matches) > 0: device_node_id = matches[0].node_id return True return False node.spin_until(is_dut_online) logger.info('Device online, NID %d', device_node_id) orig_node_info = node.monitor.get(device_node_id).info def get_node_status(): try: e = node.monitor.get(device_node_id) except KeyError: raise TestException('Node is offline') return e.status def match_node_health_mode(health, mode): ns = get_node_status() if health is not None: return ns.mode == mode and ns.health == health else: return ns.mode == mode # Optionally stopping the allocator if _maybe(): node.node_id_allocator.close() node.node_id_allocator = None # Selecting the firmware, possibly invalid using_valid_firmware = _maybe() fw_path = _choose_random_path_from_directory(self._valid_firmware_dir if using_valid_firmware else self._invalid_firmware_dir, '*.bin') logger.info('Using firmware %r, which is %s', fw_path, 'VALID' if using_valid_firmware else 'INVALID') # Requesting a firmware update; about ~50% chance of providing an incorrect server node ID begin_fw_update_request = uavcan.protocol.file.BeginFirmwareUpdate.Request() begin_fw_update_request.source_node_id = random.choice(list(range(1, 128)) + [node.node_id] * 128) using_valid_source_node_id = begin_fw_update_request.source_node_id in (0, node.node_id) begin_fw_update_request.image_file_remote_path.path = fw_path logger.info('Sending BeginFirmwareUpdate with source NID %d, which is %s', begin_fw_update_request.source_node_id, 'VALID' if using_valid_source_node_id else 'INVALID') response = node.request(begin_fw_update_request, device_node_id) logger.info('BeginFirmwareUpdate response: %s', response) # Spinning for a while in order to update the node status representation locally node.spin_for(5) logger.info('Latest NodeStatus\n%s', uavcan.to_yaml(get_node_status())) # Making sure that NodeInfo values reported by the bootloader and by the application are consistent def check_latest_node_info(): curr_node_info = node.monitor.get(device_node_id).info logger.info('Comparing node info, original vs. current:\n%s\n---\n%s', uavcan.to_yaml(orig_node_info), uavcan.to_yaml(curr_node_info)) _enforce(orig_node_info.hardware_version.major == curr_node_info.hardware_version.major, 'HW version major') _enforce(orig_node_info.hardware_version.minor == curr_node_info.hardware_version.minor, 'HW version minor') _enforce(orig_node_info.hardware_version.unique_id == curr_node_info.hardware_version.unique_id, 'HW UID') _enforce(orig_node_info.hardware_version.certificate_of_authenticity == curr_node_info.hardware_version.certificate_of_authenticity, 'HW COA') _enforce(orig_node_info.name == curr_node_info.name, 'Node name') check_latest_node_info() # Checking what state the bootloader is in if match_node_health_mode(None, NODE_STATUS_CONST.MODE_OPERATIONAL): raise TestException('The node did not enter firmware update mode') if using_valid_source_node_id: _enforce(match_node_health_mode(NODE_STATUS_CONST.HEALTH_OK, NODE_STATUS_CONST.MODE_SOFTWARE_UPDATE) or match_node_health_mode(NODE_STATUS_CONST.HEALTH_ERROR, NODE_STATUS_CONST.MODE_SOFTWARE_UPDATE), 'Expected update mode') # Continuing with other tests else: _enforce(match_node_health_mode(NODE_STATUS_CONST.HEALTH_WARNING, NODE_STATUS_CONST.MODE_MAINTENANCE) or match_node_health_mode(NODE_STATUS_CONST.HEALTH_ERROR, NODE_STATUS_CONST.MODE_SOFTWARE_UPDATE), 'Expected failure') _enforce(get_node_status().vendor_specific_status_code == BootloaderError.UAVCAN_TIMEOUT, 'Expected failure') # Maybe restart, maybe not... if _maybe(): logger.info('Sending RestartRequest') r = uavcan.protocol.RestartNode.Request() r.magic_number = r.MAGIC_NUMBER response = node.request(r, device_node_id) _enforce(response.ok, 'RestartNode was supposed to return ok=True') return # Nothing more to test at this point # Waiting for the bootloader to complete downloading, then check the outcome logger.info('Waiting for the download to complete...') deadline = time.monotonic() + 3600 # An hour - a ridiculously large deadline, but whatever def poll(): _enforce(time.monotonic() < deadline, 'Stuck. :(') if _maybe(): # Generating garbage on the bus, because why not if _maybe(): r = uavcan.protocol.RestartNode.Request() # Invalid magic, this is intentional elif _maybe(0.01): r = uavcan.protocol.GetNodeInfo.Request() else: # The bootloader doesn't support this service, there will be no response r = uavcan.protocol.file.Read.Request() r.offset = int(random.random() * 1024 * 1024 * 1024) r.path.path = \ 'Senora, pray receive with your wonted kindness Senor Don Quixote of La Mancha, '\ 'whom you see before you, a knight-errant, and the bravest and wisest in the world.' node.request_async(r, device_node_id) return not match_node_health_mode(NODE_STATUS_CONST.HEALTH_OK, NODE_STATUS_CONST.MODE_SOFTWARE_UPDATE) node.spin_until(poll, check_interval=0.01) logger.info('Download finished, checking the outcome...') if using_valid_firmware: _enforce(match_node_health_mode(NODE_STATUS_CONST.HEALTH_OK, NODE_STATUS_CONST.MODE_MAINTENANCE), 'Expected successful completion') logger.info('Waiting for the board to boot...') node.spin_for(15) _enforce(match_node_health_mode(None, NODE_STATUS_CONST.MODE_OPERATIONAL), 'Application did not boot') check_latest_node_info() else: _enforce(match_node_health_mode(NODE_STATUS_CONST.HEALTH_ERROR, NODE_STATUS_CONST.MODE_SOFTWARE_UPDATE), 'Expected failure')
def logging_callback(event): logger.info('uavcan.protocol.debug.LogMessage\n%s', uavcan.to_yaml(event))
if __name__ == '__main__': # Initializing a UAVCAN node instance. # In this example we're using an SLCAN adapter on the port '/dev/ttyACM0'. # PyUAVCAN also supports other types of adapters, refer to the docs to learn more. node = uavcan.make_node('/dev/ttyACM0', node_id=10, bitrate=1000000) # Initializing a dynamic node ID allocator. # This would not be necessary if the nodes were configured to use static node ID. node_monitor = uavcan.app.node_monitor.NodeMonitor(node) dynamic_node_id_allocator = uavcan.app.dynamic_node_id.CentralizedServer( node, node_monitor) # Waiting for at least one other node to appear online (our local node is already online). while len(dynamic_node_id_allocator.get_allocation_table()) <= 1: print('Waiting for other nodes to become online...') node.spin(timeout=1) # This is how we invoke the publishing function periodically. node.periodic(0.05, publish_throttle_setpoint) # Printing ESC status message to stdout in human-readable YAML format. node.add_handler(uavcan.equipment.esc.Status, lambda msg: print(uavcan.to_yaml(msg))) # Running the node until the application is terminated or until first error. try: node.spin() except KeyboardInterrupt: pass
# Initializing a dynamic node ID allocator. # This would not be necessary if the nodes were configured to use static node ID. node_monitor = uavcan.app.node_monitor.NodeMonitor(node) dynamic_node_id_allocator = uavcan.app.dynamic_node_id.CentralizedServer( node, node_monitor) # Waiting for at least one other node to appear online (our local node is already online). while len(dynamic_node_id_allocator.get_allocation_table()) <= 1: print('Waiting for other nodes to become online...') node.spin(timeout=1) # This is how we invoke the publishing function periodically. node.periodic(0.1, publish_throttle_setpoint) last_message = "" def set(value): global last_message last_message = value # Printing ESC status message to stdout in human-readable YAML format. node.add_handler(uavcan.equipment.esc.Status, lambda msg: set(uavcan.to_yaml(msg))) # Running the node until the application is terminated or until first error. try: node.spin() except KeyboardInterrupt: pass
def _test_once(self, bitrate, no_application): logger.info('Instantiating a new node with bitrate %d bps', bitrate) node = Node(self._can_iface_name, bitrate, self._can_dump_directory, [self._valid_firmware_dir, self._invalid_firmware_dir], self._self_node_id) with closing(node): node.monitor.add_update_handler( lambda e: logger.debug('Node monitor event: %s', e)) logger.info('Waiting for the device to appear online...') deadline = time.monotonic() + 60 device_node_id = 0 def is_dut_online(): nonlocal device_node_id def predicate(e: uavcan.app.node_monitor.NodeMonitor.Entry): return (e.info is not None) and bytes( e.info.hardware_version.unique_id) == self._device_uid matches = list(node.monitor.find_all(predicate)) if len(matches) > 0: device_node_id = matches[0].node_id return True if time.monotonic() > deadline: raise TestException('Node did not become online') return False node.spin_until(is_dut_online) logger.info('Device online, NID %d', device_node_id) orig_node_info = node.monitor.get(device_node_id).info def get_node_status(): try: e = node.monitor.get(device_node_id) except KeyError: raise TestException('Node is offline') return e.status def match_node_health_mode(health, mode): try: ns = get_node_status() except TestException: return False if health is not None: return ns.mode == mode and ns.health == health else: return ns.mode == mode # Optionally stopping the allocator if _maybe(): node.node_id_allocator.close() node.node_id_allocator = None # Selecting the firmware, possibly invalid using_valid_firmware = _maybe() fw_path = _choose_random_path_from_directory( self._valid_firmware_dir if using_valid_firmware else self._invalid_firmware_dir, '*.bin') logger.info('Using firmware %r, which is %s', fw_path, 'VALID' if using_valid_firmware else 'INVALID') # Requesting a firmware update; about ~50% chance of providing an incorrect server node ID begin_fw_update_request = uavcan.protocol.file.BeginFirmwareUpdate.Request( ) begin_fw_update_request.source_node_id = random.choice( list(range(1, 128)) + [node.node_id] * 128) using_valid_source_node_id = begin_fw_update_request.source_node_id in ( 0, node.node_id) begin_fw_update_request.image_file_remote_path.path = fw_path logger.info( 'Sending BeginFirmwareUpdate with source NID %d, which is %s', begin_fw_update_request.source_node_id, 'VALID' if using_valid_source_node_id else 'INVALID') response = node.request(begin_fw_update_request, device_node_id) logger.info('BeginFirmwareUpdate response: %s', response) # Spinning for a while in order to update the node status representation locally node.spin_for(5) logger.info('Latest NodeStatus\n%s', uavcan.to_yaml(get_node_status())) # Making sure that NodeInfo values reported by the bootloader and by the application are consistent def check_latest_node_info(): curr_node_info = node.monitor.get(device_node_id).info logger.info( 'Comparing node info, original vs. current:\n%s\n---\n%s', uavcan.to_yaml(orig_node_info), uavcan.to_yaml(curr_node_info)) _enforce( orig_node_info.hardware_version.major == curr_node_info.hardware_version.major, 'HW version major') _enforce( orig_node_info.hardware_version.minor == curr_node_info.hardware_version.minor, 'HW version minor') _enforce( orig_node_info.hardware_version.unique_id == curr_node_info.hardware_version.unique_id, 'HW UID') _enforce( orig_node_info.hardware_version.certificate_of_authenticity == curr_node_info.hardware_version. certificate_of_authenticity, 'HW COA') _enforce(orig_node_info.name == curr_node_info.name, 'Node name') check_latest_node_info() # Checking what state the bootloader is in if match_node_health_mode(None, NODE_STATUS_CONST.MODE_OPERATIONAL): raise TestException( 'The node did not enter firmware update mode') if using_valid_source_node_id: _enforce( match_node_health_mode( NODE_STATUS_CONST.HEALTH_OK, NODE_STATUS_CONST.MODE_SOFTWARE_UPDATE) or match_node_health_mode( NODE_STATUS_CONST.HEALTH_ERROR, NODE_STATUS_CONST.MODE_SOFTWARE_UPDATE), 'Expected update mode') # Continuing with other tests else: _enforce( match_node_health_mode( NODE_STATUS_CONST.HEALTH_WARNING, NODE_STATUS_CONST.MODE_SOFTWARE_UPDATE) or match_node_health_mode( NODE_STATUS_CONST.HEALTH_ERROR, NODE_STATUS_CONST.MODE_SOFTWARE_UPDATE), 'Expected failure') _enforce( get_node_status().vendor_specific_status_code == BootloaderError.UAVCAN_TIMEOUT, 'Expected failure') # Maybe restart, maybe not... if _maybe(): logger.info('Sending RestartRequest') r = uavcan.protocol.RestartNode.Request() r.magic_number = r.MAGIC_NUMBER response = node.request(r, device_node_id) _enforce(response.ok, 'RestartNode was supposed to return ok=True') return # Nothing more to test at this point # Waiting for the bootloader to complete downloading, then check the outcome logger.info('Waiting for the download to complete...') deadline = time.monotonic( ) + 3600 # An hour - a ridiculously large deadline, but whatever def poll(): _enforce(time.monotonic() < deadline, 'Stuck. :(') if _maybe(): # Generating garbage on the bus, because why not if _maybe(): r = uavcan.protocol.RestartNode.Request( ) # Invalid magic, this is intentional elif _maybe(0.01): r = uavcan.protocol.GetNodeInfo.Request() else: # The bootloader doesn't support this service, there will be no response r = uavcan.protocol.file.Read.Request() r.offset = int(random.random() * 1024 * 1024 * 1024) r.path.path = \ 'Senora, pray receive with your wonted kindness Senor Don Quixote of La Mancha, '\ 'whom you see before you, a knight-errant, and the bravest and wisest in the world.' node.request_async(r, device_node_id) return not match_node_health_mode( NODE_STATUS_CONST.HEALTH_OK, NODE_STATUS_CONST.MODE_SOFTWARE_UPDATE) node.spin_until(poll, check_interval=0.01) logger.info('Download finished, checking the outcome...') if using_valid_firmware: if not no_application: _enforce( match_node_health_mode( NODE_STATUS_CONST.HEALTH_OK, NODE_STATUS_CONST.MODE_INITIALIZATION), 'Expected successful completion') logger.info('Waiting for the board to boot...') node.spin_for(15) _enforce( match_node_health_mode( None, NODE_STATUS_CONST.MODE_OPERATIONAL), 'Application did not boot') check_latest_node_info() else: _enforce( match_node_health_mode( NODE_STATUS_CONST.HEALTH_ERROR, NODE_STATUS_CONST.MODE_SOFTWARE_UPDATE), 'Expected failure')
def listen(self, msg): self.last_message = uavcan.to_yaml(msg)