From 42031ab572e4a0333a4d9c402ddd42c505634a06 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 6 Aug 2016 15:14:54 +0300 Subject: [PATCH] Better beeping during ESC enumeration --- src/modules/uavcan/uavcan_servers.cpp | 22 ++++++++++++---------- src/modules/uavcan/uavcan_servers.hpp | 2 ++ 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 7e372a1d91..2fddf0af77 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -504,6 +504,7 @@ 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, 0.5F); uavcan::protocol::enumeration::Begin::Request req; req.parameter_name = "esc_index"; req.timeout_sec = _esc_enumeration_active ? 65535 : 0; @@ -751,6 +752,14 @@ uint8_t UavcanServers::get_next_dirty_node_id(uint8_t base) return base; } +void UavcanServers::beep(float frequency, float duration) +{ + uavcan::equipment::indication::BeepCommand cmd; + cmd.frequency = 500.0f; + cmd.duration = 0.5f; + (void)_beep_pub.broadcast(cmd); +} + void UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult &result) { uint8_t next_id = get_next_active_node_id(result.getCallID().server_node_id.get()); @@ -840,24 +849,17 @@ void UavcanServers::cb_enumeration_getset(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; + beep(100.0F, 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; + beep(100.0F, 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; + beep(2000.0F, 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 || diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp index 17e94cd311..d3c6de8170 100644 --- a/src/modules/uavcan/uavcan_servers.hpp +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -179,6 +179,8 @@ private: void clear_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] &= ~(1 << (node_id & 31)); } bool are_node_params_dirty(uint8_t node_id) const { return bool((_param_dirty_bitmap[node_id >> 5] >> (node_id & 31)) & 1); } + void beep(float frequency, float duration); + bool _mutex_inited; volatile bool _check_fw;