mavlink: move tune publication to separate class

This makes it easier to allocate in MavlinkReceiver.

src/modules/mavlink/CMakeLists.txt

@@ -58,6 +58,7 @@ px4_add_module(
 		mavlink_stream.cpp
 		mavlink_ulog.cpp
 		mavlink_timesync.cpp
+		tune_publisher.cpp
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS

src/modules/mavlink/mavlink_receiver.cpp

@@ -82,8 +82,7 @@ using matrix::wrap_2pi;
 
 MavlinkReceiver::~MavlinkReceiver()
 {
-	delete _tunes;
-	delete[] _tune_buffer;
+	delete _tune_publisher;
 	delete _px4_accel;
 	delete _px4_baro;
 	delete _px4_gyro;
@@ -1797,88 +1796,25 @@ MavlinkReceiver::handle_message_play_tune_v2(mavlink_message_t *msg)
 
 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
+	// We only allocate the TunePublisher 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 (_tune_publisher == nullptr) {
+		_tune_publisher = new TunePublisher();
 
-	if (_tunes == nullptr) {
-		_tunes = new Tunes();
-
-		if (_tunes == nullptr) {
-			PX4_ERR("Could not allocate tune");
+		if (_tune_publisher == nullptr) {
+			PX4_ERR("Could not allocate tune publisher");
 			return;
 		}
 	}
 
-	strncpy(_tune_buffer, tune, MAX_TUNE_LEN);
-
-	_tunes->set_string(_tune_buffer, tune_control_s::VOLUME_LEVEL_DEFAULT);
+	const hrt_abstime now = hrt_absolute_time();
 
+	_tune_publisher->set_tune_string(tune, now);
 	// Send first one straightaway.
-	const hrt_abstime now = hrt_absolute_time();
-	_next_tune_time = now;
-	send_next_tune(now);
+	_tune_publisher->publish_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
 MavlinkReceiver::handle_message_obstacle_distance(mavlink_message_t *msg)
 {
@@ -3026,7 +2962,9 @@ MavlinkReceiver::Run()
 			last_send_update = t;
 		}
 
-		send_next_tune(t);
+		if (_tune_publisher != nullptr) {
+			_tune_publisher->publish_next_tune(t);
+		}
 	}
 }
 

src/modules/mavlink/mavlink_receiver.h

@@ -46,12 +46,12 @@
 #include "mavlink_mission.h"
 #include "mavlink_parameters.h"
 #include "mavlink_timesync.h"
+#include "tune_publisher.h"
 
 #include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
 #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>
@@ -210,7 +210,6 @@ private:
 	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.
@@ -272,7 +271,6 @@ private:
 	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)};
@@ -300,12 +298,8 @@ private:
 
 	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};
+	TunePublisher *_tune_publisher{nullptr};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::BAT_CRIT_THR>)     _param_bat_crit_thr,

src/modules/mavlink/tune_publisher.cpp

@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+
+#include "tune_publisher.h"
+#include "string.h"
+#include <px4_platform_common/log.h>
+
+void TunePublisher::set_tune_string(const char *tune, const hrt_abstime &now)
+{
+	// 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;
+	}
+
+	strncpy(_tune_buffer, tune, MAX_TUNE_LEN);
+
+	_tunes.set_string(_tune_buffer, tune_control_s::VOLUME_LEVEL_DEFAULT);
+
+	_next_publish_time = now;
+}
+
+
+void TunePublisher::publish_next_tune(const hrt_abstime now)
+{
+	if (_next_publish_time == 0) {
+		// Nothing to play.
+		return;
+	}
+
+	if (now < _next_publish_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 = now;
+		_tune_control_pub.publish(tune_control);
+
+		_next_publish_time = now + duration + silence;
+
+	} else {
+		// We're done, let's reset.
+		_next_publish_time = 0;
+	}
+}

src/modules/mavlink/tune_publisher.h

@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <uORB/PublicationQueued.hpp>
+#include <drivers/drv_hrt.h>
+#include <lib/tunes/tunes.h>
+
+
+class TunePublisher
+{
+public:
+	void set_tune_string(const char *tune, const hrt_abstime &now);
+	void publish_next_tune(const hrt_abstime now);
+
+private:
+	static constexpr unsigned MAX_TUNE_LEN {248};
+
+	Tunes _tunes {};
+	char _tune_buffer[MAX_TUNE_LEN] {0};
+	hrt_abstime _next_publish_time {0};
+
+	uORB::PublicationQueued<tune_control_s> _tune_control_pub{ORB_ID(tune_control)};
+};