From 5f676b6795bd35a582e2c9472ce6ccc53e367c03 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 4 Nov 2019 11:21:38 +0100 Subject: [PATCH] mavlink: move tune publication to a method --- src/modules/mavlink/mavlink_receiver.cpp | 49 +++++++++++++----------- src/modules/mavlink/mavlink_receiver.h | 2 + 2 files changed, 29 insertions(+), 22 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 986232a0e17f..2c0ba892933b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1762,34 +1762,39 @@ MavlinkReceiver::handle_message_play_tune(mavlink_message_t *msg) 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; - - tune_control_s tune_control {}; - tune_control.tune_id = 0; - tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT; - // 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); + publish_tune(play_tune.tune); + } +} + +void MavlinkReceiver::publish_tune(const char *tune) +{ - unsigned frequency; - unsigned duration; - unsigned silence; - uint8_t volume; + tune_control_s tune_control {}; + tune_control.tune_id = 0; + tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT; - while (tunes.get_next_note(frequency, duration, silence, volume) > 0) { - tune_control.tune_id = 0; - tune_control.frequency = static_cast(frequency); - tune_control.duration = static_cast(duration); - tune_control.silence = static_cast(silence); - tune_control.volume = static_cast(volume); - tune_control.timestamp = hrt_absolute_time(); - _tune_control_pub.publish(tune_control); + Tunes tunes; + tunes.set_string(tune, tune_control.volume); - // FIXME: this blocks this receiver thread - px4_usleep(duration + silence); - } + 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 = static_cast(frequency); + tune_control.duration = static_cast(duration); + tune_control.silence = static_cast(silence); + tune_control.volume = static_cast(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 index 72f57e4a9201..5f2e4f3595ed 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -207,6 +207,8 @@ class MavlinkReceiver : public ModuleParams void fill_thrust(float *thrust_body_array, uint8_t vehicle_type, float thrust); + void publish_tune(const char *tune); + /** * @brief Updates the battery, optical flow, and flight ID subscribed parameters. */