diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 898e2c54488e096d3e1e593e1f9365941a1cc853..5ee2a5594782f1d280a0243f55769f345c2590ef 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -26,15 +26,100 @@ #include "APMFirmwarePlugin.h" #include "Generic/GenericFirmwarePlugin.h" +#include "QGCMAVLink.h" #include IMPLEMENT_QGC_SINGLETON(APMFirmwarePlugin, FirmwarePlugin) +QGC_LOGGING_CATEGORY(APMFirmwarePluginLog, "APMFirmwarePluginLog") + +static const QRegExp APM_COPTER_REXP("^(ArduCopter|APM:Copter)"); +static const QRegExp APM_PLANE_REXP("^(ArduPlane|APM:Plane)"); +static const QRegExp APM_ROVER_REXP("^(ArduRover|APM:Rover)"); + +// 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|ArduCopter|ArduPlane|ArduRover) +[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_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_ROVER_VERSION_WITH_CORRECT_SEVERITY_MSGS("APM:Rover V2.6.0"); + + +/* + * @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(); +} APMFirmwarePlugin::APMFirmwarePlugin(QObject* parent) : FirmwarePlugin(parent) { - + _textSeverityAdjustmentNeeded = false; } bool APMFirmwarePlugin::isCapable(FirmwareCapabilities capabilities) @@ -120,7 +205,7 @@ void APMFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message) paramUnion.param_float = paramValue.param_value; break; default: - qCritical() << "Invalid/Unsupported data type used in parameter:" << paramValue.param_type; + qCCritical(APMFirmwarePluginLog) << "Invalid/Unsupported data type used in parameter:" << paramValue.param_type; } paramValue.param_value = paramUnion.param_float; @@ -161,13 +246,80 @@ void APMFirmwarePlugin::adjustMavlinkMessage(mavlink_message_t* message) // Already in param_float break; default: - qCritical() << "Invalid/Unsupported data type used in parameter:" << paramSet.param_type; + qCCritical(APMFirmwarePluginLog) << "Invalid/Unsupported data type used in parameter:" << paramSet.param_type; } mavlink_msg_param_set_encode(message->sysid, message->compid, message, ¶mSet); } - // FIXME: Need to implement mavlink message severity adjustment + if (message->msgid == MAVLINK_MSG_ID_STATUSTEXT) + { + if (!_firmwareVersion.isValid()) { + QByteArray b; + b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1); + mavlink_msg_statustext_get_text(message, b.data()); + // Ensure NUL-termination + b[b.length()-1] = '\0'; + QString text = QString(b); + qCDebug(APMFirmwarePluginLog) << text; + + // if don't know firmwareVersion yet, try and see this message contains it + if (text.contains(APM_COPTER_REXP) || text.contains(APM_PLANE_REXP) || text.contains(APM_ROVER_REXP)) { + // found version string + _firmwareVersion = APMFirmwareVersion(text); + _textSeverityAdjustmentNeeded = _isTextSeverityAdjustmentNeeded(_firmwareVersion); + } + } + + // adjust mesasge if needed + if (_textSeverityAdjustmentNeeded) { + _adjustSeverity(message); + } + } +} + +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; + } + } + + 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; + } + + mavlink_msg_statustext_encode(message->sysid, message->compid, message, &statusText); } void APMFirmwarePlugin::initializeVehicle(Vehicle* vehicle) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 6e798089bbf2fc32ea648a18a9d39456eb4104bf..67630e32614e8eaa05c3cf559d0cb61e1acf5ea2 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -29,6 +29,31 @@ #include "FirmwarePlugin.h" +Q_DECLARE_LOGGING_CATEGORY(APMFirmwarePluginLogsLog) + +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 APMFirmwarePlugin : public FirmwarePlugin { Q_OBJECT @@ -49,6 +74,12 @@ public: private: /// All access to singleton is through AutoPilotPluginManager::instance APMFirmwarePlugin(QObject* parent = NULL); + void _adjustSeverity(mavlink_message_t* message) const; + static bool _isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion); + + APMFirmwareVersion _firmwareVersion; + bool _textSeverityAdjustmentNeeded; + }; #endif