diff --git a/boards/ark/can-flow/src/boot_config.h b/boards/ark/can-flow/src/boot_config.h index eab3c76c6c..76782f9a93 100644 --- a/boards/ark/can-flow/src/boot_config.h +++ b/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 diff --git a/boards/ark/can-flow/uavcan_board_identity b/boards/ark/can-flow/uavcan_board_identity index 8f389d0657..e49fbcac8a 100644 --- a/boards/ark/can-flow/uavcan_board_identity +++ b/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_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +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} -) \ No newline at end of file +) diff --git a/boards/ark/can-gps/src/boot_config.h b/boards/ark/can-gps/src/boot_config.h index eab3c76c6c..76782f9a93 100644 --- a/boards/ark/can-gps/src/boot_config.h +++ b/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 diff --git a/boards/ark/can-gps/uavcan_board_identity b/boards/ark/can-gps/uavcan_board_identity index ca6d098e59..00b23ea028 100644 --- a/boards/ark/can-gps/uavcan_board_identity +++ b/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_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/ark/can-rtk-gps/src/boot_config.h b/boards/ark/can-rtk-gps/src/boot_config.h index eab3c76c6c..76782f9a93 100644 --- a/boards/ark/can-rtk-gps/src/boot_config.h +++ b/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 diff --git a/boards/ark/can-rtk-gps/uavcan_board_identity b/boards/ark/can-rtk-gps/uavcan_board_identity index 5db21a4ffb..655384bc07 100644 --- a/boards/ark/can-rtk-gps/uavcan_board_identity +++ b/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_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/ark/cannode/src/boot_config.h b/boards/ark/cannode/src/boot_config.h index eab3c76c6c..76782f9a93 100644 --- a/boards/ark/cannode/src/boot_config.h +++ b/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 diff --git a/boards/ark/cannode/uavcan_board_identity b/boards/ark/cannode/uavcan_board_identity index 44ea18cc7d..490b7678bd 100644 --- a/boards/ark/cannode/uavcan_board_identity +++ b/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_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/cuav/can-gps-v1/uavcan_board_identity b/boards/cuav/can-gps-v1/uavcan_board_identity index a547589079..0cf273dde7 100644 --- a/boards/cuav/can-gps-v1/uavcan_board_identity +++ b/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_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/freefly/can-rtk-gps/uavcan_board_identity b/boards/freefly/can-rtk-gps/uavcan_board_identity index 997b26e2ae..2531725501 100644 --- a/boards/freefly/can-rtk-gps/uavcan_board_identity +++ b/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_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/holybro/can-gps-v1/uavcan_board_identity b/boards/holybro/can-gps-v1/uavcan_board_identity index c34b51cd77..bb7a514fd6 100644 --- a/boards/holybro/can-gps-v1/uavcan_board_identity +++ b/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_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/matek/gnss-m9n-f4/uavcan_board_identity b/boards/matek/gnss-m9n-f4/uavcan_board_identity index 70123f7d91..fb24ed9f11 100755 --- a/boards/matek/gnss-m9n-f4/uavcan_board_identity +++ b/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_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/nxp/ucans32k146/uavcan_board_identity b/boards/nxp/ucans32k146/uavcan_board_identity index dd7e3fcb77..7ff76347f8 100644 --- a/boards/nxp/ucans32k146/uavcan_board_identity +++ b/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_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/px4/fmu-v4/uavcan_board_identity b/boards/px4/fmu-v4/uavcan_board_identity index 0905752e49..507d3ec578 100644 --- a/boards/px4/fmu-v4/uavcan_board_identity +++ b/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_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index 810a4fd0ca..2940906c25 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/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) { - PX4_ERR("CAN driver init failed %i", can_init_res); + _node.setNodeID(_dyn_node_id_client.getAllocatedNodeID()); } - 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) { - PX4_ERR("Failed to start the node"); - } + // Set up the time synchronization + 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 (_node.getNodeID() == 0) { - - 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"); + if (slave_init_res < 0) { + PX4_ERR("Failed to start time_sync_slave"); + _task_should_exit.store(true); } - watchdog_pet(); // If allocation takes too long reboot + _node.getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); - /* - * Waiting for the client to obtain a node ID. - * This may take a few seconds. - */ + _node.setModeOperational(); - while (!client.isAllocationComplete()) { - 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()); + _initialized = true; } - - 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; } diff --git a/src/drivers/uavcannode/UavcanNode.hpp b/src/drivers/uavcannode/UavcanNode.hpp index f7a3a25374..bbe7158ec1 100644 --- a/src/drivers/uavcannode/UavcanNode.hpp +++ b/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")}; diff --git a/src/drivers/uavcannode/uavcannode_params.c b/src/drivers/uavcannode/uavcannode_params.c index 8a9e1ba0ee..5b6a27e44e 100644 --- a/src/drivers/uavcannode/uavcannode_params.c +++ b/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. *