From b86f0d90065484a03f518f6609228e7f98aabbdb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=B4=A2=E6=9D=B0?= Date: Mon, 20 Apr 2020 16:28:06 +0800 Subject: [PATCH 1/7] Add CAN beep --- src/drivers/uavcan/CMakeLists.txt | 1 + src/drivers/uavcan/sensors/beep.cpp | 106 ++++++++++++++++++++++++++++ src/drivers/uavcan/sensors/beep.hpp | 92 ++++++++++++++++++++++++ src/drivers/uavcan/uavcan_main.cpp | 7 ++ src/drivers/uavcan/uavcan_main.hpp | 3 + 5 files changed, 209 insertions(+) create mode 100644 src/drivers/uavcan/sensors/beep.cpp create mode 100644 src/drivers/uavcan/sensors/beep.hpp diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index b176d8bf8ba5..832c74f9a78b 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -130,6 +130,7 @@ px4_add_module( sensors/flow.cpp sensors/gnss.cpp sensors/mag.cpp + sensors/beep.cpp DEPENDS px4_uavcan_dsdlc diff --git a/src/drivers/uavcan/sensors/beep.cpp b/src/drivers/uavcan/sensors/beep.cpp new file mode 100644 index 000000000000..bc7f1e90048e --- /dev/null +++ b/src/drivers/uavcan/sensors/beep.cpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (c) 2014, 2015 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. + * + ****************************************************************************/ + +/** + * @file beep.cpp + * + * @author CUAVcaijie + */ + + +#include "beep.hpp" + +UavcanUavcanBeep::UavcanUavcanBeep(uavcan::INode &node) : + _beep_pub(node), + _timer(node) +{ +} + +int +UavcanUavcanBeep::init() +{ + /* + * Setup timer and call back function for periodic updates + */ + + if (!_timer.isRunning()) { + _timer.setCallback(TimerCbBinder(this, &UavcanUavcanBeep::periodic_update)); + _timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ)); + } + + return 0; +} + +void +UavcanUavcanBeep::periodic_update(const uavcan::TimerEvent &) +{ + if (_tune_control_sub.updated()) { + _tune_control_sub.copy(&_tune); + + if (_tune.timestamp > 0) { + _play_tone = _tunes.set_control(_tune) == 0; + } + } + + if ((hrt_absolute_time() - interval_timestamp <= duration) + || (!_play_tone)) { + return; + } + + interval_timestamp = hrt_absolute_time(); + + if (_silence_length > 0) { + duration = _silence_length; + _silence_length = 0; + + } else if (_play_tone) { + int parse_ret_val = _tunes.get_next_note(frequency, duration, _silence_length); + + if (parse_ret_val > 0) { + // Continue playing. + _play_tone = true; + + if (frequency > 0) { + // Start playing the note. + uavcan::equipment::indication::BeepCommand cmd; + cmd.frequency = frequency; + cmd.duration = duration / 1000000.f; + (void)_beep_pub.broadcast(cmd); + } + + } else { + _play_tone = false; + } + + } +} diff --git a/src/drivers/uavcan/sensors/beep.hpp b/src/drivers/uavcan/sensors/beep.hpp new file mode 100644 index 000000000000..6cdce1829f28 --- /dev/null +++ b/src/drivers/uavcan/sensors/beep.hpp @@ -0,0 +1,92 @@ +/**************************************************************************** +* +* Copyright (c) 2014, 2015 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. +* +****************************************************************************/ + +/** + * @file beep.cpp + * + * @author CUAVcaijie + */ + + +#pragma once + +#include +#include +#include +#include +#include +#include + +/** + * @brief The beep class + */ + +class UavcanUavcanBeep +{ +public: + UavcanUavcanBeep(uavcan::INode &node); + + /* + * setup periodic updater + */ + int init(); + +private: + /* + * Max update rate to avoid exessive bus traffic + */ + static constexpr unsigned MAX_RATE_HZ = 100; + + void periodic_update(const uavcan::TimerEvent &); + + typedef uavcan::MethodBinder + TimerCbBinder; + + uORB::Subscription _tune_control_sub{ORB_ID(tune_control)}; + + /* + * libuavcan related things + */ + uavcan::Publisher _beep_pub; + + uavcan::TimerEventForwarder _timer; + + hrt_abstime interval_timestamp{0}; + tune_control_s _tune{}; + Tunes _tunes = Tunes(); + bool _play_tone{false}; + unsigned int _silence_length{0}; ///< If nonzero, silence before next note. + unsigned int frequency{0}; + unsigned int duration{0}; + +}; diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 97fff6e87328..276cc7d27ed9 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -82,6 +82,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys _node(can_driver, system_clock, _pool_allocator), _esc_controller(_node), _hardpoint_controller(_node), + _beep_controller(_node), _time_sync_master(_node), _time_sync_slave(_node), _node_status_monitor(_node), @@ -621,6 +622,12 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) return ret; } + ret = _beep_controller.init(); + + if (ret < 0) { + return ret; + } + // Sensor bridges IUavcanSensorBridge::make_all(_node, _sensor_bridges); diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 64db78a9d38c..13f1b2f11da1 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -52,6 +52,7 @@ #include "actuators/esc.hpp" #include "actuators/hardpoint.hpp" #include "sensors/sensor_bridge.hpp" +#include "sensors/beep.hpp" #include #include @@ -200,6 +201,8 @@ class UavcanNode : public cdev::CDev, public px4::ScheduledWorkItem, public Modu UavcanEscController _esc_controller; UavcanMixingInterface _mixing_interface{_node_mutex, _esc_controller}; UavcanHardpointController _hardpoint_controller; + UavcanUavcanBeep _beep_controller; + uavcan::GlobalTimeSyncMaster _time_sync_master; uavcan::GlobalTimeSyncSlave _time_sync_slave; uavcan::NodeStatusMonitor _node_status_monitor; From 01128b2991ba455603f6f3dea9fb28290c307298 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=B4=A2=E6=9D=B0?= Date: Mon, 20 Apr 2020 17:24:36 +0800 Subject: [PATCH 2/7] Modify notes --- src/drivers/uavcan/sensors/beep.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/uavcan/sensors/beep.hpp b/src/drivers/uavcan/sensors/beep.hpp index 6cdce1829f28..992fcaf22b65 100644 --- a/src/drivers/uavcan/sensors/beep.hpp +++ b/src/drivers/uavcan/sensors/beep.hpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file beep.cpp + * @file beep.hpp * * @author CUAVcaijie */ @@ -48,7 +48,7 @@ #include /** - * @brief The beep class + * @brief The UavcanUavcanBeep class */ class UavcanUavcanBeep From fedfaee9d6c06be631187521e63244faf5640159 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=B4=A2=E6=9D=B0?= Date: Mon, 20 Apr 2020 17:28:37 +0800 Subject: [PATCH 3/7] Modify the required #include --- src/drivers/uavcan/sensors/beep.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/drivers/uavcan/sensors/beep.hpp b/src/drivers/uavcan/sensors/beep.hpp index 992fcaf22b65..d6f6aaf5ddea 100644 --- a/src/drivers/uavcan/sensors/beep.hpp +++ b/src/drivers/uavcan/sensors/beep.hpp @@ -42,8 +42,7 @@ #include #include -#include -#include +#include #include #include From 8982edde8ee399819099dff82e0abc3f56e8b765 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=B4=A2=E6=9D=B0?= Date: Tue, 21 Apr 2020 09:21:14 +0800 Subject: [PATCH 4/7] Alphabetically --- src/drivers/uavcan/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 832c74f9a78b..9e28d0ace667 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -127,10 +127,10 @@ px4_add_module( sensors/differential_pressure.cpp sensors/baro.cpp sensors/battery.cpp + sensors/beep.cpp sensors/flow.cpp sensors/gnss.cpp sensors/mag.cpp - sensors/beep.cpp DEPENDS px4_uavcan_dsdlc From d7a34223824f715044f4cdb28bb8e2845ad6d2ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=B4=A2=E6=9D=B0?= Date: Tue, 21 Apr 2020 09:26:34 +0800 Subject: [PATCH 5/7] Modify notes --- src/drivers/uavcan/sensors/beep.cpp | 2 +- src/drivers/uavcan/sensors/beep.hpp | 20 +++++++++++++------- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/src/drivers/uavcan/sensors/beep.cpp b/src/drivers/uavcan/sensors/beep.cpp index bc7f1e90048e..faed5ffed934 100644 --- a/src/drivers/uavcan/sensors/beep.cpp +++ b/src/drivers/uavcan/sensors/beep.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2020 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 diff --git a/src/drivers/uavcan/sensors/beep.hpp b/src/drivers/uavcan/sensors/beep.hpp index d6f6aaf5ddea..32e570eaed6c 100644 --- a/src/drivers/uavcan/sensors/beep.hpp +++ b/src/drivers/uavcan/sensors/beep.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. +* Copyright (c) 2020 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 @@ -35,6 +35,9 @@ * @file beep.hpp * * @author CUAVcaijie + * + * @brief Control CAN buzzer by subscribing to tune_control + * */ @@ -46,9 +49,6 @@ #include #include -/** - * @brief The UavcanUavcanBeep class - */ class UavcanUavcanBeep { @@ -64,21 +64,27 @@ class UavcanUavcanBeep /* * Max update rate to avoid exessive bus traffic */ - static constexpr unsigned MAX_RATE_HZ = 100; + static constexpr unsigned MAX_RATE_HZ = 100; + /* + * Setup timer and call back function for periodic updates + */ void periodic_update(const uavcan::TimerEvent &); typedef uavcan::MethodBinder TimerCbBinder; + /* + * Subscription tune_control + */ uORB::Subscription _tune_control_sub{ORB_ID(tune_control)}; /* - * libuavcan related things + * Publish CAN Beep */ uavcan::Publisher _beep_pub; - uavcan::TimerEventForwarder _timer; + uavcan::TimerEventForwarder _timer; hrt_abstime interval_timestamp{0}; tune_control_s _tune{}; From e83092384b52b7682095aba2cbf01695196f6a9f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=B4=A2=E6=9D=B0?= Date: Tue, 21 Apr 2020 14:47:51 +0800 Subject: [PATCH 6/7] chang the class's name --- src/drivers/uavcan/sensors/beep.cpp | 8 ++++---- src/drivers/uavcan/sensors/beep.hpp | 6 +++--- src/drivers/uavcan/uavcan_main.hpp | 3 +-- 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/src/drivers/uavcan/sensors/beep.cpp b/src/drivers/uavcan/sensors/beep.cpp index faed5ffed934..40b1cacf4c32 100644 --- a/src/drivers/uavcan/sensors/beep.cpp +++ b/src/drivers/uavcan/sensors/beep.cpp @@ -40,21 +40,21 @@ #include "beep.hpp" -UavcanUavcanBeep::UavcanUavcanBeep(uavcan::INode &node) : +UavcanBeep::UavcanBeep(uavcan::INode &node) : _beep_pub(node), _timer(node) { } int -UavcanUavcanBeep::init() +UavcanBeep::init() { /* * Setup timer and call back function for periodic updates */ if (!_timer.isRunning()) { - _timer.setCallback(TimerCbBinder(this, &UavcanUavcanBeep::periodic_update)); + _timer.setCallback(TimerCbBinder(this, &UavcanBeep::periodic_update)); _timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ)); } @@ -62,7 +62,7 @@ UavcanUavcanBeep::init() } void -UavcanUavcanBeep::periodic_update(const uavcan::TimerEvent &) +UavcanBeep::periodic_update(const uavcan::TimerEvent &) { if (_tune_control_sub.updated()) { _tune_control_sub.copy(&_tune); diff --git a/src/drivers/uavcan/sensors/beep.hpp b/src/drivers/uavcan/sensors/beep.hpp index 32e570eaed6c..5dfa5f523575 100644 --- a/src/drivers/uavcan/sensors/beep.hpp +++ b/src/drivers/uavcan/sensors/beep.hpp @@ -50,10 +50,10 @@ #include -class UavcanUavcanBeep +class UavcanBeep { public: - UavcanUavcanBeep(uavcan::INode &node); + UavcanBeep(uavcan::INode &node); /* * setup periodic updater @@ -71,7 +71,7 @@ class UavcanUavcanBeep */ void periodic_update(const uavcan::TimerEvent &); - typedef uavcan::MethodBinder + typedef uavcan::MethodBinder TimerCbBinder; /* diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 13f1b2f11da1..fba0ba6518e6 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -201,8 +201,7 @@ class UavcanNode : public cdev::CDev, public px4::ScheduledWorkItem, public Modu UavcanEscController _esc_controller; UavcanMixingInterface _mixing_interface{_node_mutex, _esc_controller}; UavcanHardpointController _hardpoint_controller; - UavcanUavcanBeep _beep_controller; - + UavcanBeep _beep_controller; uavcan::GlobalTimeSyncMaster _time_sync_master; uavcan::GlobalTimeSyncSlave _time_sync_slave; uavcan::NodeStatusMonitor _node_status_monitor; From d0148c5da19978f423bdc4a65908335d76fd4ed1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=B4=A2=E6=9D=B0?= Date: Tue, 21 Apr 2020 16:34:55 +0800 Subject: [PATCH 7/7] moved away from the multi-line function header --- src/drivers/uavcan/sensors/beep.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/drivers/uavcan/sensors/beep.cpp b/src/drivers/uavcan/sensors/beep.cpp index 40b1cacf4c32..ad57cc5a82b2 100644 --- a/src/drivers/uavcan/sensors/beep.cpp +++ b/src/drivers/uavcan/sensors/beep.cpp @@ -46,8 +46,7 @@ UavcanBeep::UavcanBeep(uavcan::INode &node) : { } -int -UavcanBeep::init() +int UavcanBeep::init() { /* * Setup timer and call back function for periodic updates @@ -61,8 +60,7 @@ UavcanBeep::init() return 0; } -void -UavcanBeep::periodic_update(const uavcan::TimerEvent &) +void UavcanBeep::periodic_update(const uavcan::TimerEvent &) { if (_tune_control_sub.updated()) { _tune_control_sub.copy(&_tune);