zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

New Solo video handshake sequence (#9155) 

* Add 3DR Solo, Parrot Discovery video sources

* New solo video handshake, remove stale code
QGC4.4
Don Gagne 5 years ago committed by GitHub
parent
10a51723f1
commit
0221660733
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 44 additions and 308 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 274
      src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
  2. 37
      src/FirmwarePlugin/APM/APMFirmwarePlugin.h
  3. 18
      src/Settings/VideoSettings.cc
  4. 4
      src/Settings/VideoSettings.h
  5. 19
      src/VideoManager/VideoManager.cc

274
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
Unescape View File

@ -31,100 +31,12 @@ @@ -31,100 +31,12 @@
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
*/
APMCustomMode::APMCustomMode(uint32_t mode, bool settable) :
@ -147,13 +59,6 @@ QString APMCustomMode::modeString() const @@ -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*/, @@ -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 @@ -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) @@ -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) @@ -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 @@ -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)

37
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
Unescape View File

@ -23,29 +23,6 @@ @@ -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: @@ -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: @@ -146,14 +121,4 @@ private:
static const int _artooVideoHandshakePort;
};
class APMFirmwarePluginInstanceData : public QObject
{
Q_OBJECT
public:
APMFirmwarePluginInstanceData(QObject* parent = nullptr);
bool textSeverityAdjustmentNeeded;
};
#endif

18
src/Settings/VideoSettings.cc
Unescape View File

@ -19,13 +19,15 @@ @@ -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") @@ -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();

4
src/Settings/VideoSettings.h
Unescape View File

@ -42,7 +42,7 @@ public: @@ -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: @@ -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);

19
src/VideoManager/VideoManager.cc
Unescape View File

@ -525,13 +525,14 @@ VideoManager::isGStreamer() @@ -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) @@ -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;
}

Write Preview
Loading…
Cancel
Save