fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-29 15:34:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Improved uavcan activity indication

Browse Source
This commit is contained in:
Pavel Kirienko 2016-08-07 13:39:41 +03:00 committed by Lorenz Meier
parent 64d7946cdc
commit 2784c3c5f0
2 changed files with 22 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -504,7 +504,6 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
_esc_enumeration_active = command_id;
_esc_enumeration_index = 0;
_esc_count = 0;
beep(_esc_enumeration_active ? 500.0F : 1000.0F);
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.
@ -513,6 +512,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
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);
beep(BeepFrequencyError);
} else {
beep(BeepFrequencyGenericIndication);
}
break;
}
@ -596,11 +598,13 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::
_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;
@ -804,13 +808,12 @@ void UavcanServers::cb_enumeration_indication(const uavcan::ReceivedDataStructur
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++) {
// First, check if we've already seen an indication from this ESC. If so, just ignore this indication.
int i = 0;
for (; 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;
warnx("UAVCAN ESC enumeration: already enumerated ESC ID %hhu as index %d, ignored", _esc_enumeration_ids[i], i);
return;
}
}
@ -853,21 +856,24 @@ void UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan
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(100.0F);
beep(BeepFrequencyError);
} else if (!result.getResponse().ok) {
warnx("UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get());
beep(100.0F);
beep(BeepFrequencyError);
} else {
warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
beep(2000.0F);
beep(this_is_the_last_one ? BeepFrequencySuccess : BeepFrequencyGenericIndication);
}
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) {
if (this_is_the_last_one) {
_esc_enumeration_active = false;
// Tell all ESCs to stop enumerating

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

@ -79,6 +79,10 @@ class UavcanServers
static constexpr unsigned VirtualIfaceBlockAllocationQuota = 80;
static constexpr float BeepFrequencyGenericIndication = 1000.0F;
static constexpr float BeepFrequencySuccess = 2000.0F;
static constexpr float BeepFrequencyError = 100.0F;
public:
UavcanServers(uavcan::INode &main_node);