only ack vehicle_cmd_start_rx_pair in commander, make led

blink and send tune directly from SafetyButton driver
PX4 · Oct 29, 2019 · eaa2da8 · eaa2da8
1 parent 7a95ab7
commit eaa2da8
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 0 deletions.
diff --git 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,13 +131,31 @@ 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;
 		vcmd.param1 = 10.f; // GCS pairing request handled by a companion. TODO: requires mavlink spec
 		_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
@@ -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};