From 02216607339e9736db32b7ce4db85a6c16c2c241 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 31 Oct 2020 08:56:24 -0700 Subject: [PATCH] New Solo video handshake sequence (#9155) * Add 3DR Solo, Parrot Discovery video sources * New solo video handshake, remove stale code --- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 274 ++------------------ src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 37 +-- src/Settings/VideoSettings.cc | 18 +- src/Settings/VideoSettings.h | 4 +- src/VideoManager/VideoManager.cc | 19 +- 5 files changed, 44 insertions(+), 308 deletions(-) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index db67aad15..895d00a83 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -31,99 +31,11 @@ QGC_LOGGING_CATEGORY(APMFirmwarePluginLog, "APMFirmwarePluginLog") -static const QRegExp APM_COPTER_REXP("^(ArduCopter|APM:Copter)"); -static const QRegExp APM_SOLO_REXP("^(APM:Copter solo-)"); -static const QRegExp APM_PLANE_REXP("^(ArduPlane|APM:Plane)"); -static const QRegExp APM_ROVER_REXP("^(ArduRover|APM:Rover)"); -static const QRegExp APM_SUB_REXP("^(ArduSub|APM:Sub)"); -static const QRegExp APM_PX4NUTTX_REXP("^PX4: .*NuttX: .*"); static const QRegExp APM_FRAME_REXP("^Frame: "); -static const QRegExp APM_SYSID_REXP("^PX4v2 "); - -// Regex to parse version text coming from APM, gives out firmware type, major, minor and patch level numbers -static const QRegExp VERSION_REXP("^(APM:Copter|APM:Plane|APM:Rover|APM:Sub|ArduCopter|ArduPlane|ArduRover|ArduSub) +[vV](\\d*)\\.*(\\d*)*\\.*(\\d*)*"); - -// minimum firmware versions that don't suffer from mavlink severity inversion bug. -// https://github.com/diydrones/apm_planner/issues/788 -static const QString MIN_SOLO_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Copter solo-1.2.0"); -static const QString MIN_COPTER_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Copter V3.4.0"); -static const QString MIN_PLANE_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Plane V3.4.0"); -static const QString MIN_SUB_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Sub 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 - * It encabsules vehicleType, major version, minor version and patch level version - * and provides accessors for the same. - * isValid() can be used, to know whether version infromation is available or not - * supports < operator - */ -APMFirmwareVersion::APMFirmwareVersion(const QString &versionText) -{ - _major = 0; - _minor = 0; - _patch = 0; - - _parseVersion(versionText); -} - -bool APMFirmwareVersion::isValid() const -{ - return !_versionString.isEmpty(); -} - -bool APMFirmwareVersion::isBeta() const -{ - return _versionString.contains(QStringLiteral(".rc")); -} - -bool APMFirmwareVersion::isDev() const -{ - return _versionString.contains(QStringLiteral(".dev")); -} - -bool APMFirmwareVersion::operator <(const APMFirmwareVersion& other) const -{ - int myVersion = _major << 16 | _minor << 8 | _patch ; - int otherVersion = other.majorNumber() << 16 | other.minorNumber() << 8 | other.patchNumber(); - return myVersion < otherVersion; -} - -void APMFirmwareVersion::_parseVersion(const QString &versionText) -{ - if (versionText.isEmpty()) { - return; - } - - - if (VERSION_REXP.indexIn(versionText) == -1) { - qCWarning(APMFirmwarePluginLog) << "firmware version regex didn't match anything" - << "version text to be parsed" << versionText; - return; - } - - QStringList capturedTexts = VERSION_REXP.capturedTexts(); - - if (capturedTexts.count() < 5) { - qCWarning(APMFirmwarePluginLog) << "something wrong with parsing the version text, not hitting anything" - << VERSION_REXP.captureCount() << VERSION_REXP.capturedTexts(); - return; - } - - // successful extraction of version numbers - // even though we could have collected the version string atleast - // but if the parsing has faild, not much point - _versionString = versionText; - _vehicleType = capturedTexts[1]; - _major = capturedTexts[2].toInt(); - _minor = capturedTexts[3].toInt(); - _patch = capturedTexts[4].toInt(); -} - - /* * @brief APMCustomMode encapsulates the custom modes for APM */ @@ -147,13 +59,6 @@ QString APMCustomMode::modeString() const return mode; } -APMFirmwarePluginInstanceData::APMFirmwarePluginInstanceData(QObject* parent) - : QObject(parent) - , textSeverityAdjustmentNeeded(false) -{ - -} - APMFirmwarePlugin::APMFirmwarePlugin(void) : _coaxialMotors(false) { @@ -339,106 +244,18 @@ void APMFirmwarePlugin::_handleOutgoingParamSetThreadSafe(Vehicle* /*vehicle*/, _adjustOutgoingMavlinkMutex.unlock(); } -bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message) +bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* /*vehicle*/, mavlink_message_t* message) { - QString messageText; - APMFirmwarePluginInstanceData* instanceData = qobject_cast(vehicle->firmwarePluginInstanceData()); - - int severity = mavlink_msg_statustext_get_severity(message); - - if (vehicle->firmwareMajorVersion() == Vehicle::versionNotSetValue || severity < MAV_SEVERITY_NOTICE) { - messageText = _getMessageText(message); - qCDebug(APMFirmwarePluginLog) << messageText; - - if (!messageText.contains(APM_SOLO_REXP)) { - // if don't know firmwareVersion yet, try and see if this message contains it - if (messageText.contains(APM_COPTER_REXP) || messageText.contains(APM_PLANE_REXP) || messageText.contains(APM_ROVER_REXP) || messageText.contains(APM_SUB_REXP)) { - // found version string - APMFirmwareVersion firmwareVersion(messageText); - instanceData->textSeverityAdjustmentNeeded = _isTextSeverityAdjustmentNeeded(firmwareVersion); - - vehicle->setFirmwareVersion(firmwareVersion.majorNumber(), firmwareVersion.minorNumber(), firmwareVersion.patchNumber()); - - int supportedMajorNumber = -1; - int supportedMinorNumber = -1; - - switch (vehicle->vehicleType()) { - case MAV_TYPE_VTOL_DUOROTOR: - case MAV_TYPE_VTOL_QUADROTOR: - case MAV_TYPE_VTOL_TILTROTOR: - case MAV_TYPE_VTOL_RESERVED2: - case MAV_TYPE_VTOL_RESERVED3: - case MAV_TYPE_VTOL_RESERVED4: - case MAV_TYPE_VTOL_RESERVED5: - case MAV_TYPE_FIXED_WING: - supportedMajorNumber = 3; - supportedMinorNumber = 8; - break; - case MAV_TYPE_QUADROTOR: - // Start TCP video handshake with ARTOO in case it's a Solo running ArduPilot firmware - _soloVideoHandshake(vehicle, false /* originalSoloFirmware */); - [[fallthrough]]; - case MAV_TYPE_COAXIAL: - case MAV_TYPE_HELICOPTER: - case MAV_TYPE_SUBMARINE: - case MAV_TYPE_HEXAROTOR: - case MAV_TYPE_OCTOROTOR: - case MAV_TYPE_TRICOPTER: - supportedMajorNumber = 3; - supportedMinorNumber = 5; - break; - case MAV_TYPE_GROUND_ROVER: - case MAV_TYPE_SURFACE_BOAT: - supportedMajorNumber = 3; - supportedMinorNumber = 4; - default: - break; - } - - if (supportedMajorNumber != -1) { - if (firmwareVersion.majorNumber() < supportedMajorNumber || - (firmwareVersion.majorNumber() == supportedMajorNumber && firmwareVersion.minorNumber() < supportedMinorNumber)) { - qgcApp()->showAppMessage(tr("QGroundControl fully supports Version %1.%2 and above. You are using a version prior to that. This combination is untested, you may run into unpredictable results.").arg(supportedMajorNumber).arg(supportedMinorNumber)); - } - } - } - } - - // APM user facing calibration messages come through as high severity, we need to parse them out - // and lower the severity on them so that they don't pop in the users face. - - if (messageText.contains("Place vehicle") || messageText.contains("Calibration successful")) { - _adjustCalibrationMessageSeverity(message); - return true; - } - } + // APM user facing calibration messages come through as high severity, we need to parse them out + // and lower the severity on them so that they don't pop in the users face. - // adjust mesasge if needed - if (instanceData->textSeverityAdjustmentNeeded) { - _adjustSeverity(message); - } - - if (messageText.isEmpty()) { - messageText = _getMessageText(message); - } - - // 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_PLANE_REXP) || messageText.contains(APM_ROVER_REXP) || messageText.contains(APM_SUB_REXP) || - messageText.contains(APM_PX4NUTTX_REXP) || messageText.contains(APM_FRAME_REXP) || messageText.contains(APM_SYSID_REXP)) { - _setInfoSeverity(message); + QString messageText = _getMessageText(message); + if (messageText.contains("Place vehicle") || messageText.contains("Calibration successful")) { + _adjustCalibrationMessageSeverity(message); + return true; } - if (messageText.contains(APM_SOLO_REXP)) { - qDebug() << "Found Solo"; - vehicle->setSoloFirmware(true); - - // Fix up severity - _setInfoSeverity(message); - - // Start TCP video handshake with ARTOO - _soloVideoHandshake(vehicle, true /* originalSoloFirmware */); - } else if (messageText.contains(APM_FRAME_REXP)) { + if (messageText.contains(APM_FRAME_REXP)) { // We need to parse the Frame: message in order to determine whether the motors are coaxial or not QRegExp frameTypeRegex("^Frame: (\\S*)"); if (frameTypeRegex.indexIn(messageText) != -1) { @@ -529,61 +346,6 @@ QString APMFirmwarePlugin::_getMessageText(mavlink_message_t* message) const return QString(b); } -bool APMFirmwarePlugin::_isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion) -{ - if (!firmwareVersion.isValid()) { - return false; - } - - bool adjustmentNeeded = false; - if (firmwareVersion.vehicleType().contains(APM_COPTER_REXP)) { - if (firmwareVersion < APMFirmwareVersion(MIN_COPTER_VERSION_WITH_CORRECT_SEVERITY_MSGS)) { - adjustmentNeeded = true; - } - } else if (firmwareVersion.vehicleType().contains(APM_PLANE_REXP)) { - if (firmwareVersion < APMFirmwareVersion(MIN_PLANE_VERSION_WITH_CORRECT_SEVERITY_MSGS)) { - adjustmentNeeded = true; - } - } else if (firmwareVersion.vehicleType().contains(APM_ROVER_REXP)) { - if (firmwareVersion < APMFirmwareVersion(MIN_ROVER_VERSION_WITH_CORRECT_SEVERITY_MSGS)) { - adjustmentNeeded = true; - } - } else if (firmwareVersion.vehicleType().contains(APM_SUB_REXP)) { - if (firmwareVersion < APMFirmwareVersion(MIN_SUB_VERSION_WITH_CORRECT_SEVERITY_MSGS)) { - adjustmentNeeded = true; - } - } - - return adjustmentNeeded; -} - -void APMFirmwarePlugin::_adjustSeverity(mavlink_message_t* message) const -{ - // lets make QGC happy with right severity values - mavlink_statustext_t statusText; - mavlink_msg_statustext_decode(message, &statusText); - switch(statusText.severity) { - case MAV_SEVERITY_ALERT: /* SEVERITY_LOW according to old codes */ - statusText.severity = MAV_SEVERITY_WARNING; - break; - case MAV_SEVERITY_CRITICAL: /*SEVERITY_MEDIUM according to old codes */ - statusText.severity = MAV_SEVERITY_ALERT; - break; - case MAV_SEVERITY_ERROR: /*SEVERITY_HIGH according to old codes */ - statusText.severity = MAV_SEVERITY_CRITICAL; - break; - } - - // Re-Encoding is always done using mavlink 1.0 - mavlink_status_t* mavlinkStatusReEncode = mavlink_get_channel_status(0); - mavlinkStatusReEncode->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1; - mavlink_msg_statustext_encode_chan(message->sysid, - message->compid, - 0, // Re-encoding uses reserved channel 0 - message, - &statusText); -} - void APMFirmwarePlugin::_setInfoSeverity(mavlink_message_t* message) const { // Re-Encoding is always done using mavlink 1.0 @@ -650,8 +412,6 @@ void APMFirmwarePlugin::initializeStreamRates(Vehicle* vehicle) void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle) { - vehicle->setFirmwarePluginInstanceData(new APMFirmwarePluginInstanceData); - if (vehicle->isOfflineEditingVehicle()) { switch (vehicle->vehicleType()) { case MAV_TYPE_QUADROTOR: @@ -689,6 +449,10 @@ void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle) initializeStreamRates(vehicle); } } + + if (qgcApp()->toolbox()->settingsManager()->videoSettings()->videoSource()->rawValue() == VideoSettings::videoSource3DRSolo) { + _soloVideoHandshake(); + } } void APMFirmwarePlugin::setSupportedModes(QList supportedModes) @@ -814,20 +578,16 @@ bool APMFirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const return vehicle->flightMode() == "Guided"; } -void APMFirmwarePlugin::_soloVideoHandshake(Vehicle* vehicle, bool originalSoloFirmware) +void APMFirmwarePlugin::_soloVideoHandshake(void) { - Q_UNUSED(vehicle); - - QTcpSocket* socket = new QTcpSocket(); + QTcpSocket* socket = new QTcpSocket(this); - socket->connectToHost(_artooIP, _artooVideoHandshakePort); - if (originalSoloFirmware) { #if QT_VERSION < QT_VERSION_CHECK(5, 15, 0) - QObject::connect(socket, static_cast(&QTcpSocket::error), this, &APMFirmwarePlugin::_artooSocketError); + QObject::connect(socket, static_cast(&QTcpSocket::error), this, &APMFirmwarePlugin::_artooSocketError); #else - QObject::connect(socket, &QAbstractSocket::errorOccurred, this, &APMFirmwarePlugin::_artooSocketError); + QObject::connect(socket, &QAbstractSocket::errorOccurred, this, &APMFirmwarePlugin::_artooSocketError); #endif - } + socket->connectToHost(_artooIP, _artooVideoHandshakePort); } void APMFirmwarePlugin::_artooSocketError(QAbstractSocket::SocketError socketError) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 5c4fe9480..2bf246105 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -23,29 +23,6 @@ Q_DECLARE_LOGGING_CATEGORY(APMFirmwarePluginLog) -class APMFirmwareVersion -{ -public: - APMFirmwareVersion(const QString &versionText = ""); - bool isValid() const; - bool isBeta() const; - bool isDev() const; - bool operator<(const APMFirmwareVersion& other) const; - QString versionString() const { return _versionString; } - QString vehicleType() const { return _vehicleType; } - int majorNumber() const { return _major; } - int minorNumber() const { return _minor; } - int patchNumber() const { return _patch; } - -private: - void _parseVersion(const QString &versionText); - QString _versionString; - QString _vehicleType; - int _major; - int _minor; - int _patch; -}; - class APMCustomMode { public: @@ -118,16 +95,14 @@ private slots: void _artooSocketError(QAbstractSocket::SocketError socketError); private: - void _adjustSeverity(mavlink_message_t* message) const; void _adjustCalibrationMessageSeverity(mavlink_message_t* message) const; - static bool _isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion); void _setInfoSeverity(mavlink_message_t* message) const; QString _getMessageText(mavlink_message_t* message) const; void _handleIncomingParamValue(Vehicle* vehicle, mavlink_message_t* message); bool _handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message); void _handleIncomingHeartbeat(Vehicle* vehicle, mavlink_message_t* message); void _handleOutgoingParamSetThreadSafe(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message); - void _soloVideoHandshake(Vehicle* vehicle, bool originalSoloFirmware); + void _soloVideoHandshake(void); bool _guidedModeTakeoff(Vehicle* vehicle, double altitudeRel); void _handleRCChannels(Vehicle* vehicle, mavlink_message_t* message); void _handleRCChannelsRaw(Vehicle* vehicle, mavlink_message_t* message); @@ -146,14 +121,4 @@ private: static const int _artooVideoHandshakePort; }; -class APMFirmwarePluginInstanceData : public QObject -{ - Q_OBJECT - -public: - APMFirmwarePluginInstanceData(QObject* parent = nullptr); - - bool textSeverityAdjustmentNeeded; -}; - #endif diff --git a/src/Settings/VideoSettings.cc b/src/Settings/VideoSettings.cc index c42d46cf5..8374747f0 100644 --- a/src/Settings/VideoSettings.cc +++ b/src/Settings/VideoSettings.cc @@ -19,13 +19,15 @@ #include #endif -const char* VideoSettings::videoSourceNoVideo = "No Video Available"; -const char* VideoSettings::videoDisabled = "Video Stream Disabled"; -const char* VideoSettings::videoSourceRTSP = "RTSP Video Stream"; -const char* VideoSettings::videoSourceUDPH264 = "UDP h.264 Video Stream"; -const char* VideoSettings::videoSourceUDPH265 = "UDP h.265 Video Stream"; -const char* VideoSettings::videoSourceTCP = "TCP-MPEG2 Video Stream"; -const char* VideoSettings::videoSourceMPEGTS = "MPEG-TS (h.264) Video Stream"; +const char* VideoSettings::videoSourceNoVideo = "No Video Available"; +const char* VideoSettings::videoDisabled = "Video Stream Disabled"; +const char* VideoSettings::videoSourceRTSP = "RTSP Video Stream"; +const char* VideoSettings::videoSourceUDPH264 = "UDP h.264 Video Stream"; +const char* VideoSettings::videoSourceUDPH265 = "UDP h.265 Video Stream"; +const char* VideoSettings::videoSourceTCP = "TCP-MPEG2 Video Stream"; +const char* VideoSettings::videoSourceMPEGTS = "MPEG-TS (h.264) Video Stream"; +const char* VideoSettings::videoSource3DRSolo = "3DR Solo"; +const char* VideoSettings::videoSourceParrotDiscovery = "Parrot Discovery"; DECLARE_SETTINGGROUP(Video, "Video") { @@ -41,6 +43,8 @@ DECLARE_SETTINGGROUP(Video, "Video") #endif videoSourceList.append(videoSourceTCP); videoSourceList.append(videoSourceMPEGTS); + videoSourceList.append(videoSource3DRSolo); + videoSourceList.append(videoSourceParrotDiscovery); #endif #ifndef QGC_DISABLE_UVC QList cameras = QCameraInfo::availableCameras(); diff --git a/src/Settings/VideoSettings.h b/src/Settings/VideoSettings.h index 1e927889d..24373dc9a 100644 --- a/src/Settings/VideoSettings.h +++ b/src/Settings/VideoSettings.h @@ -42,7 +42,7 @@ public: Q_PROPERTY(QString udp265VideoSource READ udp265VideoSource CONSTANT) Q_PROPERTY(QString tcpVideoSource READ tcpVideoSource CONSTANT) Q_PROPERTY(QString mpegtsVideoSource READ mpegtsVideoSource CONSTANT) - Q_PROPERTY(QString disabledVideoSource READ disabledVideoSource CONSTANT) + Q_PROPERTY(QString disabledVideoSource READ disabledVideoSource CONSTANT) bool streamConfigured (); QString rtspVideoSource () { return videoSourceRTSP; } @@ -59,6 +59,8 @@ public: static const char* videoSourceRTSP; static const char* videoSourceTCP; static const char* videoSourceMPEGTS; + static const char* videoSource3DRSolo; + static const char* videoSourceParrotDiscovery; signals: void streamConfiguredChanged (bool configured); diff --git a/src/VideoManager/VideoManager.cc b/src/VideoManager/VideoManager.cc index e7440b00a..75f0bcaa5 100644 --- a/src/VideoManager/VideoManager.cc +++ b/src/VideoManager/VideoManager.cc @@ -525,13 +525,14 @@ VideoManager::isGStreamer() { #if defined(QGC_GST_STREAMING) QString videoSource = _videoSettings->videoSource()->rawValue().toString(); - return - videoSource == VideoSettings::videoSourceUDPH264 || - videoSource == VideoSettings::videoSourceUDPH265 || - videoSource == VideoSettings::videoSourceRTSP || - videoSource == VideoSettings::videoSourceTCP || - videoSource == VideoSettings::videoSourceMPEGTS || - autoStreamConfigured(); + return videoSource == VideoSettings::videoSourceUDPH264 || + videoSource == VideoSettings::videoSourceUDPH265 || + videoSource == VideoSettings::videoSourceRTSP || + videoSource == VideoSettings::videoSourceTCP || + videoSource == VideoSettings::videoSourceMPEGTS || + videoSource == VideoSettings::videoSource3DRSolo || + videoSource == VideoSettings::videoSourceParrotDiscovery || + autoStreamConfigured(); #else return false; #endif @@ -685,6 +686,10 @@ VideoManager::_updateSettings(unsigned id) settingsChanged |= _updateVideoUri(0, _videoSettings->rtspUrl()->rawValue().toString()); else if (source == VideoSettings::videoSourceTCP) settingsChanged |= _updateVideoUri(0, QStringLiteral("tcp://%1").arg(_videoSettings->tcpUrl()->rawValue().toString())); + else if (source == VideoSettings::videoSource3DRSolo) + settingsChanged |= _updateVideoUri(0, QStringLiteral("udp://0.0.0.0:5600")); + else if (source == VideoSettings::videoSourceParrotDiscovery) + settingsChanged |= _updateVideoUri(0, QStringLiteral("udp://0.0.0.0:8888")); return settingsChanged; } -- 2.22.0