diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 185d8f2686273b581476a934ec50a72ea2c71745..b67318b0f4f977925cef2a7b18bc783ce3593c29 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -29,6 +29,8 @@ #include "QGCMAVLink.h" #include "QGCApplication.h" +#include + QGC_LOGGING_CATEGORY(APMFirmwarePluginLog, "APMFirmwarePluginLog") static const QRegExp APM_COPTER_REXP("^(ArduCopter|APM:Copter)"); @@ -49,6 +51,8 @@ static const QString MIN_COPTER_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Copter V static const QString MIN_PLANE_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Plane V3.4.0"); static const QString MIN_ROVER_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Rover V2.6.0"); +const char* APMFirmwarePlugin::_artooIP = "10.1.1.1"; ///< IP address of ARTOO controller +const int APMFirmwarePlugin::_artooVideoHandshakePort = 5502; ///< Port for video handshake on ARTOO controller /* * @brief APMFirmwareVersion is a small class to represent the firmware version @@ -372,11 +376,21 @@ bool APMFirmwarePlugin::_handleStatusText(Vehicle* vehicle, mavlink_message_t* m // The following messages are incorrectly labeled as warning message. // Fixed in newer firmware (unreleased at this point), but still in older firmware. - if (messageText.contains(APM_COPTER_REXP) || messageText.contains(APM_SOLO_REXP) || messageText.contains(APM_PLANE_REXP) || messageText.contains(APM_ROVER_REXP) || + if (messageText.contains(APM_COPTER_REXP) || messageText.contains(APM_PLANE_REXP) || messageText.contains(APM_ROVER_REXP) || messageText.contains(APM_PX4NUTTX_REXP) || messageText.contains(APM_FRAME_REXP) || messageText.contains(APM_SYSID_REXP)) { _setInfoSeverity(message); } + if (messageText.contains(APM_SOLO_REXP)) { + qDebug() << "Found Solo"; + + // Fix up severity + _setInfoSeverity(message); + + // Start TCP video handshake with ARTOO + _soloVideoHandshake(vehicle); + } + // ArduPilot PreArm messages can come across very frequently especially on Solo, which seems to send them once a second. // Filter them out if they come too quickly. if (messageText.startsWith("PreArm")) { @@ -599,3 +613,18 @@ void APMFirmwarePlugin::pauseVehicle(Vehicle* vehicle) // Best we can do in this case vehicle->setFlightMode("Loiter"); } + +void APMFirmwarePlugin::_soloVideoHandshake(Vehicle* vehicle) +{ + Q_UNUSED(vehicle); + + QTcpSocket* socket = new QTcpSocket(); + + socket->connectToHost(_artooIP, _artooVideoHandshakePort); + QObject::connect(socket, static_cast(&QTcpSocket::error), this, &APMFirmwarePlugin::_artooSocketError); +} + +void APMFirmwarePlugin::_artooSocketError(QAbstractSocket::SocketError socketError) +{ + qgcApp()->showMessage(tr("Error during Solo video link setup: %1").arg(socketError)); +} diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index bacd4dafa3112fdf49d05885cf8ec9e71c93521f..24c8efa43580a111b1792c9b035a01e3a65ad3a2 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -31,6 +31,8 @@ #include "QGCLoggingCategory.h" #include "APMParameterMetaData.h" +#include + Q_DECLARE_LOGGING_CATEGORY(APMFirmwarePluginLog) class APMFirmwareVersion @@ -107,6 +109,9 @@ protected: /// All access to singleton is through stack specific implementation APMFirmwarePlugin(void); void setSupportedModes(QList supportedModes); + +private slots: + void _artooSocketError(QAbstractSocket::SocketError socketError); private: void _adjustSeverity(mavlink_message_t* message) const; @@ -118,11 +123,16 @@ private: void _handleParamSet(Vehicle* vehicle, mavlink_message_t* message); bool _handleStatusText(Vehicle* vehicle, mavlink_message_t* message); void _handleHeartbeat(Vehicle* vehicle, mavlink_message_t* message); + void _soloVideoHandshake(Vehicle* vehicle); APMFirmwareVersion _firmwareVersion; bool _textSeverityAdjustmentNeeded; QList _supportedModes; QMap _noisyPrearmMap; + + static const char* _artooIP; + static const int _artooVideoHandshakePort; + }; #endif diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 995eef73f8fd0cbefde8d705b219e452d6a4ddc9..6b6787bb2714040e597413be4571af73c8e76325 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -56,6 +56,9 @@ public: GuidedModeCapability = 1 << 3, ///< Vehicle Support guided mode commands } FirmwareCapabilities; + /// Called when Vehicle is first created to send any necessary mavlink messages to the firmware. + virtual void initializeVehicle(Vehicle* vehicle); + /// @return true: Firmware supports all specified capabilites virtual bool isCapable(FirmwareCapabilities capabilities); @@ -130,9 +133,6 @@ public: /// @param message[in,out] Mavlink message to adjust if needed. virtual void adjustOutgoingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message); - /// Called when Vehicle is first created to send any necessary mavlink messages to the firmware. - virtual void initializeVehicle(Vehicle* vehicle); - /// Determines how to handle the first item of the mission item list. Internally to QGC the first item /// is always the home position. /// @return diff --git a/src/FlightDisplay/FlightDisplayViewController.cc b/src/FlightDisplay/FlightDisplayViewController.cc index 387bc1215b007152e475903f2db137c2fd2211c2..94bdf3309e20bc506f446438cb7f8d28971b7e74 100644 --- a/src/FlightDisplay/FlightDisplayViewController.cc +++ b/src/FlightDisplay/FlightDisplayViewController.cc @@ -40,7 +40,7 @@ FlightDisplayViewController::FlightDisplayViewController(QObject *parent) * This is the receiving end of an UDP RTP stream. The sender can be setup with this command: * * gst-launch-1.0 uvch264src initial-bitrate=1000000 average-bitrate=1000000 iframe-period=1000 name=src auto-start=true src.vidsrc ! \ - * video/x-h264,width=1280,height=720,framerate=24/1 ! h264parse ! rtph264pay ! udpsink host=192.168.1.9 port=5000 + * video/x-h264,width=1280,height=720,framerate=24/1 ! h264parse ! rtph264pay ! udpsink host=192.168.1.9 port=5600 * * Where the main parameters are: * @@ -63,7 +63,7 @@ FlightDisplayViewController::FlightDisplayViewController(QObject *parent) */ _videoSurface = new VideoSurface; _videoReceiver = new VideoReceiver(this); - _videoReceiver->setUri(QLatin1Literal("udp://0.0.0.0:5000")); + _videoReceiver->setUri(QLatin1Literal("udp://0.0.0.0:5600")); // Port 5600=Solo UDP port, if you change you will break Solo video support #if defined(QGC_GST_STREAMING) _videoReceiver->setVideoSink(_videoSurface->videoSink()); connect(&_frameTimer, &QTimer::timeout, this, &FlightDisplayViewController::_updateTimer); diff --git a/src/VideoStreaming/README.md b/src/VideoStreaming/README.md index 5b989b6bcd8d558536129aa0bd1d74051e159e34..2c4d57cbdb9e6f09bba44d3cf6374f7e2c81c077 100644 --- a/src/VideoStreaming/README.md +++ b/src/VideoStreaming/README.md @@ -12,19 +12,19 @@ If you do have the proper GStreamer development libraries installed where QGC lo For the time being, the pipeline is somewhat hardcoded, using h.264. It's best to use a camera capable of hardware encoding h.264, such as the Logitech C920. On the sender end, you would run something like this: ``` -gst-launch-1.0 uvch264src initial-bitrate=1000000 average-bitrate=1000000 iframe-period=1000 device=/dev/video0 name=src auto-start=true src.vidsrc ! video/x-h264,width=1920,height=1080,framerate=24/1 ! h264parse ! rtph264pay ! udpsink host=xxx.xxx.xxx.xxx port=5000 +gst-launch-1.0 uvch264src initial-bitrate=1000000 average-bitrate=1000000 iframe-period=1000 device=/dev/video0 name=src auto-start=true src.vidsrc ! video/x-h264,width=1920,height=1080,framerate=24/1 ! h264parse ! rtph264pay ! udpsink host=xxx.xxx.xxx.xxx port=5600 ``` Where xxx.xxx.xxx.xxx is the IP address where QGC is running. You may tweak the bitrate, the resolution and the FPS based on your needs and/or available bandwidth. To test using a test source on localhost, you can run this command: ``` -gst-launch-1.0 videotestsrc pattern=ball ! x264enc ! rtph264pay ! udpsink host=127.0.0.1 port=5000 +gst-launch-1.0 videotestsrc pattern=ball ! x264enc ! rtph264pay ! udpsink host=127.0.0.1 port=5600 ``` On the receiving end, if you want to test it from the command line, you can use something like: ``` -gst-launch-1.0 udpsrc port=5000 caps='application/x-rtp, media=(string)video, clock-rate=(int)90000, encoding-name=(string)H264' ! rtph264depay ! avdec_h264 ! autovideosink fps-update-interval=1000 sync=false +gst-launch-1.0 udpsrc port=5600 caps='application/x-rtp, media=(string)video, clock-rate=(int)90000, encoding-name=(string)H264' ! rtph264depay ! avdec_h264 ! autovideosink fps-update-interval=1000 sync=false ``` ### Linux