mavlink: REQUEST_DATA_STREAM hadling implemented

1 files changed, 21 insertions, 0 deletions

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 3ec40ee0a..cf8b23bc3 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -152,6 +152,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_manual_control(msg);
 		break;
 
+	case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
+		handle_message_request_data_stream(msg);
+		break;
+
 	default:
 		break;
 	}
@@ -458,6 +462,23 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 }
 
 void
+MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
+{
+	mavlink_request_data_stream_t req;
+	mavlink_msg_request_data_stream_decode(msg, &req);
+
+	if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid) {
+		float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f;
+
+		for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
+			if (req.req_stream_id == 0 || req.req_stream_id == streams_list[i]->get_id()) {
+				_mavlink->configure_stream_threadsafe(streams_list[i]->get_name(), rate);
+			}
+		}
+	}
+}
+
+void
 MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 {
 	mavlink_hil_sensor_t imu;