Merge pull request #3085 from DonLakeFlyer/SoloVideo

Add Solo video support

src/FirmwarePlugin/APM/APMFirmwarePlugin.cc

@@ -29,6 +29,8 @@
 #include "QGCMAVLink.h"
 #include "QGCApplication.h"
 
+#include <QTcpSocket>
+
 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<void (QTcpSocket::*)(QAbstractSocket::SocketError)>(&QTcpSocket::error), this, &APMFirmwarePlugin::_artooSocketError);
+}
+
+void APMFirmwarePlugin::_artooSocketError(QAbstractSocket::SocketError socketError)
+{
+    qgcApp()->showMessage(tr("Error during Solo video link setup: %1").arg(socketError));
+}

src/FirmwarePlugin/APM/APMFirmwarePlugin.h

@@ -31,6 +31,8 @@
 #include "QGCLoggingCategory.h"
 #include "APMParameterMetaData.h"
 
+#include <QAbstractSocket>
+
 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<APMCustomMode> 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<APMCustomMode>    _supportedModes;
     QMap<QString, QTime>    _noisyPrearmMap;
+
+    static const char*  _artooIP;
+    static const int    _artooVideoHandshakePort;
+
 };
 
 #endif

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

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);

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