fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

staging debug

Browse Source
This commit is contained in:
Jacob Dahl 2025-10-15 23:46:35 -08:00
parent 527b9c1428
commit 870fb864ef
4 changed files with 183 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
boards/ark/can-rtk-gps/nuttx-config/canbootloader/defconfig
View File

@ -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

5
boards/ark/can-rtk-gps/src/boot_config.h
View File

@ -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

162
platforms/nuttx/src/canbootloader/uavcan/main.c
View File

@ -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;

41
src/drivers/uavcannode/UavcanNode.cpp
View File

@ -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)