From eaa2da81cb881a7a9ffd8279ffa2fbabf569f654 Mon Sep 17 00:00:00 2001
From: Martina Rivizzigno <martina@rivizzigno.it>
Date: Mon, 28 Oct 2019 14:24:01 +0100
Subject: [PATCH] only ack vehicle_cmd_start_rx_pair in commander, make led
 blink and send tune directly from SafetyButton driver

---
 src/drivers/safety_button/SafetyButton.cpp | 19 +++++++++++++++++++
 src/drivers/safety_button/SafetyButton.hpp |  5 +++++
 2 files changed, 24 insertions(+)

diff --git a/src/drivers/safety_button/SafetyButton.cpp b/src/drivers/safety_button/SafetyButton.cpp
index fd153f2cf3d1..3e7bd53d85da 100644
--- a/src/drivers/safety_button/SafetyButton.cpp
+++ b/src/drivers/safety_button/SafetyButton.cpp
@@ -31,6 +31,7 @@
  *
  ****************************************************************************/
 
+#include <drivers/drv_tone_alarm.h>
 #include "SafetyButton.hpp"
 
 #ifndef GPIO_BTN_SAFETY
@@ -130,6 +131,8 @@ SafetyButton::CheckPairingRequest(bool button_pressed)
 
 
 	if (_pairing_button_counter == 3) {
+
+
 		vehicle_command_s vcmd{};
 		vcmd.command = vehicle_command_s::VEHICLE_CMD_START_RX_PAIR;
 		vcmd.timestamp = now;
@@ -137,6 +140,22 @@ SafetyButton::CheckPairingRequest(bool button_pressed)
 		_to_command.publish(vcmd);
 		PX4_DEBUG("Sending GCS pairing request");
 
+		led_control_s led_control{};
+		led_control.led_mask = 0xff;
+		led_control.mode = led_control_s::MODE_BLINK_FAST;
+		led_control.color = led_control_s::COLOR_GREEN;
+		led_control.num_blinks = 1;
+		led_control.priority = 0;
+		led_control.timestamp = hrt_absolute_time();
+		_to_led_control.publish(led_control);
+
+		tune_control_s tune_control{};
+		tune_control.tune_id = TONE_NOTIFY_POSITIVE_TUNE;
+		tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT;
+		tune_control.tune_override = 0;
+		tune_control.timestamp = hrt_absolute_time();
+		_to_tune_control.publish(tune_control);
+
 		// reset state
 		_pairing_start = 0;
 		_pairing_button_counter = 0;
diff --git a/src/drivers/safety_button/SafetyButton.hpp b/src/drivers/safety_button/SafetyButton.hpp
index acf4f5b98a78..e44fdcec6643 100644
--- a/src/drivers/safety_button/SafetyButton.hpp
+++ b/src/drivers/safety_button/SafetyButton.hpp
@@ -48,6 +48,8 @@
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/safety.h>
 #include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/led_control.h>
+#include <uORB/topics/tune_control.h>
 
 class SafetyButton : public ModuleBase<SafetyButton>, public px4::ScheduledWorkItem
 {
@@ -81,6 +83,9 @@ class SafetyButton : public ModuleBase<SafetyButton>, public px4::ScheduledWorkI
 	uORB::Subscription		_armed_sub{ORB_ID(actuator_armed)};
 	uORB::Publication<safety_s>	_to_safety{ORB_ID(safety)};
 	uORB::PublicationQueued<vehicle_command_s>	_to_command{ORB_ID(vehicle_command)};
+	uORB::PublicationQueued<led_control_s> _to_led_control{ORB_ID(led_control)};
+	uORB::Publication<tune_control_s> _to_tune_control{ORB_ID(tune_control)};
+
 
 	uint8_t				_button_counter{0};
 	uint8_t				_blink_counter{0};