From 870fb864ef48bff6252db2c2b50e375809f4686f Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 15 Oct 2025 23:46:35 -0800 Subject: [PATCH] staging debug --- .../nuttx-config/canbootloader/defconfig | 5 + boards/ark/can-rtk-gps/src/boot_config.h | 5 + .../nuttx/src/canbootloader/uavcan/main.c | 162 +++++++++++++++--- src/drivers/uavcannode/UavcanNode.cpp | 41 ++++- 4 files changed, 183 insertions(+), 30 deletions(-) diff --git a/boards/ark/can-rtk-gps/nuttx-config/canbootloader/defconfig b/boards/ark/can-rtk-gps/nuttx-config/canbootloader/defconfig index 5c1f3b54bc..0c6a2034c6 100644 --- a/boards/ark/can-rtk-gps/nuttx-config/canbootloader/defconfig +++ b/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 diff --git a/boards/ark/can-rtk-gps/src/boot_config.h b/boards/ark/can-rtk-gps/src/boot_config.h index 76782f9a93..a0b5379ed1 100644 --- a/boards/ark/can-rtk-gps/src/boot_config.h +++ b/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 diff --git a/platforms/nuttx/src/canbootloader/uavcan/main.c b/platforms/nuttx/src/canbootloader/uavcan/main.c index 015943f0e9..b053761f7f 100644 --- a/platforms/nuttx/src/canbootloader/uavcan/main.c +++ b/platforms/nuttx/src/canbootloader/uavcan/main.c @@ -59,6 +59,47 @@ #include #include #include +#include + +/**************************************************************************** + * 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; diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index 779935c6dc..3f151e64ea 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/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,7 +843,7 @@ extern "C" int uavcannode_start(int argc, char *argv[]) } } - // Read the static node ID parameter and use it if no valid node_id from shared memory + // 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); @@ -834,16 +853,22 @@ extern "C" int uavcannode_start(int argc, char *argv[]) cannode_node_id = 0; } - // Assign the static node ID if no dynamic allocation is used. Do wen't override a valid node ID from the bootloader? - if (node_id == 0 && cannode_node_id > 0 && cannode_node_id <= uavcan::NodeID::Max) { + // 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); } - // Persist the node ID for the bootloader - bootloader_app_shared_t shared_write = {}; - shared_write.node_id = node_id; - shared_write.bus_speed = bitrate; - bootloader_app_shared_write(&shared_write, BootLoader); + // 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)