From d55f76c88e2458f5cb712d07ae27cac653f292df Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 31 Aug 2019 12:45:02 +1000
Subject: [PATCH] AP_UAVCAN: added buzzer support

---
 libraries/AP_UAVCAN/AP_UAVCAN.cpp | 31 +++++++++++++++++++++++++++++++
 libraries/AP_UAVCAN/AP_UAVCAN.h   | 13 +++++++++++++
 2 files changed, 44 insertions(+)

diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp
index 4a872dfadb..0f3815cde5 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp
+++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp
@@ -34,6 +34,7 @@
 #include <uavcan/equipment/esc/RawCommand.hpp>
 #include <uavcan/equipment/indication/LightsCommand.hpp>
 #include <uavcan/equipment/indication/SingleLightCommand.hpp>
+#include <uavcan/equipment/indication/BeepCommand.hpp>
 #include <uavcan/equipment/indication/RGB565.hpp>
 
 #include <AP_Baro/AP_Baro_UAVCAN.h>
@@ -98,6 +99,7 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = {
 static uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand>* act_out_array[MAX_NUMBER_OF_CAN_DRIVERS];
 static uavcan::Publisher<uavcan::equipment::esc::RawCommand>* esc_raw[MAX_NUMBER_OF_CAN_DRIVERS];
 static uavcan::Publisher<uavcan::equipment::indication::LightsCommand>* rgb_led[MAX_NUMBER_OF_CAN_DRIVERS];
+static uavcan::Publisher<uavcan::equipment::indication::BeepCommand>* buzzer[MAX_NUMBER_OF_CAN_DRIVERS];
 
 AP_UAVCAN::AP_UAVCAN() :
     _node_allocator()
@@ -222,6 +224,10 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
     rgb_led[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
     rgb_led[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
 
+    buzzer[driver_index] = new uavcan::Publisher<uavcan::equipment::indication::BeepCommand>(*_node);
+    buzzer[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
+    buzzer[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
+    
     _led_conf.devices_count = 0;
     if (enable_filters) {
         configureCanAcceptanceFilters(*_node);
@@ -291,6 +297,7 @@ void AP_UAVCAN::loop(void)
         }
 
         led_out_send();
+        buzzer_send();
     }
 }
 
@@ -474,4 +481,28 @@ bool AP_UAVCAN::led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t
     return true;
 }
 
+// buzzer send
+void AP_UAVCAN::buzzer_send()
+{
+    uavcan::equipment::indication::BeepCommand msg;
+    WITH_SEMAPHORE(_buzzer.sem);
+    uint8_t mask = (1U << _driver_index);
+    if ((_buzzer.pending_mask & mask) == 0) {
+        return;
+    }
+    _buzzer.pending_mask &= ~mask;
+    msg.frequency = _buzzer.frequency;
+    msg.duration = _buzzer.duration;
+    buzzer[_driver_index]->broadcast(msg);
+}
+
+// buzzer support
+void AP_UAVCAN::set_buzzer_tone(float frequency, float duration_s)
+{
+    WITH_SEMAPHORE(_buzzer.sem);
+    _buzzer.frequency = frequency;
+    _buzzer.duration = duration_s;
+    _buzzer.pending_mask = 0xFF;
+}
+
 #endif // HAL_WITH_UAVCAN
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h
index b63455916e..ce423bdbea 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN.h
+++ b/libraries/AP_UAVCAN/AP_UAVCAN.h
@@ -81,6 +81,8 @@ public:
     ///// LED /////
     bool led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue);
 
+    // buzzer
+    void set_buzzer_tone(float frequency, float duration_s);
 
     template <typename DataType_>
     class RegistryBinder {
@@ -147,6 +149,9 @@ private:
     ///// LED /////
     void led_out_send();
 
+    // buzzer
+    void buzzer_send();
+    
     uavcan::PoolAllocator<UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_BLOCK_SIZE, AP_UAVCAN::RaiiSynchronizer> _node_allocator;
 
     // UAVCAN parameters
@@ -190,6 +195,14 @@ private:
     } _led_conf;
 
     HAL_Semaphore _led_out_sem;
+
+    // buzzer
+    struct {
+        HAL_Semaphore sem;
+        float frequency;
+        float duration;
+        uint8_t pending_mask; // mask of interfaces to send to
+    } _buzzer;
 };
 
 #endif /* AP_UAVCAN_H_ */