staging debug

boards/ark/can-rtk-gps/nuttx-config/canbootloader/defconfig

@@ -54,3 +54,8 @@ CONFIG_STM32_NOEXT_VECTORS=y
 CONFIG_STM32_TIM8=y
 CONFIG_TASK_NAME_SIZE=0
 CONFIG_USEC_PER_TICK=1000
+CONFIG_STM32_USART2=y
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_SERIAL_CONSOLE=y
+CONFIG_USART2_TXBUFSIZE=1100

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

@@ -128,3 +128,8 @@
  *
  */
 #define GPIO_GETNODEINFO_JUMPER (GPIO_BOOT_CONFIG & ~GPIO_EXTI)
+
+/* Debug output option - set to 1 to enable bootloader debug messages */
+#ifndef OPT_ENABLE_DEBUG
+#  define OPT_ENABLE_DEBUG 1
+#endif

docs/en/dronecan/ark_cannode.md

@@ -83,9 +83,10 @@ This is done using the the parameters named like `UAVCAN_SUB_*` in the parameter
 
 On the ARK CANnode, you may need to configure the following parameters:
 
-| Parameter                                                                                       | Description                   |
-| ----------------------------------------------------------------------------------------------- | ----------------------------- |
-| <a id="CANNODE_TERM"></a>[CANNODE_TERM](../advanced_config/parameter_reference.md#CANNODE_TERM) | CAN built-in bus termination. |
+| Parameter                                                                                             | Description                                                                                                                                        |
+| ----------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------- |
+| <a id="CANNODE_NODE_ID"></a>[CANNODE_NODE_ID](../advanced_config/parameter_reference.md#CANNODE_NODE_ID) | CAN node ID (0 for dynamic allocation). If set to 0 (default), dynamic node allocation is used. Set to 1-127 to use a static node ID.            |
+| <a id="CANNODE_TERM"></a>[CANNODE_TERM](../advanced_config/parameter_reference.md#CANNODE_TERM)       | CAN built-in bus termination.                                                                                                                      |
 
 ## LED Meanings
 

docs/en/dronecan/ark_flow.md

@@ -110,9 +110,10 @@ When optical flow is the only source of horizontal position/velocity, then lower
 
 On the ARK Flow, you may need to configure the following parameters:
 
-| Parameter                                                                                       | Description                   |
-| ----------------------------------------------------------------------------------------------- | ----------------------------- |
-| <a id="CANNODE_TERM"></a>[CANNODE_TERM](../advanced_config/parameter_reference.md#CANNODE_TERM) | CAN built-in bus termination. |
+| Parameter                                                                                             | Description                                                                                                                                        |
+| ----------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------- |
+| <a id="CANNODE_NODE_ID"></a>[CANNODE_NODE_ID](../advanced_config/parameter_reference.md#CANNODE_NODE_ID) | CAN node ID (0 for dynamic allocation). If set to 0 (default), dynamic node allocation is used. Set to 1-127 to use a static node ID.            |
+| <a id="CANNODE_TERM"></a>[CANNODE_TERM](../advanced_config/parameter_reference.md#CANNODE_TERM)       | CAN built-in bus termination.                                                                                                                      |
 
 ## LED Meanings
 

docs/en/dronecan/ark_flow_mr.md

@@ -105,9 +105,10 @@ Set the following parameters in _QGroundControl_:
 
 You may need to [configure the following parameters](../dronecan/index.md#qgc-cannode-parameter-configuration) on the ARK Flow MR itself:
 
-| Parameter                                                                                       | Description                   |
-| ----------------------------------------------------------------------------------------------- | ----------------------------- |
-| <a id="CANNODE_TERM"></a>[CANNODE_TERM](../advanced_config/parameter_reference.md#CANNODE_TERM) | CAN built-in bus termination. |
+| Parameter                                                                                             | Description                                                                                                                                        |
+| ----------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------- |
+| <a id="CANNODE_NODE_ID"></a>[CANNODE_NODE_ID](../advanced_config/parameter_reference.md#CANNODE_NODE_ID) | CAN node ID (0 for dynamic allocation). If set to 0 (default), dynamic node allocation is used. Set to 1-127 to use a static node ID.            |
+| <a id="CANNODE_TERM"></a>[CANNODE_TERM](../advanced_config/parameter_reference.md#CANNODE_TERM)       | CAN built-in bus termination.                                                                                                                      |
 
 ## LED Meanings
 

docs/en/dronecan/ark_gps.md

@@ -91,9 +91,17 @@ If the sensor is not centred within the vehicle you will also need to define sen
 
 - Enable GPS yaw fusion by setting bit 3 of [EKF2_GPS_CTRL](../advanced_config/parameter_reference.md#EKF2_GPS_CTRL) to true.
 - Enable [UAVCAN_SUB_GPS](../advanced_config/parameter_reference.md#UAVCAN_SUB_GPS), [UAVCAN_SUB_MAG](../advanced_config/parameter_reference.md#UAVCAN_SUB_MAG), and [UAVCAN_SUB_BARO](../advanced_config/parameter_reference.md#UAVCAN_SUB_BARO).
-- Set [CANNODE_TERM](../advanced_config/parameter_reference.md#CANNODE_TERM) to `1` if this is that last node on the CAN bus.
 - The parameters [EKF2_GPS_POS_X](../advanced_config/parameter_reference.md#EKF2_GPS_POS_X), [EKF2_GPS_POS_Y](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Y) and [EKF2_GPS_POS_Z](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Z) can be set to account for the offset of the ARK GPS from the vehicles centre of gravity.
 
+### ARK GPS Configuration
+
+You may need to [configure the following parameters](../dronecan/index.md#qgc-cannode-parameter-configuration) on the ARK GPS itself:
+
+| Parameter                                                                                             | Description                                                                                                                                        |
+| ----------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------- |
+| <a id="CANNODE_NODE_ID"></a>[CANNODE_NODE_ID](../advanced_config/parameter_reference.md#CANNODE_NODE_ID) | CAN node ID (0 for dynamic allocation). If set to 0 (default), dynamic node allocation is used. Set to 1-127 to use a static node ID.            |
+| <a id="CANNODE_TERM"></a>[CANNODE_TERM](../advanced_config/parameter_reference.md#CANNODE_TERM)       | CAN built-in bus termination. Set to `1` if this is the last node on the CAN bus.                                                                 |
+
 ## LED Meanings
 
 You will see green, blue and red LEDs on the ARK GPS when it is being flashed, and a blinking green LED if it is running properly.

docs/en/dronecan/ark_rtk_gps.md

@@ -85,7 +85,15 @@ You need to set necessary [DroneCAN](index.md) parameters and define offsets if
 - Enable GPS blending to ensure the heading is always published by setting [SENS_GPS_MASK](../advanced_config/parameter_reference.md#SENS_GPS_MASK) to 7 (all three bits checked).
 - Enable [UAVCAN_SUB_GPS](../advanced_config/parameter_reference.md#UAVCAN_SUB_GPS), [UAVCAN_SUB_MAG](../advanced_config/parameter_reference.md#UAVCAN_SUB_MAG), and [UAVCAN_SUB_BARO](../advanced_config/parameter_reference.md#UAVCAN_SUB_BARO).
 - The parameters [EKF2_GPS_POS_X](../advanced_config/parameter_reference.md#EKF2_GPS_POS_X), [EKF2_GPS_POS_Y](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Y) and [EKF2_GPS_POS_Z](../advanced_config/parameter_reference.md#EKF2_GPS_POS_Z) can be set to account for the offset of the ARK RTK GPS from the vehicles centre of gravity.
-- Set [CANNODE_TERM](../advanced_config/parameter_reference.md#CANNODE_TERM) to `1` on the GPS if this it that last node on the CAN bus.
+
+### ARK RTK GPS Configuration
+
+You may need to [configure the following parameters](../dronecan/index.md#qgc-cannode-parameter-configuration) on the ARK RTK GPS itself:
+
+| Parameter                                                                                             | Description                                                                                                                                        |
+| ----------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------- |
+| <a id="CANNODE_NODE_ID"></a>[CANNODE_NODE_ID](../advanced_config/parameter_reference.md#CANNODE_NODE_ID) | CAN node ID (0 for dynamic allocation). If set to 0 (default), dynamic node allocation is used. Set to 1-127 to use a static node ID.            |
+| <a id="CANNODE_TERM"></a>[CANNODE_TERM](../advanced_config/parameter_reference.md#CANNODE_TERM)       | CAN built-in bus termination. Set to `1` if this is the last node on the CAN bus.                                                                 |
 
 ### Setting Up Rover and Fixed Base
 

docs/en/dronecan/index.md

@@ -96,6 +96,10 @@ If the DNA is still running and certain devices need to be manually configured,
 ::: info
 The PX4 node ID can be configured using the [UAVCAN_NODE_ID](../advanced_config/parameter_reference.md#UAVCAN_NODE_ID) parameter.
 The parameter is set to 1 by default.
+
+Devices running the [PX4 DroneCAN firmware](px4_cannode_fw.md) (such as [ARK CANnode](ark_cannode.md)) can use the
+[CANNODE_NODE_ID](../advanced_config/parameter_reference.md#CANNODE_NODE_ID) parameter to set a static node ID.
+Set it to 0 (default) for dynamic allocation, or to a value between 1-127 to use a specific static node ID.
 :::
 
 :::warning
@@ -282,6 +286,11 @@ For example, the screenshot below shows the parameters for a CAN GPS with node i
 
 ![QGC Parameter showing selected DroneCAN node](../../assets/can/dronecan/qgc_can_parameters.png)
 
+Common CANNODE parameters that you can configure include:
+
+- [CANNODE_NODE_ID](../advanced_config/parameter_reference.md#CANNODE_NODE_ID): Set a static node ID (1-127) or use 0 for dynamic allocation. See [PX4 DroneCAN Firmware > Static Node ID](px4_cannode_fw.md#static-node-id) for more information.
+- [CANNODE_TERM](../advanced_config/parameter_reference.md#CANNODE_TERM): Enable CAN bus termination on the last node in the bus.
+
 ## Device Specific Setup
 
 Most DroneCAN nodes require no further setup, unless specifically noted in their device-specific documentation.

docs/en/dronecan/px4_cannode_fw.md

@@ -20,6 +20,26 @@ make ark_can-flow_default
 
 This will create an output in **build/ark_can-flow_default** named **XX-X.X.XXXXXXXX.uavcan.bin**. Follow the instructions at [DroneCAN firmware update](index.md#firmware-update) to flash the firmware.
 
+## Configuration
+
+### Static Node ID
+
+By default, DroneCAN devices use [Dynamic Node Allocation (DNA)](index.md#node-id-allocation) to automatically obtain a unique node ID from the flight controller.
+However, you can configure a static node ID using the [CANNODE_NODE_ID](../advanced_config/parameter_reference.md#CANNODE_NODE_ID) parameter.
+
+To configure a static node ID:
+
+1. Set [CANNODE_NODE_ID](../advanced_config/parameter_reference.md#CANNODE_NODE_ID) to a value between 1-127 using [QGroundControl](index.md#qgc-cannode-parameter-configuration)
+2. Reboot the device
+
+To return to dynamic allocation, set `CANNODE_NODE_ID` back to 0.
+Note that when switching back to dynamic allocation, the flight controller will typically continue to allocate the same node ID that was previously used (this is normal DNA behavior).
+
+:::warning
+When using static node IDs, you must ensure that each device on the CAN bus has a unique node ID.
+Configuring two devices with the same ID will cause communication conflicts.
+:::
+
 ## Developer Information
 
 This section has information that is relevant to developers who want to add support for new DroneCAN hardware to the PX4 Autopilot.

platforms/nuttx/src/canbootloader/uavcan/main.c

@@ -59,6 +59,47 @@
 #include <drivers/bootloaders/boot_alt_app_shared.h>
 #include <drivers/drv_watchdog.h>
 #include <lib/crc/crc.h>
+#include <stdio.h>
+
+/****************************************************************************
+ * Bootloader Debug Logging
+ *
+ * Bootloader bypasses stdio initialization, so we use arm_lowputc directly
+ * for debug output. These macros are similar to PX4_INFO/PX4_DEBUG.
+ *
+ * Enable by setting OPT_ENABLE_DEBUG=1 in boot_config.h or board config.
+ ****************************************************************************/
+extern void arm_lowputc(char ch);
+
+static inline void bl_puts(const char *str)
+{
+	while (*str) { arm_lowputc(*str++); }
+}
+
+static inline void bl_putnum(uint32_t val)
+{
+	char buf[12];
+	snprintf(buf, sizeof(buf), "%lu", val);
+	bl_puts(buf);
+}
+
+#if OPT_ENABLE_DEBUG
+#  define BL_DEBUG(fmt, ...) \
+	do { \
+		bl_puts("[BL] " fmt); \
+		__VA_ARGS__; \
+		bl_puts("\r\n"); \
+	} while (0)
+#  define BL_DEBUG_NUM(label, val) \
+	do { \
+		bl_puts("[BL] " label ": "); \
+		bl_putnum(val); \
+		bl_puts("\r\n"); \
+	} while (0)
+#else
+#  define BL_DEBUG(fmt, ...)
+#  define BL_DEBUG_NUM(label, val)
+#endif
 
 //#define DEBUG_APPLICATION_INPLACE    1 /* Never leave defined */
 #define DEBUG_NO_FW_UPDATE           1 /* With DEBUG_APPLICATION_INPLACE
@@ -1111,9 +1152,40 @@ __EXPORT int main(int argc, char *argv[])
 	 */
 	common.crc.valid = false;
 
-	/* Either way prevent Deja vu by invalidating the struct*/
+	/*
+	 * Check if Node ID has been previously assigned and skip DNA if so.
+	 * IMPORTANT: Read this BEFORE invalidating!
+	 */
+	bootloader_app_shared_t shared_data;
+	bool has_node_id = false;
+
+	if (bootloader_app_shared_read(&shared_data, App) == OK) {
+		/* We have valid shared_data data from a previous app run */
+		if (shared_data.node_id > 0 && shared_data.node_id <= 127) {
+			has_node_id = true;
+			common.node_id = shared_data.node_id;
+			BL_DEBUG_NUM("Read persisted node ID", shared_data.node_id);
+
+			/* Also restore the bus speed if we have it */
+			if (shared_data.bus_speed != 0) {
+				common.bus_speed = shared_data.bus_speed;
+				BL_DEBUG_NUM("Read persisted bus speed", shared_data.bus_speed);
+			}
+
+		} else {
+			BL_DEBUG("Shared data has invalid node ID",);
+		}
+
+	} else {
+		BL_DEBUG("No valid shared data found",);
+	}
+
+	BL_DEBUG_NUM("has_node_id", has_node_id);
+
+	/* Now invalidate to prevent deja vu (after we've read the static node ID) */
 	bootloader_app_shared_invalidate();
 
+
 	/* Set up the Timers */
 	bl_timer_cb_t p = null_cb;
 	p.cb = uptime_process;
@@ -1193,34 +1265,78 @@ __EXPORT int main(int argc, char *argv[])
 		 * or the Node allocation runs longer the tBoot
 		 */
 
-		/* Preferred Node Address */
+		BL_DEBUG("Regular boot - checking for static node ID",);
 
-		common.node_id = OPT_PREFERRED_NODE_ID;
-
-		if (CAN_OK != autobaud_and_get_dynamic_node_id(tboot, (can_speed_t *)&bootloader.bus_speed, &common.node_id)) {
+		/* Skip DNA if we already have a node ID from previously */
+		if (has_node_id) {
+			BL_DEBUG("Using persisted static node ID - skipping DNA",);
+			BL_DEBUG_NUM("Static node ID", common.node_id);
 
 			/*
-			 * It is OK that node ID is set to the preferred Node ID because
-			 *  common.crc.valid is not true yet
+			 * We have a saved node ID.
+			 * Only do autobaud if we don't have the bus speed.
+			 */
+			if (common.bus_speed == 0) {
+				BL_DEBUG("Bus speed unknown - performing autobaud only",);
+
+				bootloader.bus_speed = CAN_UNDEFINED;
+
+				if (CAN_OK != autobaud_and_get_dynamic_node_id(tboot, (can_speed_t *)&bootloader.bus_speed, &common.node_id)) {
+					goto boot;
+				}
+
+				common.bus_speed = can_speed2freq(bootloader.bus_speed);
+				BL_DEBUG_NUM("Autobaud completed - bus speed", common.bus_speed);
+
+			} else {
+				BL_DEBUG("Using both persisted node ID and bus speed",);
+				BL_DEBUG_NUM("Node ID", common.node_id);
+				BL_DEBUG_NUM("Bus speed", common.bus_speed);
+
+				/* We have both node ID and bus speed from shared data */
+				bootloader.bus_speed = can_freq2speed(common.bus_speed);
+				can_init(bootloader.bus_speed, CAN_Mode_Normal);
+			}
+
+			bootloader.uptime = 0;
+			common.crc.valid = true;
+
+		} else {
+			/* No node ID, do full DNA */
+			BL_DEBUG("No persisted node ID - performing full DNA",);
+
+			/* Preferred Node Address */
+			common.node_id = OPT_PREFERRED_NODE_ID;
+
+			if (CAN_OK != autobaud_and_get_dynamic_node_id(tboot, (can_speed_t *)&bootloader.bus_speed, &common.node_id)) {
+
+				/*
+				 * It is OK that node ID is set to the preferred Node ID because
+				 *  common.crc.valid is not true yet
+				 */
+				BL_DEBUG("DNA failed or timed out",);
+
+				goto boot;
+			}
+
+			BL_DEBUG("DNA completed successfully",);
+			BL_DEBUG_NUM("Allocated node ID", common.node_id);
+
+			/* We have autobauded and got a Node ID. So reset uptime
+			 * and save the speed and node_id in both the common and
+			 * and bootloader data sets
 			 */
 
-			goto boot;
+			bootloader.uptime = 0;
+			common.bus_speed = can_speed2freq(bootloader.bus_speed);
+
+			/*
+			 * Mark CRC to say this is from
+			 * auto baud and Node Allocation
+			 */
+			common.crc.valid = true;
 		}
 
-		/* We have autobauded and got a Node ID. So reset uptime
-		 * and save the speed and node_id in both the common and
-		 * and bootloader data sets
-		 */
-
-		bootloader.uptime = 0;
-		common.bus_speed = can_speed2freq(bootloader.bus_speed);
-
-		/*
-		 * Mark CRC to say this is from
-		 * auto baud and Node Allocation
-		 */
-		common.crc.valid = true;
-
 		/* Auto bauding may have taken a long time, so restart the tboot time*/
 
 		if (bootloader.app_valid && !bootloader.wait_for_getnodeinfo) {
@@ -1230,6 +1346,8 @@ __EXPORT int main(int argc, char *argv[])
 
 	}
 
+	BL_DEBUG_NUM("Configuring UAVCAN with node ID", common.node_id);
+
 	/* Now that we have a node Id configure the uavcan library */
 	g_this_node_id = common.node_id;
 

src/drivers/uavcannode/UavcanNode.cpp

@@ -586,11 +586,30 @@ void UavcanNode::Run()
 
 	// check for parameter updates
 	if (_parameter_update_sub.updated()) {
+		PX4_INFO("params updated");
 		// clear update
 		parameter_update_s pupdate;
 		_parameter_update_sub.copy(&pupdate);
 
 		// update parameters from storage
+		int32_t cannode_node_id = 0;
+		param_get(param_find("CANNODE_NODE_ID"), &cannode_node_id);
+
+		if (_node.getNodeID().get() != (uint8_t)cannode_node_id) {
+			if (cannode_node_id > 0 && cannode_node_id <= uavcan::NodeID::Max) {
+				// Write new static node ID
+				PX4_INFO("Param changed - writing static node ID %ld to bootloader", cannode_node_id);
+				bootloader_app_shared_t shared_write = {};
+				shared_write.node_id = cannode_node_id;
+				shared_write.bus_speed = 0;  // Force autobaud on next boot (distinguishes from FW update request)
+				bootloader_app_shared_write(&shared_write, App);
+
+			} else if (cannode_node_id == 0) {
+				// User set to 0 to force DNA on next boot
+				PX4_INFO("CANNODE_NODE_ID set to 0 - invalidating shared memory to force DNA on next boot");
+				bootloader_app_shared_invalidate();
+			}
+		}
 	}
 
 	_node.spinOnce();
@@ -824,6 +843,33 @@ extern "C" int uavcannode_start(int argc, char *argv[])
 		}
 	}
 
+	// Read the static node ID parameter - this always takes precedence if set
+	int32_t cannode_node_id = 0;
+	param_get(param_find("CANNODE_NODE_ID"), &cannode_node_id);
+
+	// Check if the static node ID is in range
+	if (cannode_node_id < 0 || cannode_node_id > uavcan::NodeID::Max) {
+		PX4_ERR("Invalid static node ID %ld, using dynamic allocation", cannode_node_id);
+		cannode_node_id = 0;
+	}
+
+	// Always prefer static node ID parameter if set (overrides bootloader's node_id)
+	if (cannode_node_id > 0 && cannode_node_id <= uavcan::NodeID::Max) {
+		node_id = cannode_node_id;
+		PX4_INFO("Using static node ID %ld from parameter", cannode_node_id);
+
+		bootloader_app_shared_t shared_write = {};
+		shared_write.node_id = node_id;
+		shared_write.bus_speed = 0;  // Force autobaud on next boot (distinguishes from FW update request)
+		PX4_INFO("Writing static node ID %ld to shared memory (bus_speed=0)", (int32_t)shared_write.node_id);
+		bootloader_app_shared_write(&shared_write, App);
+	} else {
+		PX4_INFO("No static node ID set (CANNODE_NODE_ID=%ld)", cannode_node_id);
+	}
+
+	// If no static node ID parameter, use what bootloader provided.
+	// Shared memory is invalidated above, so bootloader will do DNA on next boot.
+
 	if (
 #if defined(SUPPORT_ALT_CAN_BOOTLOADER)
 		board_booted_by_px4() &&

src/drivers/uavcannode/uavcannode_params.c

@@ -31,6 +31,15 @@
  *
  ****************************************************************************/
 
+/**
+ * UAVCAN CAN node ID (0 for dynamic allocation).
+ *
+ * @min 0
+ * @max 127
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(CANNODE_NODE_ID, 0);
+
 /**
  * UAVCAN CAN bus bitrate.
  *