Skip to content

Q
qgroundcontrol
Unverified Commit 02216607 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

New Solo video handshake sequence (#9155) 

* Add 3DR Solo, Parrot Discovery video sources

* New solo video handshake, remove stale code
parent 10a51723
Hide whitespace changes
Inline Side-by-side
Showing with 44 additions and 308 deletions
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
View file @ 02216607
......@@ -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<APMFirmwarePluginInstanceData*>(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<APMCustomMode> 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<void (QTcpSocket::*)(QAbstractSocket::SocketError)>(&QTcpSocket::error), this, &APMFirmwarePlugin::_artooSocketError);
QObject::connect(socket, static_cast<void (QTcpSocket::*)(QAbstractSocket::SocketError)>(&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)
......
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
View file @ 02216607
......@@ -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
src/Settings/VideoSettings.cc
View file @ 02216607
......@@ -19,13 +19,15 @@
#include <QCameraInfo>
#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<QCameraInfo> cameras = QCameraInfo::availableCameras();
......
src/Settings/VideoSettings.h
View file @ 02216607
......@@ -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);
......
src/VideoManager/VideoManager.cc
View file @ 02216607
......@@ -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;
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment