mavlink: add missing uORB publication of tunes

The old tune device interface is not working anymore and we need to publish to uORB tune_control. This solution is not optimal though because blocks the receiving thread.
PX4 · Apr 28, 2020 · 18b3954 · 18b3954
1 parent 48cf38d
commit 18b3954
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 11 deletions.
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
@@ -47,6 +47,7 @@
 #include <drivers/drv_tone_alarm.h>
 #include <ecl/geo/geo.h>
 #include <systemlib/px4_macros.h>
+#include <lib/tunes/tunes.h>
 
 #include <math.h>
 #include <poll.h>
@@ -1758,20 +1759,36 @@ MavlinkReceiver::handle_message_play_tune(mavlink_message_t *msg)
 	mavlink_play_tune_t play_tune;
 	mavlink_msg_play_tune_decode(msg, &play_tune);
 
-	char *tune = play_tune.tune;
+	if ((mavlink_system.sysid == play_tune.target_system || play_tune.target_system == 0) &&
+	    (mavlink_system.compid == play_tune.target_component || play_tune.target_component == 0)) {
 
-	if ((mavlink_system.sysid == play_tune.target_system ||
-	     play_tune.target_system == 0) &&
-	    (mavlink_system.compid == play_tune.target_component ||
-	     play_tune.target_component == 0)) {
+		Tunes tunes;
 
-		if (*tune == 'M') {
-			int fd = px4_open(TONE_ALARM0_DEVICE_PATH, PX4_F_WRONLY);
+		tune_control_s tune_control {};
+		tune_control.tune_id = 0;
+		tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT;
 
-			if (fd >= 0) {
-				px4_write(fd, tune, strlen(tune) + 1);
-				px4_close(fd);
-			}
+		// Let's make sure the input is 0 terminated and we don't ever overrun it.
+		play_tune.tune2[sizeof(play_tune.tune2) - 1] = '\0';
+
+		tunes.set_string(play_tune.tune, tune_control.volume);
+
+		unsigned frequency;
+		unsigned duration;
+		unsigned silence;
+		uint8_t volume;
+
+		while (tunes.get_next_note(frequency, duration, silence, volume) > 0) {
+			tune_control.tune_id = 0;
+			tune_control.frequency = (uint16_t)frequency;
+			tune_control.duration = (uint32_t)duration;
+			tune_control.silence = (uint32_t)silence;
+			tune_control.volume = (uint8_t)volume;
+			tune_control.timestamp = hrt_absolute_time();
+			_tune_control_pub.publish(tune_control);
+
+			// FIXME: this blocks this receiver thread
+			px4_usleep(duration + silence);
 		}
 	}
 }

diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
@@ -85,6 +85,7 @@
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/transponder_report.h>
+#include <uORB/topics/tune_control.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
@@ -266,6 +267,7 @@ class MavlinkReceiver : public ModuleParams
 	uORB::PublicationQueued<transponder_report_s>	_transponder_report_pub{ORB_ID(transponder_report)};
 	uORB::PublicationQueued<vehicle_command_ack_s>	_cmd_ack_pub{ORB_ID(vehicle_command_ack)};
 	uORB::PublicationQueued<vehicle_command_s>	_cmd_pub{ORB_ID(vehicle_command)};
+	uORB::PublicationQueued<tune_control_s>		_tune_control_pub{ORB_ID(tune_control)};
 
 	// ORB subscriptions
 	uORB::Subscription	_actuator_armed_sub{ORB_ID(actuator_armed)};