Implement Mavlink PLAY_TUNE

src/modules/mavlink/mavlink_receiver.cpp

@@ -60,6 +60,7 @@
 #include <drivers/drv_baro.h>
 #include <drivers/drv_range_finder.h>
 #include <drivers/drv_rc_input.h>
+#include <drivers/drv_tone_alarm.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>
@@ -293,6 +294,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_logging_ack(msg);
 		break;
 
+	case MAVLINK_MSG_ID_PLAY_TUNE:
+		handle_message_play_tune(msg);
+		break;
+
 	default:
 		break;
 	}
@@ -1437,6 +1442,30 @@ MavlinkReceiver::handle_message_logging_ack(mavlink_message_t *msg)
 	}
 }
 
+void
+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 (*tune == 'M') {
+			int fd = px4_open(TONEALARM0_DEVICE_PATH, PX4_F_WRONLY);
+
+			if (fd >= 0) {
+				px4_write(fd, tune, strlen(tune) + 1);
+				px4_close(fd);
+			}
+		}
+	}
+}
+
 switch_pos_t
 MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw)
 {

src/modules/mavlink/mavlink_receiver.h

@@ -151,6 +151,7 @@ private:
 	void handle_message_battery_status(mavlink_message_t *msg);
 	void handle_message_serial_control(mavlink_message_t *msg);
 	void handle_message_logging_ack(mavlink_message_t *msg);
+	void handle_message_play_tune(mavlink_message_t *msg);
 
 	void *receive_thread(void *arg);