mavlink: copy tune to buffer to play later

Instead of blocking the receiver thread while playing a tune we now copy
the tune to a buffer and check if we can play the next note on each
iteration of the receiver thread.

The buffer and tune object is only created on the heap if we receive a
tune to play once and doesn't use resources otherwise.

src/modules/mavlink/mavlink_receiver.cpp

@@ -47,7 +47,6 @@
 #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>
@@ -83,6 +82,8 @@ using matrix::wrap_2pi;
 
 MavlinkReceiver::~MavlinkReceiver()
 {
+	delete _tunes;
+	delete[] _tune_buffer;
 	delete _px4_accel;
 	delete _px4_baro;
 	delete _px4_gyro;
@@ -1769,7 +1770,7 @@ MavlinkReceiver::handle_message_play_tune(mavlink_message_t *msg)
 		// 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';
 
-		publish_tune(play_tune.tune);
+		schedule_tune(play_tune.tune);
 	}
 }
 
@@ -1790,26 +1791,78 @@ MavlinkReceiver::handle_message_play_tune_v2(mavlink_message_t *msg)
 		// 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';
 
-		publish_tune(play_tune_v2.tune);
+		schedule_tune(play_tune_v2.tune);
 	}
 }
 
-void MavlinkReceiver::publish_tune(const char *tune)
+void MavlinkReceiver::schedule_tune(const char *tune)
 {
+	// The tune string needs to be 0 terminated.
+	const unsigned tune_len = strlen(tune);
 
-	tune_control_s tune_control {};
-	tune_control.tune_id = 0;
-	tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT;
+	// 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;
+	}
 
-	Tunes tunes;
-	tunes.set_string(tune, tune_control.volume);
+	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;
 
-	while (tunes.get_next_note(frequency, duration, silence, volume) > 0) {
+	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);
@@ -1818,8 +1871,11 @@ void MavlinkReceiver::publish_tune(const char *tune)
 		tune_control.timestamp = hrt_absolute_time();
 		_tune_control_pub.publish(tune_control);
 
-		// FIXME: this blocks this receiver thread
-		px4_usleep(duration + silence);
+		_next_tune_time = now + duration + silence;
+
+	} else {
+		// We're done, let's reset.
+		_next_tune_time = 0;
 	}
 }
 
@@ -2970,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>
@@ -208,7 +209,8 @@ class MavlinkReceiver : public ModuleParams
 
 	void fill_thrust(float *thrust_body_array, uint8_t vehicle_type, float thrust);
 
-	void publish_tune(const char *tune);
+	void schedule_tune(const char *tune);
+	void send_next_tune(hrt_abstime now);
 
 	/**
 	 * @brief Updates the battery, optical flow, and flight ID subscribed parameters.
@@ -298,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,