diff --git a/src/drivers/uavcan/uavcan_servers.cpp b/src/drivers/uavcan/uavcan_servers.cpp index a279cba2e9..210887be43 100644 --- a/src/drivers/uavcan/uavcan_servers.cpp +++ b/src/drivers/uavcan/uavcan_servers.cpp @@ -117,7 +117,7 @@ UavcanServers::stop() if (server->_subnode_thread) { PX4_INFO("stopping fw srv thread..."); - server->_subnode_thread_should_exit = true; + server->_subnode_thread_should_exit.store(true); (void)pthread_join(server->_subnode_thread, NULL); } @@ -279,7 +279,7 @@ UavcanServers::init() pthread_addr_t UavcanServers::run(pthread_addr_t) { - prctl(PR_SET_NAME, "uavcan fw srv", 0); + prctl(PR_SET_NAME, "uavcan_fw_srv", 0); Lock lock(_subnode_mutex); @@ -311,7 +311,7 @@ UavcanServers::run(pthread_addr_t) memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids)); _esc_enumeration_index = 0; - while (!_subnode_thread_should_exit) { + while (!_subnode_thread_should_exit.load()) { if (_check_fw == true) { _check_fw = false; @@ -347,7 +347,7 @@ UavcanServers::run(pthread_addr_t) int call_res = _param_getset_client.call(request.node_id, req); if (call_res < 0) { - PX4_ERR("UAVCAN command bridge: couldn't send GetSet: %d", call_res); + PX4_ERR("couldn't send GetSet: %d", call_res); } else { _param_in_progress = true; @@ -380,7 +380,7 @@ UavcanServers::run(pthread_addr_t) int call_res = _param_getset_client.call(request.node_id, req); if (call_res < 0) { - PX4_ERR("UAVCAN command bridge: couldn't send GetSet: %d", call_res); + PX4_ERR("couldn't send GetSet: %d", call_res); } else { _param_in_progress = true; @@ -394,7 +394,7 @@ UavcanServers::run(pthread_addr_t) _param_list_node_id = request.node_id; _param_list_all_nodes = false; - PX4_INFO("UAVCAN command bridge: starting component-specific param list"); + PX4_DEBUG("starting component-specific param list"); } } else if (request.node_id == uavcan_parameter_request_s::NODE_ID_ALL) { @@ -408,7 +408,7 @@ UavcanServers::run(pthread_addr_t) _param_list_node_id = get_next_active_node_id(0); _param_list_all_nodes = true; - PX4_INFO("UAVCAN command bridge: starting global param list with node %hhu", _param_list_node_id); + PX4_DEBUG("starting global param list with node %hhu", _param_list_node_id); if (_param_counts[_param_list_node_id] == 0) { param_count(_param_list_node_id); @@ -427,7 +427,7 @@ 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]) { - PX4_INFO("UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id); + PX4_DEBUG("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; @@ -449,7 +449,7 @@ UavcanServers::run(pthread_addr_t) // Keep on listing. _param_index = 0; _param_list_in_progress = true; - PX4_INFO("UAVCAN command bridge: started param list for node %hhu", _param_list_node_id); + PX4_DEBUG("started param list for node %hhu", _param_list_node_id); } } } @@ -466,7 +466,7 @@ UavcanServers::run(pthread_addr_t) if (call_res < 0) { _param_list_in_progress = false; - PX4_ERR("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res); + PX4_ERR("couldn't send param list GetSet: %d", call_res); } else { _param_in_progress = true; @@ -487,7 +487,7 @@ UavcanServers::run(pthread_addr_t) int node_id = static_cast(cmd.param2 + 0.5f); int call_res; - PX4_INFO("UAVCAN command bridge: received UAVCAN command ID %d, node ID %d", command_id, node_id); + PX4_DEBUG("received UAVCAN command ID %d, node ID %d", command_id, node_id); switch (command_id) { case 0: @@ -515,7 +515,7 @@ UavcanServers::run(pthread_addr_t) } default: { - PX4_ERR("UAVCAN command bridge: unknown command ID %d", command_id); + PX4_ERR("unknown command ID %d", command_id); cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED; break; } @@ -525,7 +525,7 @@ UavcanServers::run(pthread_addr_t) acknowledge = true; int command_id = static_cast(cmd.param1 + 0.5f); - PX4_INFO("UAVCAN command bridge: received storage command ID %d", command_id); + PX4_DEBUG("received storage command ID %d", command_id); switch (command_id) { case 1: { @@ -573,15 +573,13 @@ UavcanServers::run(pthread_addr_t) actuator_armed_s armed; if (armed_sub.copy(&armed)) { - if (armed.armed && !(armed.lockdown || armed.manual_lockdown)) { - PX4_INFO("UAVCAN command bridge: system armed, exiting now."); + if (armed.armed) { + _subnode_thread_should_exit.store(true); } } } } - _subnode_thread_should_exit = false; - return (pthread_addr_t) 0; } @@ -612,14 +610,14 @@ UavcanServers::cb_getset(const uavcan::ServiceCallResult