diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 423a246866..7fe1cb4705 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #include #include @@ -1758,20 +1759,36 @@ 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)) { + Tunes tunes; - if (*tune == 'M') { - int fd = px4_open(TONE_ALARM0_DEVICE_PATH, PX4_F_WRONLY); + tune_control_s tune_control {}; + tune_control.tune_id = 0; + tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT; - if (fd >= 0) { - px4_write(fd, tune, strlen(tune) + 1); - px4_close(fd); - } + // 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); + + 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 = (uint16_t)frequency; + tune_control.duration = (uint32_t)duration; + tune_control.silence = (uint32_t)silence; + tune_control.volume = (uint8_t)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 7c55679e9c..72f57e4a92 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -85,6 +85,7 @@ #include #include #include +#include #include #include #include @@ -266,6 +267,7 @@ private: uORB::PublicationQueued _transponder_report_pub{ORB_ID(transponder_report)}; uORB::PublicationQueued _cmd_ack_pub{ORB_ID(vehicle_command_ack)}; uORB::PublicationQueued _cmd_pub{ORB_ID(vehicle_command)}; + uORB::PublicationQueued _tune_control_pub{ORB_ID(tune_control)}; // ORB subscriptions uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};