From f4fc9249f7307314c5ac4bb87807077aa1e8ea47 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Tue, 24 Jul 2018 19:24:53 -0700 Subject: [PATCH] AP_Notify: support extended PLAY_TUNE message --- libraries/AP_Notify/ToneAlarm.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/libraries/AP_Notify/ToneAlarm.cpp b/libraries/AP_Notify/ToneAlarm.cpp index 4474892286..b73d89d669 100644 --- a/libraries/AP_Notify/ToneAlarm.cpp +++ b/libraries/AP_Notify/ToneAlarm.cpp @@ -17,6 +17,7 @@ */ #include +#include #ifdef HAL_PWM_ALARM #include "ToneAlarm.h" @@ -392,6 +393,20 @@ void ToneAlarm::handle_play_tune(mavlink_message_t *msg) mavlink_msg_play_tune_decode(msg, &packet); + if (_sem) { + _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER); + _mml_player.stop(); + + strncpy(_tone_buf, packet.tune, MIN(sizeof(packet.tune), sizeof(_tone_buf)-1)); + _tone_buf[sizeof(_tone_buf)-1] = 0; + uint8_t len = strlen(_tone_buf); + uint8_t len2 = strnlen(packet.tune2, sizeof(packet.tune2)); + len2 = MIN((sizeof(_tone_buf)-1)-len, len2); + strncpy(_tone_buf+len, packet.tune2, len2); + _tone_buf[sizeof(_tone_buf)-1] = 0; + _sem->give(); + } + play_string(packet.tune); }