From 4e48923c64aeb92096e890b22985c405a94007d7 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Wed, 25 Jul 2018 09:50:02 -0700
Subject: [PATCH] AP_HAL_PX4: implement new tonealarm interface

---
 libraries/AP_HAL_PX4/Util.cpp | 24 ++++++++++++++++++++++++
 libraries/AP_HAL_PX4/Util.h   |  5 +++++
 2 files changed, 29 insertions(+)

diff --git a/libraries/AP_HAL_PX4/Util.cpp b/libraries/AP_HAL_PX4/Util.cpp
index b07ecea056..0958992946 100644
--- a/libraries/AP_HAL_PX4/Util.cpp
+++ b/libraries/AP_HAL_PX4/Util.cpp
@@ -15,6 +15,7 @@
 #include <drivers/drv_gpio.h>
 #include <AP_Math/AP_Math.h>
 #include <AP_BoardConfig/AP_BoardConfig.h>
+#include <drivers/drv_tone_alarm.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -278,5 +279,28 @@ bool PX4Util::flash_bootloader()
     return false;
 }
 
+bool PX4Util::toneAlarm_init() {
+    _tonealarm_fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY);
+    if (_tonealarm_fd == -1) {
+        hal.console->printf("ToneAlarm_PX4: Unable to open " TONEALARM0_DEVICE_PATH);
+        return false;
+    }
+
+
+    return true;
+}
+
+void PX4Util::toneAlarm_set_buzzer_tone(float frequency, float volume) {
+    if (is_zero(frequency) || is_zero(volume)) {
+        write(_tonealarm_fd, "MFP", 4);
+    } else {
+        unsigned note = constrain_float(roundf(17.3123404906676f*logf(frequency) - 71.3763165622959f), 4, 87); // constraint based on comment in PX4 tonealarm driver - "note value in the range C1 to B7"
+        char tune[20];
+        snprintf(tune, sizeof(tune), "MFMLT32L1N%u", note);
+        tune[sizeof(tune)-1] = '\0';
+        write(_tonealarm_fd, tune, strlen(tune)+1);
+    }
+}
+
 
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4
diff --git a/libraries/AP_HAL_PX4/Util.h b/libraries/AP_HAL_PX4/Util.h
index aa3845e0c0..eaf2308296 100644
--- a/libraries/AP_HAL_PX4/Util.h
+++ b/libraries/AP_HAL_PX4/Util.h
@@ -61,6 +61,9 @@ public:
     void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) override;
 
     bool flash_bootloader() override;
+
+    bool toneAlarm_init() override;
+    void toneAlarm_set_buzzer_tone(float frequency, float volume) override;
     
 private:
     int _safety_handle;
@@ -74,4 +77,6 @@ private:
         uint32_t last_update_ms;
         int fd = -1;
     } _heater;
+
+    int _tonealarm_fd;
 };