mavlink: add missing uORB publication of tunes

src/modules/mavlink/mavlink_receiver.cpp

@@ -82,6 +82,8 @@ using matrix::wrap_2pi;
 
 MavlinkReceiver::~MavlinkReceiver()
 {
+	delete _tunes;
+	delete[] _tune_buffer;
 	delete _px4_accel;
 	delete _px4_baro;
 	delete _px4_gyro;
@@ -237,6 +239,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_play_tune(msg);
 		break;
 
+	case MAVLINK_MSG_ID_PLAY_TUNE_V2:
+		handle_message_play_tune_v2(msg);
+		break;
+
 	case MAVLINK_MSG_ID_OBSTACLE_DISTANCE:
 		handle_message_obstacle_distance(msg);
 		break;
@@ -1758,22 +1764,119 @@ 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)) {
+		// Let's make sure the input is 0 terminated, so we don't ever overrun it.
+		play_tune.tune2[sizeof(play_tune.tune2) - 1] = '\0';
 
-		if (*tune == 'M') {
-			int fd = px4_open(TONE_ALARM0_DEVICE_PATH, PX4_F_WRONLY);
+		schedule_tune(play_tune.tune);
+	}
+}
 
-			if (fd >= 0) {
-				px4_write(fd, tune, strlen(tune) + 1);
-				px4_close(fd);
-			}
+void
+MavlinkReceiver::handle_message_play_tune_v2(mavlink_message_t *msg)
+{
+	mavlink_play_tune_v2_t play_tune_v2;
+	mavlink_msg_play_tune_v2_decode(msg, &play_tune_v2);
+
+	if ((mavlink_system.sysid == play_tune_v2.target_system || play_tune_v2.target_system == 0) &&
+	    (mavlink_system.compid == play_tune_v2.target_component || play_tune_v2.target_component == 0)) {
+
+		if (play_tune_v2.format != TUNE_FORMAT_QBASIC1_1) {
+			PX4_ERR("Tune format %d not supported", play_tune_v2.format);
+			return;
+		}
+
+		// Let's make sure the input is 0 terminated, so we don't ever overrun it.
+		play_tune_v2.tune[sizeof(play_tune_v2.tune) - 1] = '\0';
+
+		schedule_tune(play_tune_v2.tune);
+	}
+}
+
+void MavlinkReceiver::schedule_tune(const char *tune)
+{
+	// The tune string needs to be 0 terminated.
+	const unsigned tune_len = strlen(tune);
+
+	// We don't expect the tune string to be longer than what can come in via MAVLink including 0 termination.
+	if (tune_len >= MAX_TUNE_LEN) {
+		PX4_ERR("Tune string too long.");
+		return;
+	}
+
+	// We only allocate the buffer and the tunes object if we ever use it but we
+	// don't remove it to avoid fragmentation over time.
+	if (_tune_buffer == nullptr) {
+		_tune_buffer = new char[MAX_TUNE_LEN];
+
+		if (_tune_buffer == nullptr) {
+			PX4_ERR("Could not allocate tune buffer");
+			return;
+		}
+	}
+
+	if (_tunes == nullptr) {
+		_tunes = new Tunes();
+
+		if (_tunes == nullptr) {
+			PX4_ERR("Could not allocate tune");
+			return;
 		}
 	}
+
+	strncpy(_tune_buffer, tune, MAX_TUNE_LEN);
+
+	_tunes->set_string(_tune_buffer, tune_control_s::VOLUME_LEVEL_DEFAULT);
+
+	// Send first one straightaway.
+	const hrt_abstime now = hrt_absolute_time();
+	_next_tune_time = now;
+	send_next_tune(now);
+}
+
+
+void MavlinkReceiver::send_next_tune(const hrt_abstime now)
+{
+	if (_tune_buffer == nullptr || _tunes == nullptr) {
+		return;
+	}
+
+	if (_next_tune_time == 0) {
+		// Nothing to play.
+		return;
+	}
+
+	if (now < _next_tune_time) {
+		// Too early, try again later.
+		return;
+	}
+
+	unsigned frequency;
+	unsigned duration;
+	unsigned silence;
+	uint8_t volume;
+
+	if (_tunes->get_next_note(frequency, duration, silence, volume) > 0) {
+		tune_control_s tune_control {};
+		tune_control.tune_id = 0;
+		tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT;
+
+		tune_control.tune_id = 0;
+		tune_control.frequency = static_cast<uint16_t>(frequency);
+		tune_control.duration = static_cast<uint32_t>(duration);
+		tune_control.silence = static_cast<uint32_t>(silence);
+		tune_control.volume = static_cast<uint8_t>(volume);
+		tune_control.timestamp = hrt_absolute_time();
+		_tune_control_pub.publish(tune_control);
+
+		_next_tune_time = now + duration + silence;
+
+	} else {
+		// We're done, let's reset.
+		_next_tune_time = 0;
+	}
 }
 
 void
@@ -2923,6 +3026,7 @@ MavlinkReceiver::Run()
 			last_send_update = t;
 		}
 
+		send_next_tune(t);
 	}
 }
 

src/modules/mavlink/mavlink_receiver.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -51,6 +51,7 @@
 #include <lib/drivers/barometer/PX4Barometer.hpp>
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
+#include <lib/tunes/tunes.h>
 #include <px4_platform_common/module_params.h>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationQueued.hpp>
@@ -85,6 +86,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>
@@ -158,6 +160,7 @@ class MavlinkReceiver : public ModuleParams
 	void handle_message_optical_flow_rad(mavlink_message_t *msg);
 	void handle_message_ping(mavlink_message_t *msg);
 	void handle_message_play_tune(mavlink_message_t *msg);
+	void handle_message_play_tune_v2(mavlink_message_t *msg);
 	void handle_message_radio_status(mavlink_message_t *msg);
 	void handle_message_rc_channels_override(mavlink_message_t *msg);
 	void handle_message_serial_control(mavlink_message_t *msg);
@@ -206,6 +209,9 @@ class MavlinkReceiver : public ModuleParams
 
 	void fill_thrust(float *thrust_body_array, uint8_t vehicle_type, float thrust);
 
+	void schedule_tune(const char *tune);
+	void send_next_tune(hrt_abstime now);
+
 	/**
 	 * @brief Updates the battery, optical flow, and flight ID subscribed parameters.
 	 */
@@ -266,6 +272,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)};
@@ -293,6 +300,13 @@ class MavlinkReceiver : public ModuleParams
 
 	hrt_abstime			_last_utm_global_pos_com{0};
 
+	static constexpr unsigned MAX_TUNE_LEN {248};
+
+	// Allocated if needed.
+	Tunes *_tunes {nullptr};
+	char *_tune_buffer {nullptr};
+	hrt_abstime _next_tune_time {0};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::BAT_CRIT_THR>)     _param_bat_crit_thr,
 		(ParamFloat<px4::params::BAT_EMERGEN_THR>)  _param_bat_emergen_thr,