From 49915ed06f418a4fc60a132c04eb52e773f92eac Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sun, 27 Sep 2020 01:42:40 +0530 Subject: [PATCH] AP_UAVCAN: save some stack space --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 61 ++++++++++++++++--------------- 1 file changed, 31 insertions(+), 30 deletions(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 9b47feca9f..a211731587 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -202,40 +202,41 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: node was already started?\n\r"); return; } + { + uavcan::NodeID self_node_id(_uavcan_node); + _node->setNodeID(self_node_id); - uavcan::NodeID self_node_id(_uavcan_node); - _node->setNodeID(self_node_id); + char ndname[20]; + snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", driver_index); - char ndname[20]; - snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", driver_index); - - uavcan::NodeStatusProvider::NodeName name(ndname); - _node->setName(name); - - uavcan::protocol::SoftwareVersion sw_version; // Standard type uavcan.protocol.SoftwareVersion - sw_version.major = AP_UAVCAN_SW_VERS_MAJOR; - sw_version.minor = AP_UAVCAN_SW_VERS_MINOR; - _node->setSoftwareVersion(sw_version); - - uavcan::protocol::HardwareVersion hw_version; // Standard type uavcan.protocol.HardwareVersion - - hw_version.major = AP_UAVCAN_HW_VERS_MAJOR; - hw_version.minor = AP_UAVCAN_HW_VERS_MINOR; - - const uint8_t uid_buf_len = hw_version.unique_id.capacity(); - uint8_t uid_len = uid_buf_len; - uint8_t unique_id[uid_buf_len]; - - - if (hal.util->get_system_id_unformatted(unique_id, uid_len)) { - //This is because we are maintaining a common Server Record for all UAVCAN Instances. - //In case the node IDs are different, and unique id same, it will create - //conflict in the Server Record. - unique_id[uid_len - 1] += _uavcan_node; - uavcan::copy(unique_id, unique_id + uid_len, hw_version.unique_id.begin()); + uavcan::NodeStatusProvider::NodeName name(ndname); + _node->setName(name); } - _node->setHardwareVersion(hw_version); + { + uavcan::protocol::SoftwareVersion sw_version; // Standard type uavcan.protocol.SoftwareVersion + sw_version.major = AP_UAVCAN_SW_VERS_MAJOR; + sw_version.minor = AP_UAVCAN_SW_VERS_MINOR; + _node->setSoftwareVersion(sw_version); + uavcan::protocol::HardwareVersion hw_version; // Standard type uavcan.protocol.HardwareVersion + + hw_version.major = AP_UAVCAN_HW_VERS_MAJOR; + hw_version.minor = AP_UAVCAN_HW_VERS_MINOR; + + const uint8_t uid_buf_len = hw_version.unique_id.capacity(); + uint8_t uid_len = uid_buf_len; + uint8_t unique_id[uid_buf_len]; + + + if (hal.util->get_system_id_unformatted(unique_id, uid_len)) { + //This is because we are maintaining a common Server Record for all UAVCAN Instances. + //In case the node IDs are different, and unique id same, it will create + //conflict in the Server Record. + unique_id[uid_len - 1] += _uavcan_node; + uavcan::copy(unique_id, unique_id + uid_len, hw_version.unique_id.begin()); + } + _node->setHardwareVersion(hw_version); + } int start_res = _node->start(); if (start_res < 0) { debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: node start problem, error %d\n\r", start_res);