void handleResponse(const uavcan::ServiceCallResult<DataType>& result)
{
std::cout << result << std::endl;
last_status = result.getStatus();
last_response = result.getResponse();
last_server_node_id = result.getCallID().server_node_id;
responses.push(result.getResponse());
}
void UavcanNode::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
{
uavcan::protocol::RestartNode::Response resp;
_callback_success = result.isSuccessful();
resp = result.getResponse();
_callback_success &= resp.ok;
}
void UavcanNode::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
{
uavcan::protocol::param::ExecuteOpcode::Response resp;
_callback_success = result.isSuccessful();
resp = result.getResponse();
_callback_success &= resp.ok;
}
void UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result)
{
uint8_t next_id = get_next_active_node_id(result.getCallID().server_node_id.get());
if (!result.isSuccessful()) {
warnx("UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get());
} 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 < 128) {
// Still other active nodes to send the request to
uavcan::protocol::enumeration::Begin::Request req;
// TODO: Incorrect implementation; the parameter name field should be left empty.
// Leaving it as-is to avoid breaking compatibility with non-compliant nodes.
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: begun enumeration on all nodes.");
}
}
void handleReadResponse(const uavcan::ServiceCallResult<uavcan::protocol::file::Read>& result)
{
if (result.isSuccessful() &&
result.getResponse().error.value == 0)
{
auto& data = result.getResponse().data;
image_.insert(image_.end(), data.begin(), data.end());
if (data.size() < data.capacity()) // Termination condition
{
status_ = Status::Success;
uavcan::TimerBase::stop();
}
}
else
{
status_ = Status::Failure;
uavcan::TimerBase::stop();
}
}
void UavcanServers::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
{
bool success = result.isSuccessful();
uint8_t node_id = result.getCallID().server_node_id.get();
uavcan::protocol::param::ExecuteOpcode::Response resp = result.getResponse();
success &= resp.ok;
_cmd_in_progress = false;
if (!result.isSuccessful()) {
warnx("UAVCAN command bridge: save request for node %hhu timed out.", node_id);
} else if (!result.getResponse().ok) {
warnx("UAVCAN command bridge: save request for node %hhu rejected.", node_id);
} else {
warnx("UAVCAN command bridge: save request for node %hhu completed OK, restarting.", node_id);
uavcan::protocol::RestartNode::Request restart_req;
restart_req.magic_number = restart_req.MAGIC_NUMBER;
int call_res = _param_restartnode_client.call(node_id, restart_req);
if (call_res < 0) {
warnx("UAVCAN command bridge: couldn't send RestartNode: %d", call_res);
} else {
warnx("UAVCAN command bridge: sent RestartNode");
_cmd_in_progress = true;
}
}
if (!_cmd_in_progress) {
/*
* Something went wrong, so cb_restart is never going to be called as a result of this request.
* To ensure we try to execute the opcode on all nodes that permit it, get the next dirty node
* ID and keep processing here. The dirty bit on the current node is still set, so the
* save/erase attempt will occur when the next save/erase command is received over MAVLink.
*/
node_id = get_next_dirty_node_id(node_id);
if (node_id < 128) {
param_opcode(node_id);
}
}
}
void UavcanServers::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
{
bool success = result.isSuccessful();
uint8_t node_id = result.getCallID().server_node_id.get();
uavcan::protocol::RestartNode::Response resp = result.getResponse();
success &= resp.ok;
_cmd_in_progress = false;
if (success) {
warnx("UAVCAN command bridge: restart request for node %hhu completed OK.", node_id);
// Clear the dirty flag
clear_node_params_dirty(node_id);
} else {
warnx("UAVCAN command bridge: restart request for node %hhu failed.", node_id);
}
// Get the next dirty node ID and send the same command to it
node_id = get_next_dirty_node_id(node_id);
if (node_id < 128) {
param_opcode(node_id);
}
}
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);
warnx("UAVCAN ESC enumeration: completed %hhu of %hhu", _esc_enumeration_index, _esc_count);
if (_esc_enumeration_index == uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1 ||
_esc_enumeration_index == _esc_count) {
_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");
}
}
}
void UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
{
const bool this_is_the_last_one =
_esc_enumeration_index >= uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1 ||
_esc_enumeration_index >= _esc_count;
if (!result.isSuccessful()) {
warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
beep(BeepFrequencyError);
} else if (!result.getResponse().ok) {
warnx("UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get());
beep(BeepFrequencyError);
} else {
warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
beep(this_is_the_last_one ? BeepFrequencySuccess : BeepFrequencyGenericIndication);
}
warnx("UAVCAN ESC enumeration: completed %hhu of %hhu", _esc_enumeration_index, _esc_count);
if (this_is_the_last_one) {
_esc_enumeration_active = false;
// Tell all ESCs to stop enumerating
uavcan::protocol::enumeration::Begin::Request req;
// TODO: Incorrect implementation; the parameter name field should be left empty.
// Leaving it as-is to avoid breaking compatibility with non-compliant nodes.
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");
}
}
}
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, (uint8_t)(esc_index + 1));
_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 UavcanNode::cb_setget(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
{
_callback_success = result.isSuccessful();
*_setget_response = result.getResponse();
}
void handler(const uavcan::ServiceCallResult<DataType>& tmp_result)
{
std::cout << tmp_result << std::endl;
result.reset(new Result(tmp_result.getStatus(), tmp_result.getCallID(), tmp_result.getResponse()));
}
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()) {
uavcan::protocol::param::GetSet::Response resp = result.getResponse();
if (resp.name.size()) {
_count_index++;
_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);
beep(BeepFrequencyError);
}
} 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]);
beep(BeepFrequencyGenericIndication);
}
} 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>();
}
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++;
}
}