yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added MAVLink/UAVCAN parameter bridge; implemented UAVCAN ESC enumeration

Browse Source
This commit is contained in:
Ben Dyer
2015-10-06 17:52:11 +11:00
parent a3b863bdd9
commit 250c912704
7 changed files with 689 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
msg/uavcan_parameter_request.msg Normal file
View File

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

8
msg/uavcan_parameter_value.msg Normal file
View File

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

101
src/modules/mavlink/mavlink_parameters.cpp
View File

@@ -41,25 +41,25 @@
#include <stdio.h> #include <stdio.h>
#include <uORB/topics/uavcan_parameter_request.h>
#include <uORB/topics/uavcan_parameter_value.h>
#include "mavlink_parameters.h" #include "mavlink_parameters.h"
#include "mavlink_main.h" #include "mavlink_main.h"
MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_send_all_index(-1), _send_all_index(-1),
_rc_param_map_pub(nullptr), _rc_param_map_pub(nullptr),
_rc_param_map() _rc_param_map(),
_uavcan_parameter_request_pub(nullptr),
_uavcan_parameter_value_sub(-1)
{ {
} }
unsigned unsigned
MavlinkParametersManager::get_size() MavlinkParametersManager::get_size()
{ {
if (_send_all_index >= 0) {
return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
} else {
return 0;
}
} }
void void
@@ -76,12 +76,26 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
_send_all_index = 0; _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; break;
} }
case MAVLINK_MSG_ID_PARAM_SET: { case MAVLINK_MSG_ID_PARAM_SET: {
/* set parameter */ /* set parameter */
if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
mavlink_param_set_t set; mavlink_param_set_t set;
mavlink_msg_param_set_decode(msg, &set); mavlink_msg_param_set_decode(msg, &set);
@@ -107,6 +121,31 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
send_param(param); 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; 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; break;
} }
@@ -192,11 +248,38 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
void void
MavlinkParametersManager::send(const hrt_abstime t) MavlinkParametersManager::send(const hrt_abstime t)
{ {
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, &param_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 */ /* send all parameters if requested, but only after the system has booted */
if (_send_all_index >= 0 && _mavlink->boot_complete()) {
/* skip if no space is available */ /* skip if no space is available */
if (_mavlink->get_free_tx_buf() < get_size()) { if (!space_available) {
return; return;
} }

3
src/modules/mavlink/mavlink_parameters.h
View File

@@ -118,4 +118,7 @@ protected:
orb_advert_t _rc_param_map_pub; orb_advert_t _rc_param_map_pub;
struct rc_parameter_map_s _rc_param_map; struct rc_parameter_map_s _rc_param_map;
orb_advert_t _uavcan_parameter_request_pub;
int _uavcan_parameter_value_sub;
}; };

53
src/modules/mavlink/mavlink_tests/module.mk
View File

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

487
src/modules/uavcan/uavcan_servers.cpp
View File

@@ -58,11 +58,20 @@
#include <uavcan_posix/firmware_version_checker.hpp> #include <uavcan_posix/firmware_version_checker.hpp>
#include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_command.h>
#include <uORB/topics/uavcan_parameter_request.h>
#include <uORB/topics/uavcan_parameter_value.h>
#include <mavlink/v1.0/common/mavlink.h>
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 //todo:The Inclusion of file_server_backend is killing
// #include <sys/types.h> and leaving OK undefined // #include <sys/types.h> and leaving OK undefined
# define OK 0 # define OK 0
/** /**
* @file uavcan_servers.cpp * @file uavcan_servers.cpp
* *
@@ -89,8 +98,25 @@ UavcanServers::UavcanServers(uavcan::INode &main_node) :
_node_info_retriever(_subnode), _node_info_retriever(_subnode),
_fw_upgrade_trigger(_subnode, _fw_version_checker), _fw_upgrade_trigger(_subnode, _fw_version_checker),
_fw_server(_subnode, _fileserver_backend), _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), _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; return OK;
} }
__attribute__((optimize("-O0")))
pthread_addr_t UavcanServers::run(pthread_addr_t) pthread_addr_t UavcanServers::run(pthread_addr_t)
{ {
prctl(PR_SET_NAME, "uavcan fw srv", 0); 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, /* the subscribe call needs to happen in the same thread,
* so not in the constructor */ * so not in the constructor */
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); 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<uavcan::protocol::RestartNode, RestartNodeCallback> restartnode_client(_subnode);
restartnode_client.setCallback(RestartNodeCallback(this, &UavcanServers::cb_restart));
uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> 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) { while (1) {
@@ -290,21 +336,200 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
_node_info_retriever.invalidateAll(); _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 if new commands are pending */ // Check for parameter requests (get/set/list)
bool param_request_ready;
orb_check(param_request_sub, &param_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<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value;
} else if (request.param_type == MAV_PARAM_TYPE_UINT8) {
req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int_value;
} else {
req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = 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 for ESC enumeration commands
bool cmd_ready; bool cmd_ready;
orb_check(cmd_sub, &cmd_ready); orb_check(cmd_sub, &cmd_ready);
if (cmd_ready) { if (cmd_ready && !_cmd_in_progress) {
struct vehicle_command_s cmd; struct vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) { if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) {
warnx("received UAVCAN CONFIG command"); int command_id = static_cast<int>(cmd.param1 + 0.5f);
int node_id = static_cast<int>(cmd.param2 + 0.5f);
int call_res;
if (static_cast<int>(cmd.param1 + 0.5f) == 1) { warnx("UAVCAN command bridge: received command ID %d, node ID %d", command_id, node_id);
warnx("UAVCAN CONFIG: Actuator assignment requested");
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; return (pthread_addr_t) 0;
} }
void UavcanServers::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &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<uavcan::protocol::param::ExecuteOpcode> &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<uavcan::protocol::param::GetSet> &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<uavcan::protocol::param::Value::Tag::integer_value>();
} 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<uavcan::protocol::param::Value::Tag::real_value>();
} 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<uavcan::protocol::param::Value::Tag::boolean_value>();
}
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<uavcan::protocol::enumeration::Begin> &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<uavcan::protocol::enumeration::Indication> &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<uavcan::protocol::param::Value::Tag::integer_value>() = 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<uavcan::protocol::param::GetSet> &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<uavcan::protocol::param::Value::Tag::integer_value>();
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<uavcan::protocol::param::ExecuteOpcode> &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");
}
}
}

68
src/modules/uavcan/uavcan_servers.hpp
View File

@@ -50,6 +50,10 @@
#include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp> #include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
#include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp> #include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
#include <uavcan_posix/firmware_version_checker.hpp> #include <uavcan_posix/firmware_version_checker.hpp>
#include <uavcan/equipment/esc/RawCommand.hpp>
#include <uavcan/equipment/indication/BeepCommand.hpp>
#include <uavcan/protocol/enumeration/Begin.hpp>
#include <uavcan/protocol/enumeration/Indication.hpp>
#include "uavcan_virtual_can_driver.hpp" #include "uavcan_virtual_can_driver.hpp"
@@ -140,7 +144,71 @@ private:
uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger; uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger;
uavcan::BasicFileServer _fw_server; 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<UavcanServers *,
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &)> GetSetCallback;
typedef uavcan::MethodBinder<UavcanServers *,
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &)> ExecuteOpcodeCallback;
typedef uavcan::MethodBinder<UavcanServers *,
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
void cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
void cb_count(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
void cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
void cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result);
uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _param_getset_client;
void param_count(uavcan::NodeID node_id);
uavcan::NodeID get_next_active_node_id(const uavcan::NodeID &base);
bool _mutex_inited; bool _mutex_inited;
volatile bool _check_fw; 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<UavcanServers *,
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &)> EnumerationBeginCallback;
typedef uavcan::MethodBinder<UavcanServers *,
void (UavcanServers::*)(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication>&)>
EnumerationIndicationCallback;
void cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result);
void cb_enumeration_indication(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication> &msg);
void cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
void cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
uavcan::Publisher<uavcan::equipment::indication::BeepCommand> _beep_pub;
uavcan::Subscriber<uavcan::protocol::enumeration::Indication, EnumerationIndicationCallback> _enumeration_indication_sub;
uavcan::ServiceClient<uavcan::protocol::enumeration::Begin, EnumerationBeginCallback> _enumeration_client;
uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _enumeration_getset_client;
uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _enumeration_save_client;
}; };