From 3c07a22be872557aee1fbe1e053f1728e13a8b2b Mon Sep 17 00:00:00 2001 From: profff Date: Tue, 9 Feb 2021 18:35:02 +0100 Subject: [PATCH] - add video_stream_information mavlink answer (only URI & name yet for RTSP yet) --- src/CameraComponent.cpp | 8 +++++++ src/CameraComponent.h | 3 +++ src/VideoStream.h | 18 +++++++++++++++ src/VideoStreamRtsp.cpp | 38 +++++++++++++++++++++++++++---- src/VideoStreamRtsp.h | 1 + src/VideoStreamUdp.cpp | 24 ++++++++++++++++++++ src/VideoStreamUdp.h | 1 + src/mavlink_server.cpp | 50 +++++++++++++++++++++++++++++++++++++++++ src/mavlink_server.h | 2 ++ 9 files changed, 141 insertions(+), 4 deletions(-) diff --git a/src/CameraComponent.cpp b/src/CameraComponent.cpp index cee6849..0f3e9d6 100644 --- a/src/CameraComponent.cpp +++ b/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 &CameraComponent::getParamList() const { return mCamParam.getParameterList(); diff --git a/src/CameraComponent.h b/src/CameraComponent.h index 445ec66..69f6d22 100644 --- a/src/CameraComponent.h +++ b/src/CameraComponent.h @@ -50,6 +50,8 @@ class CameraComponent { int stop(); const CameraInfo &getCameraInfo() const; const StorageInfo &getStorageInfo() const; + VideoStreamInfo &getVideoStreamInfo(); + const std::map &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 mCamDev; /* Camera Device Object */ std::shared_ptr mImgCap; /* Image Capture Object */ diff --git a/src/VideoStream.h b/src/VideoStream.h index e88e1d3..cf7ad7c 100644 --- a/src/VideoStream.h +++ b/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; diff --git a/src/VideoStreamRtsp.cpp b/src/VideoStreamRtsp.cpp index 05d12f6..9a9bac8 100644 --- a/src/VideoStreamRtsp.cpp +++ b/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 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; } diff --git a/src/VideoStreamRtsp.h b/src/VideoStreamRtsp.h index 129dc6f..12a919d 100644 --- a/src/VideoStreamRtsp.h +++ b/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(); diff --git a/src/VideoStreamUdp.cpp b/src/VideoStreamUdp.cpp index 9f08398..cee8c7b 100644 --- a/src/VideoStreamUdp.cpp +++ b/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__); diff --git a/src/VideoStreamUdp.h b/src/VideoStreamUdp.h index 62359c2..b260f50 100644 --- a/src/VideoStreamUdp.h +++ b/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(); diff --git a/src/mavlink_server.cpp b/src/mavlink_server.cpp index c7bcd94..07a6609 100644 --- a/src/mavlink_server.cpp +++ b/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); diff --git a/src/mavlink_server.h b/src/mavlink_server.h index 628de90..6487411 100644 --- a/src/mavlink_server.h +++ b/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,