|
|
|
|
@ -58,11 +58,20 @@
@@ -58,11 +58,20 @@
|
|
|
|
|
#include <uavcan_posix/firmware_version_checker.hpp> |
|
|
|
|
|
|
|
|
|
#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
|
|
|
|
|
// #include <sys/types.h> and leaving OK undefined
|
|
|
|
|
# define OK 0 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* @file uavcan_servers.cpp |
|
|
|
|
* |
|
|
|
|
@ -89,8 +98,25 @@ UavcanServers::UavcanServers(uavcan::INode &main_node) :
@@ -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()
@@ -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)
@@ -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<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) { |
|
|
|
|
|
|
|
|
|
@ -290,21 +336,200 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
@@ -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<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 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<int>(cmd.param1 + 0.5f) == 1) { |
|
|
|
|
warnx("UAVCAN CONFIG: Actuator assignment requested"); |
|
|
|
|
int command_id = static_cast<int>(cmd.param1 + 0.5f); |
|
|
|
|
int node_id = static_cast<int>(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)
@@ -318,3 +543,249 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
|
|
|
|
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"); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
Ben Dyer
9 years ago