From 2a7e75043d0017e3462d6a3c305b047180136d67 Mon Sep 17 00:00:00 2001 From: Ben Dyer Date: Sun, 11 Oct 2015 23:45:15 +1100 Subject: [PATCH] Reduced logging verbosity; fixed stack size; fixed ESC indexes --- src/modules/uavcan/uavcan_servers.cpp | 51 ++++++++++++--------------- src/modules/uavcan/uavcan_servers.hpp | 12 +++---- 2 files changed, 28 insertions(+), 35 deletions(-) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index e9eb573d36..38cc01f0c8 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -61,7 +61,7 @@ #include #include -#include +#include ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s); ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s); @@ -369,7 +369,6 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) } else { _param_in_progress = true; _param_index = request.param_index; - warnx("UAVCAN command bridge: sent GetSet"); } } else if (request.message_type == MAVLINK_MSG_ID_PARAM_SET) { uavcan::protocol::param::GetSet::Request req; @@ -393,7 +392,6 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) } else { _param_in_progress = true; _param_index = request.param_index; - warnx("UAVCAN command bridge: sent GetSet"); } } else if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) { // This triggers the _param_list_in_progress case below. @@ -415,9 +413,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) _param_list_node_id = get_next_active_node_id(1); _param_list_all_nodes = true; - warnx("UAVCAN command bridge: starting global param list"); + warnx("UAVCAN command bridge: starting global param list with node %hhu", _param_list_node_id); - if (_param_counts[_param_list_node_id.get()] == 0) { + if (_param_counts[_param_list_node_id] == 0) { param_count(_param_list_node_id); } } @@ -432,25 +430,27 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) // Handle parameter listing index/node ID advancement if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) { - if (_param_index >= _param_counts[_param_list_node_id.get()]) { + if (_param_index >= _param_counts[_param_list_node_id]) { + warnx("UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id); // Reached the end of the current node's parameter set. _param_list_in_progress = false; if (_param_list_all_nodes) { // We're listing all parameters for all nodes -- get the next node ID - uavcan::NodeID next_id = get_next_active_node_id(_param_list_node_id); - if (next_id != _param_list_node_id) { + uint8_t next_id = get_next_active_node_id(_param_list_node_id); + if (next_id < 128) { + _param_list_node_id = next_id; /* * If there is a next node ID, check if that node's parameters * have been counted before. If not, do it now. */ - if (_param_counts[_param_list_node_id.get()] == 0) { + if (_param_counts[_param_list_node_id] == 0) { param_count(_param_list_node_id); } // Keep on listing. _param_index = 0; _param_list_in_progress = true; - warnx("UAVCAN command bridge: incrementing global param list node ID"); + warnx("UAVCAN command bridge: started param list for node %hhu", _param_list_node_id); } } } @@ -466,10 +466,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) int call_res = _param_getset_client.call(_param_list_node_id, req); if (call_res < 0) { _param_list_in_progress = false; - warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res); + warnx("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res); } else { _param_in_progress = true; - warnx("UAVCAN command bridge: sent GetSet during param list operation"); } } @@ -585,8 +584,6 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult(); } - warnx("UAVCAN command bridge: successful GetSet response for param %s, node %hhu", response.param_id, response.node_id); - if (_param_response_pub == nullptr) { _param_response_pub = orb_advertise(ORB_ID(uavcan_parameter_value), &response); } else { @@ -670,19 +663,17 @@ void UavcanServers::param_count(uavcan::NodeID node_id) } } -uavcan::NodeID UavcanServers::get_next_active_node_id(const uavcan::NodeID &base) +uint8_t UavcanServers::get_next_active_node_id(uint8_t base) { - for (int i = base.get(); i < 128; i++) { - if (_node_info_retriever.isNodeKnown(i) && _subnode.getNodeID() != i) { - return uavcan::NodeID(i); - } - } + base++; + for (; base < 128 && (!_node_info_retriever.isNodeKnown(base) || + _subnode.getNodeID().get() == base); base++); return base; } void UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult &result) { - uavcan::NodeID next_id = get_next_active_node_id(result.getCallID().server_node_id); + uint8_t next_id = get_next_active_node_id(result.getCallID().server_node_id.get()); if (!result.isSuccessful()) { warnx("UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get()); @@ -693,7 +684,7 @@ void UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult(); esc_index = std::min((uint8_t)(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1), esc_index); - _esc_enumeration_index = std::max(_esc_enumeration_index, esc_index); + _esc_enumeration_index = std::max(_esc_enumeration_index, (uint8_t)(esc_index + 1)); _esc_enumeration_ids[esc_index] = result.getCallID().server_node_id.get(); @@ -787,8 +778,10 @@ void UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult SubNode; @@ -155,15 +155,15 @@ private: * * The node's UAVCAN ID is used as the index into the _param_counts array. */ - uint16_t _param_counts[128]; + uint8_t _param_counts[128]; bool _count_in_progress; - uint16_t _count_index; + uint8_t _count_index; bool _param_in_progress; - uint16_t _param_index; + uint8_t _param_index; bool _param_list_in_progress; bool _param_list_all_nodes; - uavcan::NodeID _param_list_node_id; + uint8_t _param_list_node_id; bool _cmd_in_progress; @@ -184,7 +184,7 @@ private: uavcan::ServiceClient _param_getset_client; void param_count(uavcan::NodeID node_id); - uavcan::NodeID get_next_active_node_id(const uavcan::NodeID &base); + uint8_t get_next_active_node_id(uint8_t base); bool _mutex_inited; volatile bool _check_fw;