- add video_stream_information mavlink answer

src/CameraComponent.cpp

@@ -126,6 +126,14 @@ const StorageInfo &CameraComponent::getStorageInfo() const
     return mStoreInfo;
 }
 
+VideoStreamInfo &CameraComponent::getVideoStreamInfo()
+{
+   log_info("[%s::%s]", typeid(this).name(), __func__);
+    if(mVidStream) 
+        mVidStream->getInfo(mVidStreamInfo);
+    return mVidStreamInfo;
+}
+
 const std::map<std::string, std::string> &CameraComponent::getParamList() const
 {
     return mCamParam.getParameterList();

src/CameraComponent.h

@@ -50,6 +50,8 @@ class CameraComponent {
     int stop();
     const CameraInfo &getCameraInfo() const;
     const StorageInfo &getStorageInfo() const;
+    VideoStreamInfo &getVideoStreamInfo();
+
     const std::map<std::string, std::string> &getParamList() const;
     int getParamType(const char *param_id, size_t id_size);
     virtual int getParam(const char *param_id, size_t id_size, char *param_value,
@@ -79,6 +81,7 @@ class CameraComponent {
     std::string mCamDevName;               /* Camera device name */
     CameraInfo mCamInfo;                   /* Camera Information Structure */
     StorageInfo mStoreInfo;                /* Storage Information Structure */
+    VideoStreamInfo mVidStreamInfo;	   /* Videos stream info Structure */ 
     CameraParameters mCamParam;            /* Camera Parameters Object */
     std::shared_ptr<CameraDevice> mCamDev; /* Camera Device Object */
     std::shared_ptr<ImageCapture> mImgCap; /* Image Capture Object */

src/VideoStream.h

@@ -16,6 +16,23 @@
  * limitations under the License.
  */
 #pragma once
+/**
+ *  The VideoStreamInfo structure is used to hold the Streaming information.
+ */
+struct VideoStreamInfo{
+  uint8_t stream_id;		/**Video Stream ID (1 for first, 2 for second, etc.)*/
+  uint8_t count	;		/**Number of streams available.*/
+  uint8_t type	;		/**VIDEO_STREAM_TYPE	Type of stream.*/
+  uint16_t flags;		/**VIDEO_STREAM_STATUS_FLAGS	Bitmap of stream status flags.*/
+  float framerate;		/**Hz		Frame rate.*/
+  uint16_t resolution_h;	/**pix		Horizontal resolution.*/
+  uint16_t resolution_v	;	/**pix		Vertical resolution.*/
+  uint32_t bitrate	;	/**bits/s		Bit rate.*/
+  uint16_t rotation	;	/**deg		Video image rotation clockwise.*/
+  uint16_t hfov	;		/**deg		Horizontal Field of view.*/
+  char name [32]	;	/**Stream name.*/
+  char uri [160]	;	/**Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to).*/
+};
 
 class VideoStream {
 public:
@@ -26,6 +43,7 @@ class VideoStream {
 
     virtual int init() = 0;
     virtual int uninit() = 0;
+    virtual int getInfo(VideoStreamInfo &vidStreamInfo) = 0;
     virtual int start() = 0;
     virtual int stop() = 0;
     virtual int getState() = 0;

src/VideoStreamRtsp.cpp

@@ -72,10 +72,12 @@ static std::string getGstVideoEncoder(CameraParameters::VIDEO_CODING_FORMAT encF
 
     switch (encFormat) {
     case CameraParameters::VIDEO_CODING_AVC:
-        enc = std::string("vaapih264enc");
+      //        enc = std::string("vaapih264enc");
+        enc = std::string("x264enc speed-preset=ultrafast tune=zerolatency");
         break;
     default:
-        enc = std::string("vaapih264enc");
+      //        enc = std::string("vaapih264enc");
+        enc = std::string("x264enc speed-preset=ultrafast tune=zerolatency");
         break;
     }
 
@@ -208,6 +210,28 @@ int VideoStreamRtsp::uninit()
     return 0;
 }
 
+int VideoStreamRtsp::getInfo(VideoStreamInfo &vidStreamInfo)
+{
+  log_info("[%s::%s]", typeid(this).name(), __func__);
+
+     vidStreamInfo.stream_id=1;
+     vidStreamInfo.count=1;
+     vidStreamInfo.type=0;
+     vidStreamInfo.flags=0;
+     vidStreamInfo.framerate=0;
+     vidStreamInfo.resolution_h=mWidth;
+     vidStreamInfo.resolution_v=mHeight;
+     vidStreamInfo.bitrate=0;
+     vidStreamInfo.rotation=0;
+     vidStreamInfo.hfov=0;
+     strncpy((char *)vidStreamInfo.name, mPath.c_str(), sizeof(vidStreamInfo.name));
+     vidStreamInfo.name[sizeof(vidStreamInfo.name) - 1] = 0;log_warning("name [32]:	%s",vidStreamInfo.name);
+     sprintf((char *)vidStreamInfo.uri,"rtsp://%s:%d%s",mHost.c_str(),mPort,mPath.c_str());
+     vidStreamInfo.uri[sizeof(vidStreamInfo.uri) - 1] = 0;
+     return this->getState();
+}
+
+
 int VideoStreamRtsp::start()
 {
     log_debug("%s::%s", typeid(this).name(), __func__);
@@ -357,17 +381,23 @@ std::string VideoStreamRtsp::getGstPipeline(std::map<std::string, std::string> &
 {
     std::string name;
     std::string source;
+    log_warning("%s:%s", __func__, name.c_str());
+
     if (mCamDev->isGstV4l2Src()) {
         source = "v4l2src device=/dev/" + mCamDev->getDeviceId();
     } else {
         source = "appsrc name=mysrc";
     }
-
+    log_warning("source : %s", source.c_str());
+    log_warning("convertor : %s", getGstVideoConvertor().c_str());
+    log_warning("cap : %s",  getGstVideoConvertorCaps(params, mWidth, mHeight).c_str());
+    log_warning("encoder : %s",  getGstVideoEncoder(mEncFormat).c_str());
+    log_warning("sink : %s",  getGstRtspVideoSink().c_str());
+
     name = source + " ! " + getGstVideoConvertor() + " ! "
         + getGstVideoConvertorCaps(params, mWidth, mHeight) + " ! " + getGstVideoEncoder(mEncFormat)
         + " ! " + getGstRtspVideoSink();
 
-    log_debug("%s:%s", __func__, name.c_str());
     return name;
 }
 

src/VideoStreamRtsp.h

@@ -35,6 +35,7 @@ class VideoStreamRtsp final : public VideoStream {
 
     int init();
     int uninit();
+    int getInfo(VideoStreamInfo &vidStreamInfo);
     int start();
     int stop();
     int getState();

src/VideoStreamUdp.cpp

@@ -58,6 +58,30 @@ int VideoStreamUdp::uninit()
     return 0;
 }
 
+int VideoStreamUdp::getInfo(VideoStreamInfo &vidStreamInfo)
+{
+     log_debug("%s::%s", typeid(this).name(), __func__);
+     vidStreamInfo.stream_id=1;
+     vidStreamInfo.count=1;
+     vidStreamInfo.type=0;
+     vidStreamInfo.flags=0;
+     vidStreamInfo.framerate=0;
+     vidStreamInfo.resolution_h=0;
+     vidStreamInfo.resolution_v=0;
+     vidStreamInfo.bitrate=0;
+     vidStreamInfo.rotation=0;
+     vidStreamInfo.hfov=0;
+     strncpy((char *)vidStreamInfo.name, "dummy", sizeof(vidStreamInfo.name));
+     vidStreamInfo.name[sizeof(vidStreamInfo.name) - 1] = 0;
+     strncpy((char *)vidStreamInfo.uri, "none", sizeof(vidStreamInfo.uri));
+     vidStreamInfo.uri[sizeof(vidStreamInfo.uri) - 1] = 0;
+    // vidStreamInfo.name="";
+     // vidStreamInfo.uri="";
+     return this->getState();
+}
+
+
+
 int VideoStreamUdp::start()
 {
     log_info("%s::%s", typeid(this).name(), __func__);

src/VideoStreamUdp.h

@@ -31,6 +31,7 @@ class VideoStreamUdp final : public VideoStream {
 
     int init();
     int uninit();
+    int getInfo(VideoStreamInfo &vidStreamInfo);
     int start();
     int stop();
     int getState();

src/mavlink_server.cpp

@@ -143,6 +143,55 @@ void MavlinkServer::_handle_request_camera_information(const struct sockaddr_in
     _send_ack(addr, cmd.command, cmd.target_component, success);
 }
 
+void MavlinkServer::_handle_request_video_stream_information(const struct sockaddr_in &addr,
+                                                       mavlink_command_long_t &cmd)
+{
+    log_debug("%s", __func__);
+
+    mavlink_message_t msg;
+    bool success = false;
+
+    CameraComponent *tgtComp = getCameraComponent(cmd.target_component);
+    if (tgtComp) {
+      const CameraInfo &camInfo = tgtComp->getCameraInfo();
+      const VideoStreamInfo &vidInfo = tgtComp->getVideoStreamInfo();
+      log_warning("stream_id:	%d",vidInfo.stream_id);/**Video Stream ID (1 for first, 2 for second, etc.)*/
+      log_warning("count:	%d",vidInfo.count);/**Number of streams available.*/
+      log_warning("type:	%d",vidInfo.type);/**VIDEO_STREAM_TYPE type of stream.*/
+      log_warning("flags:	%d",vidInfo.flags);/**VIDEO_STREAM_STATUS_FLAGS Bitmap of stream status flags.*/
+      log_warning("framerate:	%d",vidInfo.framerate);/**Hz		Frame rate.*/
+      log_warning("resolution_h:%d",vidInfo.resolution_h);/**pix		Horizontal resolution.*/
+      log_warning("resolution_v:%d",vidInfo.resolution_v);/**pix		Vertical resolution.*/
+      log_warning("bitrate:	%d",vidInfo.bitrate);/**bits/s		Bit rate.*/
+      log_warning("rotation:	%d",vidInfo.rotation);/**deg		Video image rotation clockwise.*/
+      log_warning("hfov:	%d",vidInfo.hfov);/**deg		Horizontal Field of view.*/
+      log_warning("name [32]:	%s",vidInfo.name);/**Stream name.*/
+      log_warning("uri [160]:	%s",vidInfo.uri);/**Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to).*/
+      mavlink_msg_video_stream_information_pack(_system_id, cmd.target_component, &msg,
+						(const uint8_t )vidInfo.stream_id,
+						(const uint8_t )vidInfo.count,
+						(const uint8_t )vidInfo.type,
+						(const uint16_t )vidInfo.flags,
+						(const float)vidInfo.framerate,
+						(const uint16_t )vidInfo.resolution_h,
+						(const uint16_t )vidInfo.resolution_v,
+						(const uint32_t )vidInfo.bitrate,
+						(const uint16_t )vidInfo.rotation,
+						(const uint16_t )vidInfo.hfov,
+						(const char *)vidInfo.name,
+						(const char *)vidInfo.uri);
+
+         if (!_send_mavlink_message(&addr, msg)) {
+             log_error("Sending video stream information failed for camera %d.", cmd.target_component);
+             return;
+         }
+
+         success = true;
+    }
+
+    _send_ack(addr, cmd.command, cmd.target_component, success);
+}
+
 void MavlinkServer::_handle_request_camera_settings(const struct sockaddr_in &addr,
                                                     mavlink_command_long_t &cmd)
 {
@@ -519,6 +568,7 @@ void MavlinkServer::_handle_mavlink_message(const struct sockaddr_in &addr, mavl
             this->_handle_request_camera_information(addr, cmd);
             break;
         case MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION:
+	    this->_handle_request_video_stream_information(addr, cmd);
             break;
         case MAV_CMD_REQUEST_CAMERA_SETTINGS:
             this->_handle_request_camera_settings(addr, cmd);

src/mavlink_server.h

@@ -55,6 +55,8 @@ class MavlinkServer {
     void _handle_mavlink_message(const struct sockaddr_in &addr, mavlink_message_t *msg);
     void _handle_request_camera_information(const struct sockaddr_in &addr,
                                             mavlink_command_long_t &cmd);
+    void _handle_request_video_stream_information(const struct sockaddr_in &addr,
+                                            mavlink_command_long_t &cmd);
     void _handle_request_camera_settings(const struct sockaddr_in &addr,
                                          mavlink_command_long_t &cmd);
     void _handle_request_storage_information(const struct sockaddr_in &addr,