diff --git a/src/FactSystem/FactSystem.h b/src/FactSystem/FactSystem.h index 29089ceab3e25f62be9531ac1b39af08d2719113..2ca94973d093286e6792f285f60615c85c00f207 100644 --- a/src/FactSystem/FactSystem.h +++ b/src/FactSystem/FactSystem.h @@ -7,12 +7,7 @@ * ****************************************************************************/ - -/// @file -/// @author Don Gagne - -#ifndef FACTSYSTEM_H -#define FACTSYSTEM_H +#pragma once #include "Fact.h" #include "FactMetaData.h" @@ -26,7 +21,6 @@ class FactSystem : public QGCTool Q_OBJECT public: - /// All access to FactSystem is through FactSystem::instance, so constructor is private FactSystem(QGCApplication* app, QGCToolbox* toolbox); // Override from QGCTool @@ -41,5 +35,3 @@ public: private: static const char* _factSystemQmlUri; ///< URI for FactSystem QML imports }; - -#endif diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index df059a4fec4a1de9c5b3a740a2a1a1a3ecf2e036..68119e7ffcf0ba468f1dff1e788b441d3f50428e 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -325,16 +325,20 @@ void APMFirmwarePlugin::_handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* mavlink_msg_param_set_encode_chan(message->sysid, message->compid, outgoingLink->mavlinkChannel(), message, ¶mSet); } -bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message) +bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message, bool longVersion) { QString messageText; APMFirmwarePluginInstanceData* instanceData = qobject_cast(vehicle->firmwarePluginInstanceData()); - mavlink_statustext_t statusText; - mavlink_msg_statustext_decode(message, &statusText); + int severity; + if (longVersion) { + severity = mavlink_msg_statustext_long_get_severity(message); + } else { + severity = mavlink_msg_statustext_get_severity(message); + } - if (vehicle->firmwareMajorVersion() == Vehicle::versionNotSetValue || statusText.severity < MAV_SEVERITY_NOTICE) { - messageText = _getMessageText(message); + if (vehicle->firmwareMajorVersion() == Vehicle::versionNotSetValue || severity < MAV_SEVERITY_NOTICE) { + messageText = _getMessageText(message, longVersion); qCDebug(APMFirmwarePluginLog) << messageText; if (!messageText.contains(APM_SOLO_REXP)) { @@ -400,14 +404,14 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess } if (messageText.isEmpty()) { - messageText = _getMessageText(message); + messageText = _getMessageText(message, longVersion); } // 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); + _setInfoSeverity(message, longVersion); } if (messageText.contains(APM_SOLO_REXP)) { @@ -415,7 +419,7 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess vehicle->setSoloFirmware(true); // Fix up severity - _setInfoSeverity(message); + _setInfoSeverity(message, longVersion); // Start TCP video handshake with ARTOO _soloVideoHandshake(vehicle, true /* originalSoloFirmware */); @@ -465,7 +469,9 @@ bool APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_m _handleIncomingParamValue(vehicle, message); break; case MAVLINK_MSG_ID_STATUSTEXT: - return _handleIncomingStatusText(vehicle, message); + return _handleIncomingStatusText(vehicle, message, false /* longVersion */); + case MAVLINK_MSG_ID_STATUSTEXT_LONG: + return _handleIncomingStatusText(vehicle, message, true /* longVersion */); case MAVLINK_MSG_ID_HEARTBEAT: _handleIncomingHeartbeat(vehicle, message); break; @@ -494,12 +500,17 @@ void APMFirmwarePlugin::adjustOutgoingMavlinkMessage(Vehicle* vehicle, LinkInter } } -QString APMFirmwarePlugin::_getMessageText(mavlink_message_t* message) const +QString APMFirmwarePlugin::_getMessageText(mavlink_message_t* message, bool longVersion) const { QByteArray b; - b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1); - mavlink_msg_statustext_get_text(message, b.data()); + if (longVersion) { + b.resize(MAVLINK_MSG_STATUSTEXT_LONG_FIELD_TEXT_LEN+1); + mavlink_msg_statustext_long_get_text(message, b.data()); + } else { + 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'; @@ -561,20 +572,33 @@ void APMFirmwarePlugin::_adjustSeverity(mavlink_message_t* message) const &statusText); } -void APMFirmwarePlugin::_setInfoSeverity(mavlink_message_t* message) const +void APMFirmwarePlugin::_setInfoSeverity(mavlink_message_t* message, bool longVersion) const { - mavlink_statustext_t statusText; - mavlink_msg_statustext_decode(message, &statusText); - // 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; - statusText.severity = MAV_SEVERITY_INFO; - mavlink_msg_statustext_encode_chan(message->sysid, - message->compid, - 0, // Re-encoding uses reserved channel 0 - message, - &statusText); + + if (longVersion) { + mavlink_statustext_long_t statusTextLong; + mavlink_msg_statustext_long_decode(message, &statusTextLong); + + statusTextLong.severity = MAV_SEVERITY_INFO; + mavlink_msg_statustext_long_encode_chan(message->sysid, + message->compid, + 0, // Re-encoding uses reserved channel 0 + message, + &statusTextLong); + } else { + mavlink_statustext_t statusText; + mavlink_msg_statustext_decode(message, &statusText); + + statusText.severity = MAV_SEVERITY_INFO; + mavlink_msg_statustext_encode_chan(message->sysid, + message->compid, + 0, // Re-encoding uses reserved channel 0 + message, + &statusText); + } } void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* message) const diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h index 3abab25ee0976d6ab31f5cd4dd6f51926bf358cb..ddf754ed54ef60e4a2edc956c5a3e53b0a6ca31c 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -119,10 +119,10 @@ 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 _setInfoSeverity(mavlink_message_t* message, bool longVersion) const; + QString _getMessageText(mavlink_message_t* message, bool longVersion) const; void _handleIncomingParamValue(Vehicle* vehicle, mavlink_message_t* message); - bool _handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message); + bool _handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message, bool longVersion); void _handleIncomingHeartbeat(Vehicle* vehicle, mavlink_message_t* message); void _handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message); void _soloVideoHandshake(Vehicle* vehicle, bool originalSoloFirmware); diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 44eb73ff104ccf7c074ddc815264a15a20bb8c8d..9b5f3f56825c56bf368e6decbd6a48301387b9b6 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -425,7 +425,7 @@ void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoord return; } - if (vehicle->capabilityBits() && MAV_PROTOCOL_CAPABILITY_COMMAND_INT) { + if (vehicle->capabilityBits() & MAV_PROTOCOL_CAPABILITY_COMMAND_INT) { vehicle->sendMavCommandInt(vehicle->defaultComponentId(), MAV_CMD_DO_REPOSITION, MAV_FRAME_GLOBAL, diff --git a/src/MissionManager/FWLandingPattern.FactMetaData.json b/src/MissionManager/FWLandingPattern.FactMetaData.json index 5605343e594a4341f131e76b659b7013ea4ba92c..1970cdbdf99856e0296c60f3f166c080af44ed3f 100644 --- a/src/MissionManager/FWLandingPattern.FactMetaData.json +++ b/src/MissionManager/FWLandingPattern.FactMetaData.json @@ -51,7 +51,13 @@ "min": 0.1, "max": 90, "decimalPlaces": 1, - "defaultValue": 12.0 + "defaultValue": 6.0 +}, +{ + "name": "ValueSetIsDistance", + "shortDescription": "Value controller loiter point is distance", + "type": "bool", + "defaultValue": false }, { "name": "StopTakingPhotos", diff --git a/src/MissionManager/FWLandingPatternTest.cc b/src/MissionManager/FWLandingPatternTest.cc index 5d8bec72181593928b94d0baed2cb27fa71af0a3..aa4399e5abb1d9e06bc4ec3c147af551717e58ea 100644 --- a/src/MissionManager/FWLandingPatternTest.cc +++ b/src/MissionManager/FWLandingPatternTest.cc @@ -149,7 +149,8 @@ void FWLandingPatternTest::_testDirty(void) << _fwItem->landingDistance() << _fwItem->glideSlope() << _fwItem->stopTakingPhotos() - << _fwItem->stopTakingVideo(); + << _fwItem->stopTakingVideo() + << _fwItem->valueSetIsDistance(); for(Fact* fact: rgFacts) { qDebug() << fact->name(); QVERIFY(!_fwItem->dirty()); @@ -167,8 +168,7 @@ void FWLandingPatternTest::_testDirty(void) // These bool properties should set dirty when changed QList rgBoolNames; - rgBoolNames << "valueSetIsDistance" - << "loiterClockwise" + rgBoolNames << "loiterClockwise" << "altitudesAreRelative"; const QMetaObject* metaObject = _fwItem->metaObject(); for(const char* boolName: rgBoolNames) { @@ -223,16 +223,16 @@ void FWLandingPatternTest::_validateItem(FixedWingLandingComplexItem* newItem) QVERIFY(fuzzyCompareLatLon(newItem->loiterTangentCoordinate(), _fwItem->loiterTangentCoordinate())); QVERIFY(fuzzyCompareLatLon(newItem->landingCoordinate(), _fwItem->landingCoordinate())); - QCOMPARE(newItem->stopTakingPhotos()->rawValue().toBool(), _fwItem->stopTakingPhotos()->rawValue().toBool()); - QCOMPARE(newItem->stopTakingVideo()->rawValue().toBool(), _fwItem->stopTakingVideo()->rawValue().toBool()); - QCOMPARE(newItem->loiterAltitude()->rawValue().toInt(), _fwItem->loiterAltitude()->rawValue().toInt()); - QCOMPARE(newItem->loiterRadius()->rawValue().toInt(), _fwItem->loiterRadius()->rawValue().toInt()); - QCOMPARE(newItem->landingAltitude()->rawValue().toInt(), _fwItem->landingAltitude()->rawValue().toInt()); - QCOMPARE(newItem->landingHeading()->rawValue().toInt(), _fwItem->landingHeading()->rawValue().toInt()); - QCOMPARE(newItem->landingDistance()->rawValue().toInt(), _fwItem->landingDistance()->rawValue().toInt()); - QCOMPARE(newItem->glideSlope()->rawValue().toInt(), _fwItem->glideSlope()->rawValue().toInt()); - QCOMPARE(newItem->_valueSetIsDistance, _fwItem->_valueSetIsDistance); - QCOMPARE(newItem->_loiterClockwise, _fwItem->_loiterClockwise); - QCOMPARE(newItem->_altitudesAreRelative, _fwItem->_altitudesAreRelative); - QCOMPARE(newItem->_landingCoordSet, _fwItem->_landingCoordSet); + QCOMPARE(newItem->stopTakingPhotos()->rawValue().toBool(), _fwItem->stopTakingPhotos()->rawValue().toBool()); + QCOMPARE(newItem->stopTakingVideo()->rawValue().toBool(), _fwItem->stopTakingVideo()->rawValue().toBool()); + QCOMPARE(newItem->loiterAltitude()->rawValue().toInt(), _fwItem->loiterAltitude()->rawValue().toInt()); + QCOMPARE(newItem->loiterRadius()->rawValue().toInt(), _fwItem->loiterRadius()->rawValue().toInt()); + QCOMPARE(newItem->landingAltitude()->rawValue().toInt(), _fwItem->landingAltitude()->rawValue().toInt()); + QCOMPARE(newItem->landingHeading()->rawValue().toInt(), _fwItem->landingHeading()->rawValue().toInt()); + QCOMPARE(newItem->landingDistance()->rawValue().toInt(), _fwItem->landingDistance()->rawValue().toInt()); + QCOMPARE(newItem->glideSlope()->rawValue().toInt(), _fwItem->glideSlope()->rawValue().toInt()); + QCOMPARE(newItem->valueSetIsDistance()->rawValue().toBool(), _fwItem->valueSetIsDistance()->rawValue().toBool()); + QCOMPARE(newItem->_loiterClockwise, _fwItem->_loiterClockwise); + QCOMPARE(newItem->_altitudesAreRelative, _fwItem->_altitudesAreRelative); + QCOMPARE(newItem->_landingCoordSet, _fwItem->_landingCoordSet); } diff --git a/src/MissionManager/FixedWingLandingComplexItem.cc b/src/MissionManager/FixedWingLandingComplexItem.cc index 813205af6c6bf11a3d9caf4a7a2d500d0d1794ec..db3ad990abe4aa4318b89a4eca11b1b112a89386 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.cc +++ b/src/MissionManager/FixedWingLandingComplexItem.cc @@ -29,6 +29,7 @@ const char* FixedWingLandingComplexItem::landingAltitudeName = "LandingAlti const char* FixedWingLandingComplexItem::glideSlopeName = "GlideSlope"; const char* FixedWingLandingComplexItem::stopTakingPhotosName = "StopTakingPhotos"; const char* FixedWingLandingComplexItem::stopTakingVideoName = "StopTakingVideo"; +const char* FixedWingLandingComplexItem::valueSetIsDistanceName = "ValueSetIsDistance"; const char* FixedWingLandingComplexItem::_jsonLoiterCoordinateKey = "loiterCoordinate"; const char* FixedWingLandingComplexItem::_jsonLoiterRadiusKey = "loiterRadius"; @@ -59,9 +60,9 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, bool , _glideSlopeFact (settingsGroup, _metaDataMap[glideSlopeName]) , _stopTakingPhotosFact (settingsGroup, _metaDataMap[stopTakingPhotosName]) , _stopTakingVideoFact (settingsGroup, _metaDataMap[stopTakingVideoName]) + , _valueSetIsDistanceFact (settingsGroup, _metaDataMap[valueSetIsDistanceName]) , _loiterClockwise (true) , _altitudesAreRelative (true) - , _valueSetIsDistance (true) { _editorQml = "qrc:/qml/FWLandingPatternEditor.qml"; @@ -89,6 +90,7 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, bool connect(&_loiterRadiusFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(&_stopTakingPhotosFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(&_stopTakingVideoFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); + connect(&_valueSetIsDistanceFact, &Fact::valueChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(this, &FixedWingLandingComplexItem::loiterCoordinateChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(this, &FixedWingLandingComplexItem::landingCoordinateChanged, this, &FixedWingLandingComplexItem::_setDirty); connect(this, &FixedWingLandingComplexItem::loiterClockwiseChanged, this, &FixedWingLandingComplexItem::_setDirty); @@ -97,6 +99,13 @@ FixedWingLandingComplexItem::FixedWingLandingComplexItem(Vehicle* vehicle, bool connect(this, &FixedWingLandingComplexItem::altitudesAreRelativeChanged, this, &FixedWingLandingComplexItem::coordinateHasRelativeAltitudeChanged); connect(this, &FixedWingLandingComplexItem::altitudesAreRelativeChanged, this, &FixedWingLandingComplexItem::exitCoordinateHasRelativeAltitudeChanged); + + if (_valueSetIsDistanceFact.rawValue().toBool()) { + _recalcFromHeadingAndDistanceChange(); + } else { + _glideSlopeChanged(); + } + setDirty(false); } int FixedWingLandingComplexItem::lastSequenceNumber(void) const @@ -142,7 +151,7 @@ void FixedWingLandingComplexItem::save(QJsonArray& missionItems) saveObject[_jsonStopTakingVideoKey] = _stopTakingVideoFact.rawValue().toBool(); saveObject[_jsonLoiterClockwiseKey] = _loiterClockwise; saveObject[_jsonAltitudesAreRelativeKey] = _altitudesAreRelative; - saveObject[_jsonValueSetIsDistanceKey] = _valueSetIsDistance; + saveObject[_jsonValueSetIsDistanceKey] = _valueSetIsDistanceFact.rawValue().toBool(); missionItems.append(saveObject); } @@ -204,7 +213,7 @@ bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int seq } else { _altitudesAreRelative = loiterAltitudeRelative; } - _valueSetIsDistance = true; + _valueSetIsDistanceFact.setRawValue(true); } else if (version == 2) { QList v2KeyInfoList = { { _jsonAltitudesAreRelativeKey, QJsonValue::Bool, true }, @@ -216,7 +225,7 @@ bool FixedWingLandingComplexItem::load(const QJsonObject& complexObject, int seq } _altitudesAreRelative = complexObject[_jsonAltitudesAreRelativeKey].toBool(); - _valueSetIsDistance = complexObject[_jsonValueSetIsDistanceKey].toBool(); + _valueSetIsDistanceFact.setRawValue(complexObject[_jsonValueSetIsDistanceKey].toBool()); } else { errorString = tr("%1 complex item version %2 not supported").arg(jsonComplexItemTypeValue).arg(version); _ignoreRecalcSignals = false; diff --git a/src/MissionManager/FixedWingLandingComplexItem.h b/src/MissionManager/FixedWingLandingComplexItem.h index 0b0af9d804a24fbab4000cc5e43d272ab2862528..56732322b548d5bf350e70211f7084b4222df226 100644 --- a/src/MissionManager/FixedWingLandingComplexItem.h +++ b/src/MissionManager/FixedWingLandingComplexItem.h @@ -30,7 +30,7 @@ public: Q_PROPERTY(Fact* loiterRadius READ loiterRadius CONSTANT) Q_PROPERTY(Fact* landingAltitude READ landingAltitude CONSTANT) Q_PROPERTY(Fact* landingHeading READ landingHeading CONSTANT) - Q_PROPERTY(bool valueSetIsDistance MEMBER _valueSetIsDistance NOTIFY valueSetIsDistanceChanged) + Q_PROPERTY(Fact* valueSetIsDistance READ valueSetIsDistance CONSTANT) Q_PROPERTY(Fact* landingDistance READ landingDistance CONSTANT) Q_PROPERTY(Fact* glideSlope READ glideSlope CONSTANT) Q_PROPERTY(bool loiterClockwise MEMBER _loiterClockwise NOTIFY loiterClockwiseChanged) @@ -50,6 +50,7 @@ public: Fact* glideSlope (void) { return &_glideSlopeFact; } Fact* stopTakingPhotos (void) { return &_stopTakingPhotosFact; } Fact* stopTakingVideo (void) { return &_stopTakingVideoFact; } + Fact* valueSetIsDistance (void) { return &_valueSetIsDistanceFact; } QGeoCoordinate landingCoordinate (void) const { return _landingCoordinate; } QGeoCoordinate loiterCoordinate (void) const { return _loiterCoordinate; } QGeoCoordinate loiterTangentCoordinate (void) const { return _loiterTangentCoordinate; } @@ -110,6 +111,7 @@ public: static const char* glideSlopeName; static const char* stopTakingPhotosName; static const char* stopTakingVideoName; + static const char* valueSetIsDistanceName; signals: void loiterCoordinateChanged (QGeoCoordinate coordinate); @@ -154,10 +156,10 @@ private: Fact _glideSlopeFact; Fact _stopTakingPhotosFact; Fact _stopTakingVideoFact; + Fact _valueSetIsDistanceFact; bool _loiterClockwise; bool _altitudesAreRelative; - bool _valueSetIsDistance; static const char* settingsGroup; static const char* _jsonLoiterCoordinateKey; diff --git a/src/PlanView/FWLandingPatternEditor.qml b/src/PlanView/FWLandingPatternEditor.qml index fcd56385d3f0ec456b3953b12789a223eea1cd7c..2bfbd1e70830991878e371d14e02e15478a9ca8d 100644 --- a/src/PlanView/FWLandingPatternEditor.qml +++ b/src/PlanView/FWLandingPatternEditor.qml @@ -119,9 +119,9 @@ Rectangle { QGCRadioButton { id: specifyLandingDistance text: qsTr("Landing Dist") - checked: missionItem.valueSetIsDistance + checked: missionItem.valueSetIsDistance.rawValue exclusiveGroup: distanceGlideGroup - onClicked: missionItem.valueSetIsDistance = checked + onClicked: missionItem.valueSetIsDistance.rawValue = checked Layout.fillWidth: true } @@ -134,9 +134,9 @@ Rectangle { QGCRadioButton { id: specifyGlideSlope text: qsTr("Glide Slope") - checked: !missionItem.valueSetIsDistance + checked: !missionItem.valueSetIsDistance.rawValue exclusiveGroup: distanceGlideGroup - onClicked: missionItem.valueSetIsDistance = !checked + onClicked: missionItem.valueSetIsDistance.rawValue = !checked Layout.fillWidth: true } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 56a2762fc6705f9ed94a1cc9eb9b5658dd70fe0f..48705256514c3f00e36c9bd6de79459d6027dec6 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -783,7 +783,10 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes _handleEstimatorStatus(message); break; case MAVLINK_MSG_ID_STATUSTEXT: - _handleStatusText(message); + _handleStatusText(message, false /* longVersion */); + break; + case MAVLINK_MSG_ID_STATUSTEXT_LONG: + _handleStatusText(message, true /* longVersion */); break; case MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS: _handleOrbitExecutionStatus(message); @@ -881,15 +884,23 @@ void Vehicle::_handleCameraImageCaptured(const mavlink_message_t& message) } } -void Vehicle::_handleStatusText(mavlink_message_t& message) +void Vehicle::_handleStatusText(mavlink_message_t& message, bool longVersion) { - QByteArray b; + QByteArray b; + QString messageText; + int severity; - b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1); - mavlink_msg_statustext_get_text(&message, b.data()); + if (longVersion) { + b.resize(MAVLINK_MSG_STATUSTEXT_LONG_FIELD_TEXT_LEN+1); + mavlink_msg_statustext_long_get_text(&message, b.data()); + severity = mavlink_msg_statustext_long_get_severity(&message); + } else { + b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1); + mavlink_msg_statustext_get_text(&message, b.data()); + severity = mavlink_msg_statustext_get_severity(&message); + } b[b.length()-1] = '\0'; - QString messageText = QString(b); - int severity = mavlink_msg_statustext_get_severity(&message); + messageText = QString(b); bool skipSpoken = false; bool ardupilotPrearm = messageText.startsWith(QStringLiteral("PreArm")); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 7bb9072a0340612f06fbf07e6ad4cc195e424c59..cba6240ea911692c2e6e05377215bd9268b7e120 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -1255,7 +1255,7 @@ private: void _handleAttitudeTarget(mavlink_message_t& message); void _handleDistanceSensor(mavlink_message_t& message); void _handleEstimatorStatus(mavlink_message_t& message); - void _handleStatusText(mavlink_message_t& message); + void _handleStatusText(mavlink_message_t& message, bool longVersion); void _handleOrbitExecutionStatus(const mavlink_message_t& message); // ArduPilot dialect messages #if !defined(NO_ARDUPILOT_DIALECT) diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 9eee0deed51211b332c28917e1a420286ac012af..9edc2f30c3ed36b27b2783023e1283dc29053343 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -1065,6 +1065,14 @@ void MockLink::_sendStatusTextMessages(void) status->severity, status->msg); respondWithMavlinkMessage(msg); + + mavlink_msg_statustext_long_pack_chan(_vehicleSystemId, + _vehicleComponentId, + _mavlinkChannel, + &msg, + status->severity, + status->msg); + respondWithMavlinkMessage(msg); } } diff --git a/src/ui/MAVLinkDecoder.cc b/src/ui/MAVLinkDecoder.cc index 53381f8dd7e0cac3fc83bede9b592a924f05d51b..9c2152396c2aacff3de78ec053fcb6b5edd44952 100644 --- a/src/ui/MAVLinkDecoder.cc +++ b/src/ui/MAVLinkDecoder.cc @@ -15,6 +15,7 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol) : // messageFilter.insert(MAVLINK_MSG_ID_HEARTBEAT, false); // messageFilter.insert(MAVLINK_MSG_ID_SYS_STATUS, false); messageFilter.insert(MAVLINK_MSG_ID_STATUSTEXT, false); + messageFilter.insert(MAVLINK_MSG_ID_STATUSTEXT_LONG, false); messageFilter.insert(MAVLINK_MSG_ID_COMMAND_LONG, false); messageFilter.insert(MAVLINK_MSG_ID_COMMAND_ACK, false); messageFilter.insert(MAVLINK_MSG_ID_PARAM_SET, false);