dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
alexklimaj 2022-03-25 19:29:03 -06:00 committed by Daniel Agar
parent 23b31cc5fd
commit 3d61ab84c4
17 changed files with 94 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
boards/ark/can-flow/src/boot_config.h
View File

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

6
boards/ark/can-flow/uavcan_board_identity
View File

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

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

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

4
boards/ark/can-gps/uavcan_board_identity
View File

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

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

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

4
boards/ark/can-rtk-gps/uavcan_board_identity
View File

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

4
boards/ark/cannode/src/boot_config.h
View File

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

4
boards/ark/cannode/uavcan_board_identity
View File

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

4
boards/cuav/can-gps-v1/uavcan_board_identity
View File

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

4
boards/freefly/can-rtk-gps/uavcan_board_identity
View File

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

4
boards/holybro/can-gps-v1/uavcan_board_identity
View File

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

4
boards/matek/gnss-m9n-f4/uavcan_board_identity
View File

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

4
boards/nxp/ucans32k146/uavcan_board_identity
View File

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

4
boards/px4/fmu-v4/uavcan_board_identity
View File

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

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

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

2
src/drivers/uavcannode/UavcanNode.hpp
View File

@ -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")};

11
src/drivers/uavcannode/uavcannode_params.c
View File

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