From bdca329012b2f3507f59a406f683b4b18b2d9405 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 30 Jul 2018 13:12:37 +1000
Subject: [PATCH] HAL_PX4: fixes for new tone_alarm system

we need to start the driver
---
 libraries/AP_HAL_PX4/GPIO.cpp | 14 --------------
 libraries/AP_HAL_PX4/GPIO.h   |  1 -
 libraries/AP_HAL_PX4/Util.cpp | 16 ++++++++++++----
 libraries/AP_HAL_PX4/Util.h   |  2 +-
 4 files changed, 13 insertions(+), 20 deletions(-)

diff --git a/libraries/AP_HAL_PX4/GPIO.cpp b/libraries/AP_HAL_PX4/GPIO.cpp
index 56756505ca..ccfd9cb6fa 100644
--- a/libraries/AP_HAL_PX4/GPIO.cpp
+++ b/libraries/AP_HAL_PX4/GPIO.cpp
@@ -47,11 +47,6 @@ void PX4GPIO::init()
 #endif
 #endif
 #if !defined(CONFIG_ARCH_BOARD_AEROFC_V1)
-    _tone_alarm_fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY);
-    if (_tone_alarm_fd == -1) {
-        AP_HAL::panic("Unable to open " TONEALARM0_DEVICE_PATH);
-    }
-
     _gpio_fmu_fd = open(PX4FMU_DEVICE_PATH, 0);
     if (_gpio_fmu_fd == -1) {
         AP_HAL::panic("Unable to open GPIO");
@@ -193,15 +188,6 @@ void PX4GPIO::write(uint8_t pin, uint8_t value)
             break;
 #endif
 
-        case PX4_GPIO_PIEZO_PIN:    // Piezo beeper 
-            if (value == LOW) { // this is inverted 
-                ioctl(_tone_alarm_fd, TONE_SET_ALARM, 3);    // Alarm on !! 
-                //::write(_tone_alarm_fd, &user_tune, sizeof(user_tune));
-            } else { 
-                ioctl(_tone_alarm_fd, TONE_SET_ALARM, 0);    // Alarm off !! 
-            }
-            break;
-
 #ifdef GPIO_EXT_1
         case PX4_GPIO_EXT_FMU_RELAY1_PIN:
             ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, GPIO_EXT_1);
diff --git a/libraries/AP_HAL_PX4/GPIO.h b/libraries/AP_HAL_PX4/GPIO.h
index 9a752b2edb..9f54da3835 100644
--- a/libraries/AP_HAL_PX4/GPIO.h
+++ b/libraries/AP_HAL_PX4/GPIO.h
@@ -55,7 +55,6 @@ public:
 
 private:
     int _led_fd = -1;
-    int _tone_alarm_fd = -1;
     int _gpio_fmu_fd = -1;
     int _gpio_io_fd = -1;
     bool _usb_connected = false;
diff --git a/libraries/AP_HAL_PX4/Util.cpp b/libraries/AP_HAL_PX4/Util.cpp
index 46b999e09a..580cf61afe 100644
--- a/libraries/AP_HAL_PX4/Util.cpp
+++ b/libraries/AP_HAL_PX4/Util.cpp
@@ -266,6 +266,7 @@ void PX4Util::free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_ty
 
 extern "C" {
     int bl_update_main(int argc, char *argv[]);
+    int tone_alarm_main(int argc, char *argv[]);
 };
 
 bool PX4Util::flash_bootloader()
@@ -279,18 +280,25 @@ bool PX4Util::flash_bootloader()
     return false;
 }
 
-bool PX4Util::toneAlarm_init() {
+bool PX4Util::toneAlarm_init()
+{
+    if (!AP_BoardConfig::px4_start_driver(tone_alarm_main, "tone_alarm", "start")) {
+        hal.console->printf("Failed to start tone_alarm\n");
+        return false;
+    }
     _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, uint32_t duration_ms) {
+void PX4Util::toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms)
+{
+    if (_tonealarm_fd == -1) {
+        return;
+    }
     if (is_zero(frequency) || is_zero(volume)) {
         write(_tonealarm_fd, "MFP", 4);
     } else {
diff --git a/libraries/AP_HAL_PX4/Util.h b/libraries/AP_HAL_PX4/Util.h
index 9ec0ed782c..0425419276 100644
--- a/libraries/AP_HAL_PX4/Util.h
+++ b/libraries/AP_HAL_PX4/Util.h
@@ -78,5 +78,5 @@ private:
         int fd = -1;
     } _heater;
 
-    int _tonealarm_fd;
+    int _tonealarm_fd = -1;
 };