diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 87df50a4b9..b04f4ad0de 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -60,6 +60,7 @@ #include #include #include +#include #include #include #include @@ -293,6 +294,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_logging_ack(msg); break; + case MAVLINK_MSG_ID_PLAY_TUNE: + handle_message_play_tune(msg); + break; + default: break; } @@ -1437,6 +1442,30 @@ MavlinkReceiver::handle_message_logging_ack(mavlink_message_t *msg) } } +void +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 (*tune == 'M') { + int fd = px4_open(TONEALARM0_DEVICE_PATH, PX4_F_WRONLY); + + if (fd >= 0) { + px4_write(fd, tune, strlen(tune) + 1); + px4_close(fd); + } + } + } +} + switch_pos_t MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index c22c34ee78..5f16e7601c 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -151,6 +151,7 @@ private: void handle_message_battery_status(mavlink_message_t *msg); void handle_message_serial_control(mavlink_message_t *msg); void handle_message_logging_ack(mavlink_message_t *msg); + void handle_message_play_tune(mavlink_message_t *msg); void *receive_thread(void *arg);