diff --git a/msg/uavcan_parameter_request.msg b/msg/uavcan_parameter_request.msg new file mode 100644 index 0000000000..9ced52bbae --- /dev/null +++ b/msg/uavcan_parameter_request.msg @@ -0,0 +1,8 @@ +# UAVCAN-MAVLink parameter bridge request type +uint8 message_type # MAVLink message type: PARAM_REQUEST_READ, PARAM_REQUEST_LIST, PARAM_SET +uint8 node_id # UAVCAN node ID mapped from MAVLink component ID +char[17] param_id # MAVLink/UAVCAN parameter name +int16 param_index # -1 if the param_id field should be used as identifier +uint8 param_type # MAVLink parameter type +int64 int_value # current value if param_type is int-like +float32 real_value # current value if param_type is float-like diff --git a/msg/uavcan_parameter_value.msg b/msg/uavcan_parameter_value.msg new file mode 100644 index 0000000000..091c5fd278 --- /dev/null +++ b/msg/uavcan_parameter_value.msg @@ -0,0 +1,8 @@ +# UAVCAN-MAVLink parameter bridge response type +uint8 node_id # UAVCAN node ID mapped from MAVLink component ID +char[17] param_id # MAVLink/UAVCAN parameter name +int16 param_index # parameter index, if known +uint16 param_count # number of parameters exposed by the node +uint8 param_type # MAVLink parameter type +int64 int_value # current value if param_type is int-like +float32 real_value # current value if param_type is float-like diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 414b7ee96f..bdaa483dfc 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -41,25 +41,25 @@ #include +#include +#include + #include "mavlink_parameters.h" #include "mavlink_main.h" MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), _send_all_index(-1), _rc_param_map_pub(nullptr), - _rc_param_map() + _rc_param_map(), + _uavcan_parameter_request_pub(nullptr), + _uavcan_parameter_value_sub(-1) { } unsigned MavlinkParametersManager::get_size() { - if (_send_all_index >= 0) { - return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - - } else { - return 0; - } + return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } void @@ -76,36 +76,75 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) _send_all_index = 0; } + + if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 && + (req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { + // publish list request to UAVCAN driver via uORB. + uavcan_parameter_request_s req; + req.message_type = msg->msgid; + req.node_id = req_list.target_component; + req.param_index = 0; + + if (_uavcan_parameter_request_pub == nullptr) { + _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); + } else { + orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); + } + } break; } case MAVLINK_MSG_ID_PARAM_SET: { /* set parameter */ - if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { - mavlink_param_set_t set; - mavlink_msg_param_set_decode(msg, &set); + mavlink_param_set_t set; + mavlink_msg_param_set_decode(msg, &set); - if (set.target_system == mavlink_system.sysid && - (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { + if (set.target_system == mavlink_system.sysid && + (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter, set and send it */ - param_t param = param_find_no_notification(name); + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter, set and send it */ + param_t param = param_find_no_notification(name); - if (param == PARAM_INVALID) { - char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; - sprintf(buf, "[pm] unknown param: %s", name); - _mavlink->send_statustext_info(buf); + if (param == PARAM_INVALID) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] unknown param: %s", name); + _mavlink->send_statustext_info(buf); - } else { - /* set and send parameter */ - param_set(param, &(set.param_value)); - send_param(param); - } + } else { + /* set and send parameter */ + param_set(param, &(set.param_value)); + send_param(param); + } + } + + if (set.target_system == mavlink_system.sysid && set.target_component < 127 && + (set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { + // publish set request to UAVCAN driver via uORB. + uavcan_parameter_request_s req; + req.message_type = msg->msgid; + req.node_id = set.target_component; + req.param_index = -1; + strncpy(req.param_id, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1); + req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + if (set.param_type == MAV_PARAM_TYPE_REAL32) { + req.param_type = MAV_PARAM_TYPE_REAL32; + req.real_value = set.param_value; + } else { + int32_t val; + memcpy(&val, &set.param_value, sizeof(int32_t)); + req.param_type = MAV_PARAM_TYPE_INT64; + req.int_value = val; + } + + if (_uavcan_parameter_request_pub == nullptr) { + _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); + } else { + orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); } } break; @@ -144,6 +183,23 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) } } } + + if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 && + (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { + // publish set request to UAVCAN driver via uORB. + uavcan_parameter_request_s req; + req.message_type = msg->msgid; + req.node_id = req_read.target_component; + req.param_index = req_read.param_index; + strncpy(req.param_id, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1); + req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + + if (_uavcan_parameter_request_pub == nullptr) { + _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); + } else { + orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); + } + } break; } @@ -192,11 +248,38 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) void MavlinkParametersManager::send(const hrt_abstime t) { - /* send all parameters if requested, but only after the system has booted */ - if (_send_all_index >= 0 && _mavlink->boot_complete()) { + bool space_available = _mavlink->get_free_tx_buf() >= get_size(); + + /* Send parameter values received from the UAVCAN topic */ + if (_uavcan_parameter_value_sub < 0) { + _uavcan_parameter_value_sub = orb_subscribe(ORB_ID(uavcan_parameter_value)); + } + + bool param_value_ready; + orb_check(_uavcan_parameter_value_sub, ¶m_value_ready); + if (space_available && param_value_ready) { + struct uavcan_parameter_value_s value; + orb_copy(ORB_ID(uavcan_parameter_value), _uavcan_parameter_value_sub, &value); + + mavlink_param_value_t msg; + msg.param_count = value.param_count; + msg.param_index = value.param_index; + strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + if (value.param_type == MAV_PARAM_TYPE_REAL32) { + msg.param_type = MAVLINK_TYPE_FLOAT; + msg.param_value = value.real_value; + } else { + int32_t val; + val = (int32_t)value.int_value; + memcpy(&msg.param_value, &val, sizeof(int32_t)); + msg.param_type = MAVLINK_TYPE_INT32_T; + } + _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg, value.node_id); + } else if (_send_all_index >= 0 && _mavlink->boot_complete()) { + /* send all parameters if requested, but only after the system has booted */ /* skip if no space is available */ - if (_mavlink->get_free_tx_buf() < get_size()) { + if (!space_available) { return; } diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index 3dfed084b3..d258f3b240 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -118,4 +118,7 @@ protected: orb_advert_t _rc_param_map_pub; struct rc_parameter_map_s _rc_param_map; + + orb_advert_t _uavcan_parameter_request_pub; + int _uavcan_parameter_value_sub; }; diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk deleted file mode 100644 index e104860937..0000000000 --- a/src/modules/mavlink/mavlink_tests/module.mk +++ /dev/null @@ -1,53 +0,0 @@ -############################################################################ -# -# Copyright (c) 2014 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# System state machine tests. -# - -MODULE_COMMAND = mavlink_tests -SRCS = mavlink_tests.cpp \ - mavlink_ftp_test.cpp \ - ../mavlink_stream.cpp \ - ../mavlink_ftp.cpp \ - ../mavlink.c - -INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink - -MODULE_STACKSIZE = 5000 - -MAXOPTIMIZATION = -Os - -EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST -Wno-attributes -Wno-packed - -EXTRACFLAGS = -Wno-packed diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 2abadcafa4..0dbf81153f 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -58,11 +58,20 @@ #include #include +#include +#include + +#include + +ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s); +ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s); //todo:The Inclusion of file_server_backend is killing // #include and leaving OK undefined # define OK 0 + + /** * @file uavcan_servers.cpp * @@ -89,8 +98,25 @@ UavcanServers::UavcanServers(uavcan::INode &main_node) : _node_info_retriever(_subnode), _fw_upgrade_trigger(_subnode, _fw_version_checker), _fw_server(_subnode, _fileserver_backend), + _count_in_progress(false), + _count_index(0), + _param_in_progress(0), + _param_index(0), + _param_list_in_progress(false), + _param_list_all_nodes(false), + _param_list_node_id(1), + _cmd_in_progress(false), + _param_response_pub(nullptr), + _param_getset_client(_subnode), _mutex_inited(false), - _check_fw(false) + _check_fw(false), + _esc_enumeration_active(false), + _esc_enumeration_index(0), + _beep_pub(_subnode), + _enumeration_indication_sub(_subnode), + _enumeration_client(_subnode), + _enumeration_getset_client(_subnode), + _enumeration_save_client(_subnode) { } @@ -272,7 +298,6 @@ int UavcanServers::init() return OK; } -__attribute__((optimize("-O0"))) pthread_addr_t UavcanServers::run(pthread_addr_t) { prctl(PR_SET_NAME, "uavcan fw srv", 0); @@ -282,6 +307,27 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) /* the subscribe call needs to happen in the same thread, * so not in the constructor */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + int param_request_sub = orb_subscribe(ORB_ID(uavcan_parameter_request)); + + /* Set up shared service clients */ + _param_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_getset)); + _enumeration_client.setCallback(EnumerationBeginCallback(this, &UavcanServers::cb_enumeration_begin)); + _enumeration_indication_sub.start(EnumerationIndicationCallback(this, &UavcanServers::cb_enumeration_indication)); + _enumeration_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_enumeration_getset)); + _enumeration_save_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_enumeration_save)); + + uavcan::ServiceClient restartnode_client(_subnode); + restartnode_client.setCallback(RestartNodeCallback(this, &UavcanServers::cb_restart)); + + uavcan::ServiceClient opcode_client(_subnode); + opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_opcode)); + + _count_in_progress = _param_in_progress = _param_list_in_progress = _cmd_in_progress = _param_list_all_nodes = false; + memset(_param_counts, 0, sizeof(_param_counts)); + + _esc_enumeration_active = false; + memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids)); + _esc_enumeration_index = 0; while (1) { @@ -290,21 +336,200 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) _node_info_retriever.invalidateAll(); } - const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(100)); + const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(10)); + + // Check for parameter requests (get/set/list) + bool param_request_ready; + orb_check(param_request_sub, ¶m_request_ready); + + if (param_request_ready && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) { + struct uavcan_parameter_request_s request; + orb_copy(ORB_ID(uavcan_parameter_request), param_request_sub, &request); + + if (_param_counts[request.node_id]) { + /* + * We know how many parameters are exposed by this node, so + * process the request. + */ + if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_READ) { + uavcan::protocol::param::GetSet::Request req; + if (request.param_index >= 0) { + req.index = request.param_index; + } else { + req.name = (char*)request.param_id; + } + + int call_res = _param_getset_client.call(request.node_id, req); + if (call_res < 0) { + warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res); + } 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; + if (request.param_index >= 0) { + req.index = request.param_index; + } else { + req.name = (char*)request.param_id; + } + + if (request.param_type == MAV_PARAM_TYPE_REAL32) { + req.value.to() = request.real_value; + } else if (request.param_type == MAV_PARAM_TYPE_UINT8) { + req.value.to() = request.int_value; + } else { + req.value.to() = request.int_value; + } + + int call_res = _param_getset_client.call(request.node_id, req); + if (call_res < 0) { + warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res); + } 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. + _param_index = 0; + _param_list_in_progress = true; + _param_list_node_id = request.node_id; + _param_list_all_nodes = false; + + warnx("UAVCAN command bridge: starting component-specific param list"); + } + } else if (request.node_id == MAV_COMP_ID_ALL) { + if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) { + /* + * This triggers the _param_list_in_progress case below, + * but additionally iterates over all active nodes. + */ + _param_index = 0; + _param_list_in_progress = true; + _param_list_node_id = get_next_active_node_id(1); + _param_list_all_nodes = true; + + warnx("UAVCAN command bridge: starting global param list"); + + if (_param_counts[_param_list_node_id.get()] == 0) { + param_count(_param_list_node_id); + } + } + } else { + /* + * Need to know how many parameters this node has before we can + * continue; count them now and then process the request. + */ + param_count(request.node_id); + } + } + + // 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()]) { + // 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) { + /* + * 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) { + 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"); + } + } + } + } + + // Check if we're still listing, and need to get the next parameter + if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) { + // Ready to request the next value -- _param_index is incremented + // after each successful fetch by cb_getset + uavcan::protocol::param::GetSet::Request req; + req.index = _param_index; + + 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); + } else { + _param_in_progress = true; + warnx("UAVCAN command bridge: sent GetSet during param list operation"); + } + } - /* check if new commands are pending */ + // Check for ESC enumeration commands bool cmd_ready; orb_check(cmd_sub, &cmd_ready); - if (cmd_ready) { + if (cmd_ready && !_cmd_in_progress) { struct vehicle_command_s cmd; orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) { - warnx("received UAVCAN CONFIG command"); - - if (static_cast(cmd.param1 + 0.5f) == 1) { - warnx("UAVCAN CONFIG: Actuator assignment requested"); + int command_id = static_cast(cmd.param1 + 0.5f); + int node_id = static_cast(cmd.param2 + 0.5f); + int call_res; + + warnx("UAVCAN command bridge: received command ID %d, node ID %d", command_id, node_id); + + switch (command_id) { + case 0: + case 1: { + _esc_enumeration_active = command_id; + _esc_enumeration_index = 0; + _esc_count = 0; + uavcan::protocol::enumeration::Begin::Request req; + req.parameter_name = "esc_index"; + req.timeout_sec = _esc_enumeration_active ? 65535 : 0; + call_res = _enumeration_client.call(get_next_active_node_id(1), req); + if (call_res < 0) { + warnx("UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res); + } + break; + } + case 2: { + // Command is a restart node request + uavcan::protocol::RestartNode::Request restart_req; + restart_req.magic_number = restart_req.MAGIC_NUMBER; + call_res = restartnode_client.call(node_id, restart_req); + if (call_res < 0) { + warnx("UAVCAN command bridge: couldn't send RestartNode: %d", call_res); + } else { + _cmd_in_progress = true; + warnx("UAVCAN command bridge: sent RestartNode"); + } + break; + } + case 3: + case 4: { + // Command is a param save or erase request + uavcan::protocol::param::ExecuteOpcode::Request opcode_req; + opcode_req.opcode = command_id == 3 ? opcode_req.OPCODE_SAVE : opcode_req.OPCODE_ERASE; + call_res = opcode_client.call(node_id, opcode_req); + if (call_res < 0) { + warnx("UAVCAN command bridge: couldn't send ExecuteOpcode: %d", call_res); + } else { + _cmd_in_progress = true; + warnx("UAVCAN command bridge: sent ExecuteOpcode"); + } + break; + } + default: { + warnx("UAVCAN command bridge: unknown command ID %d", command_id); + break; + } } } } @@ -318,3 +543,249 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) return (pthread_addr_t) 0; } +void UavcanServers::cb_restart(const uavcan::ServiceCallResult &result) +{ + bool success = result.isSuccessful(); + uavcan::protocol::RestartNode::Response resp = result.getResponse(); + success &= resp.ok; + _cmd_in_progress = false; +} + +void UavcanServers::cb_opcode(const uavcan::ServiceCallResult &result) +{ + bool success = result.isSuccessful(); + uavcan::protocol::param::ExecuteOpcode::Response resp = result.getResponse(); + success &= resp.ok; + _cmd_in_progress = false; +} + +void UavcanServers::cb_getset(const uavcan::ServiceCallResult &result) +{ + if (_count_in_progress) { + /* + * Currently in parameter count mode: + * Iterate over all parameters for the node to which the request was + * originally sent, in order to find the maximum parameter ID. If a + * request fails, set the node's parameter count to zero. + */ + uint8_t node_id = result.getCallID().server_node_id.get(); + + if (result.isSuccessful()) { + warnx("UAVCAN command bridge: successful GetSet response during param count"); + + uavcan::protocol::param::GetSet::Response resp = result.getResponse(); + if (resp.name.size()) { + _param_counts[node_id] = _count_index++; + + uavcan::protocol::param::GetSet::Request req; + req.index = _count_index; + + int call_res = _param_getset_client.call(result.getCallID().server_node_id, req); + if (call_res < 0) { + _count_in_progress = false; + _count_index = 0; + warnx("UAVCAN command bridge: couldn't send GetSet during param count: %d", call_res); + } else { + warnx("UAVCAN command bridge: sent GetSet during param count"); + } + } else { + _count_in_progress = false; + _count_index = 0; + warnx("UAVCAN command bridge: completed param count for node %hhu: %hhu", node_id, _param_counts[node_id]); + } + } else { + _param_counts[node_id] = 0; + _count_in_progress = false; + _count_index = 0; + warnx("UAVCAN command bridge: GetSet error during param count"); + } + } else { + /* + * Currently in parameter get/set mode: + * Publish a uORB uavcan_parameter_value message containing the current value + * of the parameter. + */ + if (result.isSuccessful()) { + uavcan::protocol::param::GetSet::Response param = result.getResponse(); + + struct uavcan_parameter_value_s response; + response.node_id = result.getCallID().server_node_id.get(); + strncpy(response.param_id, param.name.c_str(), sizeof(response.param_id) - 1); + response.param_id[16] = '\0'; + response.param_index = _param_index; + response.param_count = _param_counts[response.node_id]; + + if (param.value.is(uavcan::protocol::param::Value::Tag::integer_value)) { + response.param_type = MAV_PARAM_TYPE_INT64; + response.int_value = param.value.to(); + } else if (param.value.is(uavcan::protocol::param::Value::Tag::real_value)) { + response.param_type = MAV_PARAM_TYPE_REAL32; + response.real_value = param.value.to(); + } else if (param.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) { + response.param_type = MAV_PARAM_TYPE_UINT8; + response.int_value = param.value.to(); + } + + 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 { + orb_publish(ORB_ID(uavcan_parameter_value), _param_response_pub, &response); + } + } else { + warnx("UAVCAN command bridge: GetSet error"); + } + + _param_in_progress = false; + _param_index++; + } +} + +void UavcanServers::param_count(uavcan::NodeID node_id) +{ + uavcan::protocol::param::GetSet::Request req; + req.index = 0; + int call_res = _param_getset_client.call(node_id, req); + if (call_res < 0) { + warnx("UAVCAN command bridge: couldn't start parameter count: %d", call_res); + } else { + _count_in_progress = true; + _count_index = 0; + warnx("UAVCAN command bridge: starting param count"); + } +} + +uavcan::NodeID UavcanServers::get_next_active_node_id(const uavcan::NodeID &base) +{ + for (int i = base.get(); i < 128; i++) { + if (_node_info_retriever.isNodeKnown(i) && _subnode.getNodeID() != i) { + return uavcan::NodeID(i); + } + } + 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); + + if (!result.isSuccessful()) { + warnx("UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get()); + } else if (result.getResponse().error) { + warnx("UAVCAN ESC enumeration: begin request for node %hhu rejected: %hhu", result.getCallID().server_node_id.get(), result.getResponse().error); + } else { + _esc_count++; + warnx("UAVCAN ESC enumeration: begin request for node %hhu completed OK.", result.getCallID().server_node_id.get()); + } + + if (next_id != result.getCallID().server_node_id) { + // Still other active nodes to send the request to + uavcan::protocol::enumeration::Begin::Request req; + req.parameter_name = "esc_index"; + req.timeout_sec = _esc_enumeration_active ? 65535 : 0; + + int call_res = _enumeration_client.call(next_id, req); + if (call_res < 0) { + warnx("UAVCAN ESC enumeration: couldn't send Begin request: %d", call_res); + } else { + warnx("UAVCAN ESC enumeration: sent Begin request"); + } + } else { + warnx("UAVCAN ESC enumeration: completed enumeration on all nodes."); + } +} + +void UavcanServers::cb_enumeration_indication(const uavcan::ReceivedDataStructure &msg) +{ + // Called whenever an ESC thinks it has received user input. + warnx("UAVCAN ESC enumeration: got indication"); + + if (!_esc_enumeration_active) { + // Ignore any messages received when we're not expecting them + return; + } + + // First, check if we've already seen an indication from this ESC. If so, + // just re-issue the previous get/set request. + int i; + for (i = 0; i < _esc_enumeration_index; i++) { + if (_esc_enumeration_ids[i] == msg.getSrcNodeID().get()) { + warnx("UAVCAN ESC enumeration: already enumerated ESC ID %hhu as index %d", _esc_enumeration_ids[i], i); + break; + } + } + + uavcan::protocol::param::GetSet::Request req; + req.name = "esc_index"; + req.value.to() = i; + + int call_res = _enumeration_getset_client.call(msg.getSrcNodeID(), req); + if (call_res < 0) { + warnx("UAVCAN ESC enumeration: couldn't send GetSet: %d", call_res); + } else { + warnx("UAVCAN ESC enumeration: sent GetSet to node %hhu (index %d)", _esc_enumeration_ids[i], i); + } +} + +void UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult &result) +{ + if (!result.isSuccessful()) { + warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get()); + } else { + warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get()); + + uavcan::protocol::param::GetSet::Response resp = result.getResponse(); + uint8_t esc_index = (uint8_t)resp.value.to(); + 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_ids[esc_index] = result.getCallID().server_node_id.get(); + + uavcan::protocol::param::ExecuteOpcode::Request opcode_req; + opcode_req.opcode = opcode_req.OPCODE_SAVE; + int call_res = _enumeration_save_client.call(result.getCallID().server_node_id, opcode_req); + if (call_res < 0) { + warnx("UAVCAN ESC enumeration: couldn't send ExecuteOpcode: %d", call_res); + } else { + warnx("UAVCAN ESC enumeration: sent ExecuteOpcode to node %hhu (index %hhu)", _esc_enumeration_ids[esc_index], esc_index); + } + } +} + +void UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult &result) +{ + uavcan::equipment::indication::BeepCommand beep; + + if (!result.isSuccessful()) { + warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get()); + beep.frequency = 880.0f; + beep.duration = 1.0f; + } else if (!result.getResponse().ok) { + warnx("UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get()); + beep.frequency = 880.0f; + beep.duration = 1.0f; + } else { + warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get()); + beep.frequency = 440.0f; + beep.duration = 0.25f; + } + + (void)_beep_pub.broadcast(beep); + + if (_esc_enumeration_index == uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1 || + _esc_enumeration_index == _esc_count - 1) { + _esc_enumeration_active = false; + + // Tell all ESCs to stop enumerating + uavcan::protocol::enumeration::Begin::Request req; + req.parameter_name = "esc_index"; + req.timeout_sec = 0; + int call_res = _enumeration_client.call(get_next_active_node_id(1), req); + if (call_res < 0) { + warnx("UAVCAN ESC enumeration: couldn't send Begin request to stop enumeration: %d", call_res); + } else { + warnx("UAVCAN ESC enumeration: sent Begin request to stop enumeration"); + } + } +} diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp index 9d21b8f93f..78de54ccaf 100644 --- a/src/modules/uavcan/uavcan_servers.hpp +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -42,16 +42,20 @@ #include #include -# include -# include -# include -# include -# include -# include -# include -# include - -# include "uavcan_virtual_can_driver.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "uavcan_virtual_can_driver.hpp" /** * @file uavcan_servers.hpp @@ -140,7 +144,71 @@ private: uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger; uavcan::BasicFileServer _fw_server; + /* + * The MAVLink parameter bridge needs to know the maximum parameter index + * of each node so that clients can determine when parameter listings have + * finished. We do that by querying a node's entire parameter set whenever + * a parameter message is received for a node with a zero _param_count, + * and storing the count here. If a node doesn't respond, or doesn't have + * any parameters, its count will stay at zero and we'll try to query the + * set again next time. + * + * The node's UAVCAN ID is used as the index into the _param_counts array. + */ + uint16_t _param_counts[128]; + bool _count_in_progress; + uint16_t _count_index; + + bool _param_in_progress; + uint16_t _param_index; + bool _param_list_in_progress; + bool _param_list_all_nodes; + uavcan::NodeID _param_list_node_id; + + bool _cmd_in_progress; + + // uORB topic handle for MAVLink parameter responses + orb_advert_t _param_response_pub; + + typedef uavcan::MethodBinder &)> GetSetCallback; + typedef uavcan::MethodBinder &)> ExecuteOpcodeCallback; + typedef uavcan::MethodBinder &)> RestartNodeCallback; + void cb_getset(const uavcan::ServiceCallResult &result); + void cb_count(const uavcan::ServiceCallResult &result); + void cb_opcode(const uavcan::ServiceCallResult &result); + void cb_restart(const uavcan::ServiceCallResult &result); + + uavcan::ServiceClient _param_getset_client; + void param_count(uavcan::NodeID node_id); + + uavcan::NodeID get_next_active_node_id(const uavcan::NodeID &base); + bool _mutex_inited; volatile bool _check_fw; + // ESC enumeration + bool _esc_enumeration_active; + uint8_t _esc_enumeration_ids[uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize]; + uint8_t _esc_enumeration_index; + uint8_t _esc_set_index; + uint8_t _esc_count; + + typedef uavcan::MethodBinder &)> EnumerationBeginCallback; + typedef uavcan::MethodBinder&)> + EnumerationIndicationCallback; + void cb_enumeration_begin(const uavcan::ServiceCallResult &result); + void cb_enumeration_indication(const uavcan::ReceivedDataStructure &msg); + void cb_enumeration_getset(const uavcan::ServiceCallResult &result); + void cb_enumeration_save(const uavcan::ServiceCallResult &result); + + uavcan::Publisher _beep_pub; + uavcan::Subscriber _enumeration_indication_sub; + uavcan::ServiceClient _enumeration_client; + uavcan::ServiceClient _enumeration_getset_client; + uavcan::ServiceClient _enumeration_save_client; };