mavlink: REQUEST_DATA_STREAM hadling implemented

5 files changed, 135 insertions, 3 deletions

diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 5a118a0ad..427b9ad35 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -194,6 +194,8 @@ public:
 
 	mavlink_channel_t get_channel();
 
+	void configure_stream_threadsafe(const char *stream_name, const float rate);
+
 	bool		_task_should_exit;		/**< if true, mavlink task should exit */
 
 protected:
@@ -313,7 +315,6 @@ private:
 	int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
 
 	int configure_stream(const char *stream_name, const float rate);
-	void configure_stream_threadsafe(const char *stream_name, const float rate);
 
 	static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index c89031fcc..5b285dc9b 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -193,6 +193,11 @@ public:
 		return "HEARTBEAT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_HEARTBEAT;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamHeartbeat();
@@ -244,6 +249,11 @@ public:
 		return "SYS_STATUS";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_SYS_STATUS;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamSysStatus();
@@ -293,6 +303,11 @@ public:
 		return "HIGHRES_IMU";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_HIGHRES_IMU;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamHighresIMU();
@@ -364,6 +379,11 @@ public:
 		return "ATTITUDE";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ATTITUDE;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamAttitude();
@@ -400,6 +420,11 @@ public:
 		return "ATTITUDE_QUATERNION";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamAttitudeQuaternion();
@@ -441,6 +466,11 @@ public:
 		return "VFR_HUD";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_VFR_HUD;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamVFRHUD();
@@ -514,6 +544,11 @@ public:
 		return "GPS_RAW_INT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GPS_RAW_INT;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGPSRawInt();
@@ -557,6 +592,11 @@ public:
 		return "GLOBAL_POSITION_INT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGlobalPositionInt();
@@ -608,6 +648,11 @@ public:
 		return "LOCAL_POSITION_NED";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_LOCAL_POSITION_NED;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamLocalPositionNED();
@@ -648,6 +693,11 @@ public:
 		return "GPS_GLOBAL_ORIGIN";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGPSGlobalOrigin();
@@ -689,6 +739,11 @@ public:
 		sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n);
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
+	}
+
 	const char *get_name()
 	{
 		return _name;
@@ -747,6 +802,11 @@ public:
 		return "HIL_CONTROLS";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_HIL_CONTROLS;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamHILControls();
@@ -874,6 +934,11 @@ public:
 		return "GLOBAL_POSITION_SETPOINT_INT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamGlobalPositionSetpointInt();
@@ -912,6 +977,11 @@ public:
 		return "LOCAL_POSITION_SETPOINT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamLocalPositionSetpoint();
@@ -950,6 +1020,11 @@ public:
 		return "ROLL_PITCH_YAW_THRUST_SETPOINT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamRollPitchYawThrustSetpoint();
@@ -988,6 +1063,11 @@ public:
 		return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamRollPitchYawRatesThrustSetpoint();
@@ -1026,6 +1106,11 @@ public:
 		return "RC_CHANNELS_RAW";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_RC_CHANNELS_RAW;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamRCChannelsRaw();
@@ -1075,6 +1160,11 @@ public:
 		return "MANUAL_CONTROL";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_MANUAL_CONTROL;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamManualControl();
@@ -1114,6 +1204,11 @@ public:
 		return "OPTICAL_FLOW";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_OPTICAL_FLOW;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamOpticalFlow();
@@ -1152,6 +1247,11 @@ public:
 		return "ATTITUDE_CONTROLS";
 	}
 
+	uint8_t get_id()
+	{
+		return 0;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamAttitudeControls();
@@ -1200,6 +1300,11 @@ public:
 		return "NAMED_VALUE_FLOAT";
 	}
 
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamNamedValueFloat();
@@ -1238,6 +1343,11 @@ public:
 		return "CAMERA_CAPTURE";
 	}
 
+	uint8_t get_id()
+	{
+		return 0;
+	}
+
 	MavlinkStream *new_instance()
 	{
 		return new MavlinkStreamCameraCapture();
@@ -1252,8 +1362,6 @@ protected:
 	{
 		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
 		status = (struct vehicle_status_s *)status_sub->get_data();
-
-
 	}
 
 	void send(const hrt_abstime t)
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;
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 8ccb2a035..eab8f071d 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -112,6 +112,7 @@ private:
 	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
 	void handle_message_radio_status(mavlink_message_t *msg);
 	void handle_message_manual_control(mavlink_message_t *msg);
+	void handle_message_request_data_stream(mavlink_message_t *msg);
 	void handle_message_hil_sensor(mavlink_message_t *msg);
 	void handle_message_hil_gps(mavlink_message_t *msg);
 	void handle_message_hil_state_quaternion(mavlink_message_t *msg);
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index 135e1bce0..aa8ca450b 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -70,6 +70,7 @@ public:
 	virtual MavlinkStream *new_instance() = 0;
 	virtual void subscribe(Mavlink *mavlink) = 0;
 	virtual const char *get_name() = 0;
+	virtual uint8_t get_id() = 0;
 };