diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 91f8681a27..4673ed6e72 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -593,9 +593,15 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const } else if (cmd_mavlink.command == MAV_CMD_REQUEST_MESSAGE) { uint16_t message_id = (uint16_t)roundf(vehicle_command.param1); - result = handle_request_message_command(message_id, - vehicle_command.param2, vehicle_command.param3, vehicle_command.param4, - vehicle_command.param5, vehicle_command.param6, vehicle_command.param7); + + if (message_id == MAVLINK_MSG_ID_MESSAGE_INTERVAL) { + get_message_interval((int)(cmd_mavlink.param2 + 0.5f)); + + } else { + result = handle_request_message_command(message_id, + vehicle_command.param2, vehicle_command.param3, vehicle_command.param4, + vehicle_command.param5, vehicle_command.param6, vehicle_command.param7); + } } else if (cmd_mavlink.command == MAV_CMD_INJECT_FAILURE) { if (_mavlink.failure_injection_enabled()) { @@ -2287,7 +2293,7 @@ MavlinkReceiver::set_message_interval(int msgId, float interval, float param3, f void MavlinkReceiver::get_message_interval(int msgId) { - unsigned interval = 0; + int interval = -1; for (const auto &stream : _mavlink.get_streams()) { if (stream->get_id() == msgId) {