Cannode add ability to get node ID after boot. Fix ARK CAN OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT

boards/ark/can-flow/src/boot_config.h

@@ -65,7 +65,7 @@
 #define OPT_PREFERRED_NODE_ID ANY_NODE_ID
 
 //todo:wrap OPT_x in in ifdefs for command line definitions
-#define OPT_TBOOT_MS                    5000
+#define OPT_TBOOT_MS                    3000
 #define OPT_NODE_STATUS_RATE_MS         800
 #define OPT_NODE_INFO_RATE_MS           50
 #define OPT_BL_NUMBER_TIMERS            7
@@ -93,7 +93,7 @@
  */
 #define OPT_WAIT_FOR_GETNODEINFO                    0
 #define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO        1
-#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 0
+#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1
 
 #define OPT_ENABLE_WD           1
 

boards/ark/can-flow/uavcan_board_identity

@@ -1,6 +1,6 @@
 # UAVCAN boot loadable Module ID
-set(uavcanblid_sw_version_major 0)
+set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
-set(uavcanblid_sw_version_minor 1)
+set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
 add_definitions(
 	-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
 	-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}
@@ -14,4 +14,4 @@ add_definitions(
 	-DHW_UAVCAN_NAME=${uavcanblid_name}
 	-DHW_VERSION_MAJOR=${uavcanblid_hw_version_major}
 	-DHW_VERSION_MINOR=${uavcanblid_hw_version_minor}
-)
+)

boards/ark/can-gps/src/boot_config.h

@@ -65,7 +65,7 @@
 #define OPT_PREFERRED_NODE_ID ANY_NODE_ID
 
 //todo:wrap OPT_x in in ifdefs for command line definitions
-#define OPT_TBOOT_MS                    5000
+#define OPT_TBOOT_MS                    3000
 #define OPT_NODE_STATUS_RATE_MS         800
 #define OPT_NODE_INFO_RATE_MS           50
 #define OPT_BL_NUMBER_TIMERS            7
@@ -93,7 +93,7 @@
  */
 #define OPT_WAIT_FOR_GETNODEINFO                    0
 #define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO        1
-#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 0
+#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1
 
 #define OPT_ENABLE_WD           1
 

boards/ark/can-gps/uavcan_board_identity

@@ -1,6 +1,6 @@
 # UAVCAN boot loadable Module ID
-set(uavcanblid_sw_version_major 0)
+set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
-set(uavcanblid_sw_version_minor 1)
+set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
 add_definitions(
 	-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
 	-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}

boards/ark/can-rtk-gps/src/boot_config.h

@@ -65,7 +65,7 @@
 #define OPT_PREFERRED_NODE_ID ANY_NODE_ID
 
 //todo:wrap OPT_x in in ifdefs for command line definitions
-#define OPT_TBOOT_MS                    5000
+#define OPT_TBOOT_MS                    3000
 #define OPT_NODE_STATUS_RATE_MS         800
 #define OPT_NODE_INFO_RATE_MS           50
 #define OPT_BL_NUMBER_TIMERS            7
@@ -93,7 +93,7 @@
  */
 #define OPT_WAIT_FOR_GETNODEINFO                    0
 #define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO        1
-#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 0
+#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1
 
 #define OPT_ENABLE_WD           1
 

boards/ark/can-rtk-gps/uavcan_board_identity

@@ -1,6 +1,6 @@
 # UAVCAN boot loadable Module ID
-set(uavcanblid_sw_version_major 0)
+set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
-set(uavcanblid_sw_version_minor 1)
+set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
 add_definitions(
 	-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
 	-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}

boards/ark/cannode/src/boot_config.h

@@ -65,7 +65,7 @@
 #define OPT_PREFERRED_NODE_ID ANY_NODE_ID
 
 //todo:wrap OPT_x in in ifdefs for command line definitions
-#define OPT_TBOOT_MS                    5000
+#define OPT_TBOOT_MS                    3000
 #define OPT_NODE_STATUS_RATE_MS         800
 #define OPT_NODE_INFO_RATE_MS           50
 #define OPT_BL_NUMBER_TIMERS            7
@@ -93,7 +93,7 @@
  */
 #define OPT_WAIT_FOR_GETNODEINFO                    0
 #define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO        1
-#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 0
+#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1
 
 #define OPT_ENABLE_WD           1
 

boards/ark/cannode/uavcan_board_identity

@@ -1,6 +1,6 @@
 # UAVCAN boot loadable Module ID
-set(uavcanblid_sw_version_major 0)
+set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
-set(uavcanblid_sw_version_minor 1)
+set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
 add_definitions(
 	-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
 	-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}

boards/cuav/can-gps-v1/uavcan_board_identity

@@ -1,6 +1,6 @@
 # UAVCAN boot loadable Module ID
-set(uavcanblid_sw_version_major 0)
+set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
-set(uavcanblid_sw_version_minor 1)
+set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
 add_definitions(
 	-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
 	-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}

boards/freefly/can-rtk-gps/uavcan_board_identity

@@ -1,6 +1,6 @@
 # UAVCAN boot loadable Module ID
-set(uavcanblid_sw_version_major 0)
+set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
-set(uavcanblid_sw_version_minor 1)
+set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
 add_definitions(
 	-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
 	-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}

boards/holybro/can-gps-v1/uavcan_board_identity

@@ -1,6 +1,6 @@
 # UAVCAN boot loadable Module ID
-set(uavcanblid_sw_version_major 0)
+set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
-set(uavcanblid_sw_version_minor 1)
+set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
 add_definitions(
 	-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
 	-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}

boards/matek/gnss-m9n-f4/uavcan_board_identity

@@ -1,6 +1,6 @@
 # UAVCAN boot loadable Module ID
-set(uavcanblid_sw_version_major 0)
+set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
-set(uavcanblid_sw_version_minor 1)
+set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
 add_definitions(
 	-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
 	-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}

boards/nxp/ucans32k146/uavcan_board_identity

@@ -1,6 +1,6 @@
 # UAVCAN boot loadable Module ID
-set(uavcanblid_sw_version_major 0)
+set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
-set(uavcanblid_sw_version_minor 1)
+set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
 add_definitions(
 	-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
 	-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}

boards/px4/fmu-v4/uavcan_board_identity

@@ -1,6 +1,6 @@
 # UAVCAN boot loadable Module ID
-set(uavcanblid_sw_version_major 0)
+set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
-set(uavcanblid_sw_version_minor 1)
+set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
 add_definitions(
 	-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
 	-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}

src/drivers/uavcannode/UavcanNode.cpp

@@ -158,6 +158,7 @@ UavcanNode::UavcanNode(CanInitHelper *can_init, uint32_t bitrate, uavcan::ICanDr
 	_time_sync_slave(_node),
 	_fw_update_listner(_node),
 	_param_server(_node),
+	_dyn_node_id_client(_node),
 	_reset_timer(_node)
 {
 	int res = pthread_mutex_init(&_node_mutex, nullptr);
@@ -168,6 +169,10 @@ UavcanNode::UavcanNode(CanInitHelper *can_init, uint32_t bitrate, uavcan::ICanDr
 	if (res < 0) {
 		std::abort();
 	}
+
+	// Ensure this param is marked as used
+	int32_t bitrate_temp = 0;
+	(void)param_get(param_find("CANNODE_BITRATE"), &bitrate_temp);
 }
 
 UavcanNode::~UavcanNode()
@@ -451,6 +456,31 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events
 
 	bus_events.registerSignalCallback(UavcanNode::busevent_signal_trampoline);
 
+	const int can_init_res = _can->init((uint32_t)_bitrate);
+
+	if (can_init_res < 0) {
+		PX4_ERR("CAN driver init failed %i", can_init_res);
+	}
+
+	int rv = _node.start();
+
+	if (rv < 0) {
+		PX4_ERR("Failed to start the node");
+	}
+
+	// If the node_id was not supplied by the bootloader do Dynamic Node ID allocation
+
+	if (_node.getNodeID() == 0) {
+
+		int client_start_res = _dyn_node_id_client.start(
+					       _node.getHardwareVersion().unique_id,    // USING THE SAME UNIQUE ID AS ABOVE
+					       _node.getNodeID());
+
+		if (client_start_res < 0) {
+			PX4_ERR("Failed to start the dynamic node ID client");
+		}
+	}
+
 	return 1;
 }
 
@@ -477,67 +507,36 @@ void UavcanNode::Run()
 
 	if (!_initialized) {
 
+		/*
+		 * Waiting for the client to obtain a node ID.
+		 * This may take a few seconds.
+		 */
 
-		const int can_init_res = _can->init((uint32_t)_bitrate);
+		if (_dyn_node_id_client.isAllocationComplete()) {
+			PX4_INFO("Got node ID %d", _dyn_node_id_client.getAllocatedNodeID().get());
 
-		if (can_init_res < 0) {
+			_node.setNodeID(_dyn_node_id_client.getAllocatedNodeID());
-			PX4_ERR("CAN driver init failed %i", can_init_res);
 		}
 
-		int rv = _node.start();
+		if (_node.getNodeID() != 0) {
+			up_time = hrt_absolute_time();
+			get_node().setRestartRequestHandler(&restart_request_handler);
+			_param_server.start(&_param_manager);
 
-		if (rv < 0) {
+			// Set up the time synchronization
-			PX4_ERR("Failed to start the node");
+			const int slave_init_res = _time_sync_slave.start();
-		}
 
-		// If the node_id was not supplied by the bootloader do Dynamic Node ID allocation
+			if (slave_init_res < 0) {
-
+				PX4_ERR("Failed to start time_sync_slave");
-		if (_node.getNodeID() == 0) {
+				_task_should_exit.store(true);
-
-			uavcan::DynamicNodeIDClient client(_node);
-
-			int client_start_res = client.start(_node.getHardwareVersion().unique_id,    // USING THE SAME UNIQUE ID AS ABOVE
-							    _node.getNodeID());
-
-			if (client_start_res < 0) {
-				PX4_ERR("Failed to start the dynamic node ID client");
 			}
 
-			watchdog_pet(); // If allocation takes too long reboot
+			_node.getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
 
-			/*
+			_node.setModeOperational();
-			 * Waiting for the client to obtain a node ID.
-			 * This may take a few seconds.
-			 */
 
-			while (!client.isAllocationComplete()) {
+			_initialized = true;
-				const int res = _node.spin(uavcan::MonotonicDuration::fromMSec(200));    // Spin duration doesn't matter
-
-				if (res < 0) {
-					PX4_ERR("Transient failure: %d", res);
-				}
-			}
-
-			_node.setNodeID(client.getAllocatedNodeID());
 		}
-
-		up_time = hrt_absolute_time();
-		get_node().setRestartRequestHandler(&restart_request_handler);
-		_param_server.start(&_param_manager);
-
-		// Set up the time synchronization
-		const int slave_init_res = _time_sync_slave.start();
-
-		if (slave_init_res < 0) {
-			PX4_ERR("Failed to start time_sync_slave");
-			_task_should_exit.store(true);
-		}
-
-		_node.getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
-
-		_node.setModeOperational();
-
-		_initialized = true;
 	}
 
 	perf_begin(_cycle_perf);
@@ -636,6 +635,19 @@ void UavcanNode::PrintInfo()
 {
 	pthread_mutex_lock(&_node_mutex);
 
+	// Firmware version
+	printf("Hardware and software status:\n");
+	printf("\tNode ID: %d\n", int(_node.getNodeID().get()));
+	printf("\tHardware version: %d.%d\n",
+	       int(_node.getHardwareVersion().major),
+	       int(_node.getHardwareVersion().minor));
+	printf("\tSoftware version: %d.%d.%08x\n",
+	       int(_node.getSoftwareVersion().major),
+	       int(_node.getSoftwareVersion().minor),
+	       int(_node.getSoftwareVersion().vcs_commit));
+
+	printf("\n");
+
 	// Memory status
 	printf("Pool allocator status:\n");
 	printf("\tCapacity hard/soft: %u/%u blocks\n",
@@ -752,7 +764,6 @@ extern "C" int uavcannode_start(int argc, char *argv[])
 		} else
 #endif
 		{
-			(void)param_get(param_find("CANNODE_NODE_ID"), &node_id);
 			(void)param_get(param_find("CANNODE_BITRATE"), &bitrate);
 		}
 	}
@@ -761,7 +772,7 @@ extern "C" int uavcannode_start(int argc, char *argv[])
 #if defined(SUPPORT_ALT_CAN_BOOTLOADER)
 		board_booted_by_px4() &&
 #endif
-		(node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast())) {
+		(node_id < 0 || node_id > uavcan::NodeID::Max)) {
 		PX4_ERR("Invalid Node ID %" PRId32, node_id);
 		return 1;
 	}

src/drivers/uavcannode/UavcanNode.hpp

@@ -181,6 +181,8 @@ private:
 	UavcanNodeParamManager _param_manager;
 	uavcan::ParamServer _param_server;
 
+	uavcan::DynamicNodeIDClient _dyn_node_id_client;
+
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
 	perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
 

src/drivers/uavcannode/uavcannode_params.c

@@ -31,17 +31,6 @@
  *
  ****************************************************************************/
 
-/**
- * UAVCAN Node ID.
- *
- * Read the specs at http://uavcan.org to learn more about Node ID.
- *
- * @min 1
- * @max 125
- * @group UAVCAN
- */
-PARAM_DEFINE_INT32(CANNODE_NODE_ID, 120);
-
 /**
  * UAVCAN CAN bus bitrate.
  *