From 8b18cf066fcf16bf9d595812d74157a8fbb3c219 Mon Sep 17 00:00:00 2001 From: DoinLakeFlyer Date: Fri, 27 Mar 2020 13:02:27 -0700 Subject: [PATCH] Add support for chunked STATUSTEXT --- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 62 ++++------- src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 6 +- src/Vehicle/Vehicle.cc | 115 +++++++++++++++----- src/Vehicle/Vehicle.h | 13 ++- src/comm/MockLink.cc | 80 ++++++++++---- src/comm/MockLink.h | 59 +++++----- 6 files changed, 214 insertions(+), 121 deletions(-) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index df0d86f89..efe625f51 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -339,20 +339,15 @@ 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 longVersion) +bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message) { QString messageText; APMFirmwarePluginInstanceData* instanceData = qobject_cast(vehicle->firmwarePluginInstanceData()); - int severity; - if (longVersion) { - severity = mavlink_msg_statustext_long_get_severity(message); - } else { - severity = mavlink_msg_statustext_get_severity(message); - } + int severity = mavlink_msg_statustext_get_severity(message); if (vehicle->firmwareMajorVersion() == Vehicle::versionNotSetValue || severity < MAV_SEVERITY_NOTICE) { - messageText = _getMessageText(message, longVersion); + messageText = _getMessageText(message); qCDebug(APMFirmwarePluginLog) << messageText; if (!messageText.contains(APM_SOLO_REXP)) { @@ -423,14 +418,14 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess } if (messageText.isEmpty()) { - messageText = _getMessageText(message, longVersion); + 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, longVersion); + _setInfoSeverity(message); } if (messageText.contains(APM_SOLO_REXP)) { @@ -438,7 +433,7 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess vehicle->setSoloFirmware(true); // Fix up severity - _setInfoSeverity(message, longVersion); + _setInfoSeverity(message); // Start TCP video handshake with ARTOO _soloVideoHandshake(vehicle, true /* originalSoloFirmware */); @@ -499,9 +494,7 @@ bool APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_m _handleIncomingParamValue(vehicle, message); break; case MAVLINK_MSG_ID_STATUSTEXT: - return _handleIncomingStatusText(vehicle, message, false /* longVersion */); - case MAVLINK_MSG_ID_STATUSTEXT_LONG: - return _handleIncomingStatusText(vehicle, message, true /* longVersion */); + return _handleIncomingStatusText(vehicle, message); case MAVLINK_MSG_ID_RC_CHANNELS: _handleRCChannels(vehicle, message); break; @@ -523,17 +516,12 @@ void APMFirmwarePlugin::adjustOutgoingMavlinkMessage(Vehicle* vehicle, LinkInter } } -QString APMFirmwarePlugin::_getMessageText(mavlink_message_t* message, bool longVersion) const +QString APMFirmwarePlugin::_getMessageText(mavlink_message_t* message) const { QByteArray b; - 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()); - } + 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'; @@ -595,33 +583,21 @@ void APMFirmwarePlugin::_adjustSeverity(mavlink_message_t* message) const &statusText); } -void APMFirmwarePlugin::_setInfoSeverity(mavlink_message_t* message, bool longVersion) const +void APMFirmwarePlugin::_setInfoSeverity(mavlink_message_t* message) const { // 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; - if (longVersion) { - mavlink_statustext_long_t statusTextLong; - mavlink_msg_statustext_long_decode(message, &statusTextLong); + mavlink_statustext_t statusText; + mavlink_msg_statustext_decode(message, &statusText); - 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); - } + 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 826ea6e13..6266067f0 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.h @@ -122,10 +122,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, bool longVersion) const; - QString _getMessageText(mavlink_message_t* message, bool longVersion) const; + 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, bool longVersion); + bool _handleIncomingStatusText(Vehicle* vehicle, mavlink_message_t* message); 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/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 75708ec4b..650c72b08 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -257,6 +257,11 @@ Vehicle::Vehicle(LinkInterface* link, _mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs); connect(&_mavCommandAckTimer, &QTimer::timeout, this, &Vehicle::_sendMavCommandAgain); + // Chunked status text timeout timer + _chunkedStatusTextTimer.setSingleShot(true); + _chunkedStatusTextTimer.setInterval(1000); + connect(&_chunkedStatusTextTimer, &QTimer::timeout, this, &Vehicle::_chunkedStatusTextTimeout); + _mav = uas(); // Listen for system messages @@ -843,10 +848,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes _handleEstimatorStatus(message); break; case MAVLINK_MSG_ID_STATUSTEXT: - _handleStatusText(message, false /* longVersion */); - break; - case MAVLINK_MSG_ID_STATUSTEXT_LONG: - _handleStatusText(message, true /* longVersion */); + _handleStatusText(message); break; case MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS: _handleOrbitExecutionStatus(message); @@ -952,27 +954,34 @@ void Vehicle::_handleCameraImageCaptured(const mavlink_message_t& message) } } -void Vehicle::_handleStatusText(mavlink_message_t& message, bool longVersion) +void Vehicle::_chunkedStatusTextTimeout(void) { - QByteArray b; - QString messageText; - int severity; - - if (longVersion) { - b.resize(MAVLINK_MSG_STATUSTEXT_LONG_FIELD_TEXT_LEN+1); - mavlink_statustext_long_t statustextLong; - mavlink_msg_statustext_long_decode(&message, &statustextLong); - strncpy(b.data(), statustextLong.text, MAVLINK_MSG_STATUSTEXT_LONG_FIELD_TEXT_LEN); - severity = statustextLong.severity; - } else { - b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1); - mavlink_statustext_t statustext; - mavlink_msg_statustext_decode(&message, &statustext); - strncpy(b.data(), statustext.text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); - severity = statustext.severity; + // Spit out all incomplete chunks + QList rgCompId = _chunkedStatusTextInfoMap.keys(); + for (uint8_t compId : rgCompId) { + _chunkedStatusTextInfoMap[compId].rgMessageChunks.append(QString()); + _chunkedStatusTextCompleted(compId); } - b[b.length()-1] = '\0'; - messageText = QString(b); +} + +void Vehicle::_chunkedStatusTextCompleted(uint8_t compId) +{ + ChunkedStatusTextInfo_t& chunkedInfo = _chunkedStatusTextInfoMap[compId]; + uint8_t severity = chunkedInfo.severity; + QStringList& rgChunks = chunkedInfo.rgMessageChunks; + + // Build up message from chunks + QString messageText; + for (const QString& chunk : rgChunks) { + if (chunk.isEmpty()) { + // Indicates missing chunk + messageText += tr(" ... ", "Indicates missing chunk from chunked STATUS_TEXT"); + } else { + messageText += chunk; + } + } + + _chunkedStatusTextInfoMap.remove(compId); bool skipSpoken = false; bool ardupilotPrearm = messageText.startsWith(QStringLiteral("PreArm")); @@ -987,7 +996,6 @@ void Vehicle::_handleStatusText(mavlink_message_t& message, bool longVersion) } } - // If the message is NOTIFY or higher severity, or starts with a '#', // then read it aloud. if (messageText.startsWith("#") || severity <= MAV_SEVERITY_NOTICE) { @@ -996,7 +1004,64 @@ void Vehicle::_handleStatusText(mavlink_message_t& message, bool longVersion) qgcApp()->toolbox()->audioOutput()->say(messageText); } } - emit textMessageReceived(id(), message.compid, severity, messageText); + emit textMessageReceived(id(), compId, severity, messageText); +} + +void Vehicle::_handleStatusText(mavlink_message_t& message) +{ + QByteArray b; + QString messageText; + + mavlink_statustext_t statustext; + mavlink_msg_statustext_decode(&message, &statustext); + + uint8_t compId = message.compid; + + b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1); + strncpy(b.data(), statustext.text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); + b[b.length()-1] = '\0'; + messageText = QString(b); + bool includesNullTerminator = messageText.length() < MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; + + if (_chunkedStatusTextInfoMap.contains(compId) && _chunkedStatusTextInfoMap[compId].chunkId != statustext.id) { + // We have an incomplete chunked status still pending + _chunkedStatusTextInfoMap[compId].rgMessageChunks.append(QString()); + _chunkedStatusTextCompleted(compId); + } + + if (statustext.id == 0) { + // Non-chunked status text. We still use common chunked text output mechanism. + ChunkedStatusTextInfo_t chunkedInfo; + chunkedInfo.chunkId = 0; + chunkedInfo.severity = statustext.severity; + chunkedInfo.rgMessageChunks.append(messageText); + _chunkedStatusTextInfoMap[compId] = chunkedInfo; + } else { + if (_chunkedStatusTextInfoMap.contains(compId)) { + // A chunk sequence is in progress + QStringList& chunks = _chunkedStatusTextInfoMap[compId].rgMessageChunks; + if (statustext.chunk_seq > chunks.size()) { + // We are missing some chunks in between, fill them in as missing + for (int i=chunks.size(); i _pidTuningMessages; QMap _pidTuningMessageRatesUsecs; + // Chunked status text support + typedef struct { + uint16_t chunkId; + uint8_t severity; + QStringList rgMessageChunks; + } ChunkedStatusTextInfo_t; + QMap _chunkedStatusTextInfoMap; + QTimer _chunkedStatusTextTimer; + // FactGroup facts Fact _rollFact; diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 5dc76cf76..f29612675 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -1051,24 +1051,63 @@ void MockLink::_sendGpsRawInt(void) _vehicleComponentId, _mavlinkChannel, &msg, - timeTick++, // time since boot - 3, // 3D fix + timeTick++, // time since boot + 3, // 3D fix (int32_t)(_vehicleLatitude * 1E7), (int32_t)(_vehicleLongitude * 1E7), (int32_t)(_vehicleAltitude * 1000), - UINT16_MAX, UINT16_MAX, // HDOP/VDOP not known - UINT16_MAX, // velocity not known - UINT16_MAX, // course over ground not known - 8, // satellite count + UINT16_MAX, UINT16_MAX, // HDOP/VDOP not known + UINT16_MAX, // velocity not known + UINT16_MAX, // course over ground not known + 8, // satellites visible //-- Extension 0, // Altitude (above WGS84, EGM96 ellipsoid), in meters * 1000 (positive for up). 0, // Position uncertainty in meters * 1000 (positive for up). 0, // Altitude uncertainty in meters * 1000 (positive for up). 0, // Speed uncertainty in meters * 1000 (positive for up). - 0); // Heading / track uncertainty in degrees * 1e5. + 0, // Heading / track uncertainty in degrees * 1e5. + 65535); // Yaw not provided respondWithMavlinkMessage(msg); } +void MockLink::_sendChunkedStatusText(uint16_t chunkId, bool missingChunks) +{ + mavlink_message_t msg; + + int cChunks = 4; + int num = 0; + for (int i=0; i 9) { + num = 0; + } + } + msgBuf[cBuf-1] = 'A' + i; + + mavlink_msg_statustext_pack_chan(_vehicleSystemId, + _vehicleComponentId, + _mavlinkChannel, + &msg, + MAV_SEVERITY_INFO, + msgBuf, + chunkId, + i); // chunk sequence number + respondWithMavlinkMessage(msg); + } + +} + void MockLink::_sendStatusTextMessages(void) { struct StatusMessage { @@ -1086,10 +1125,11 @@ void MockLink::_sendStatusTextMessages(void) { MAV_SEVERITY_NOTICE, "Status text notice" }, { MAV_SEVERITY_INFO, "Status text info" }, { MAV_SEVERITY_DEBUG, "Status text debug" }, -}; + }; + + mavlink_message_t msg; for (size_t i=0; iseverity, - status->msg); - respondWithMavlinkMessage(msg); - - mavlink_msg_statustext_long_pack_chan(_vehicleSystemId, - _vehicleComponentId, - _mavlinkChannel, - &msg, - status->severity, - status->msg); - respondWithMavlinkMessage(msg); + status->msg, + 0, // Not a chunked sequence + 0); // Not a chunked sequence + //respondWithMavlinkMessage(msg); } + + _sendChunkedStatusText(1, false /* missingChunks */); + _sendChunkedStatusText(2, true /* missingChunks */); + _sendChunkedStatusText(3, false /* missingChunks */); // This should cause the previous incomplete chunk to spit out + _sendChunkedStatusText(4, true /* missingChunks */); // This should cause the timeout to fire } MockConfiguration::MockConfiguration(const QString& name) @@ -1280,7 +1319,8 @@ void MockLink::_handlePreFlightCalibration(const mavlink_command_long_t& request _mavlinkChannel, &msg, MAV_SEVERITY_INFO, - pCalMessage); + pCalMessage, + 0, 0); // Not chunked respondWithMavlinkMessage(msg); } diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index c02d5ea99..5b8a32547 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -176,35 +176,36 @@ private: virtual void run(void); // MockLink methods - void _sendHeartBeat(void); - void _sendHighLatency2(void); - void _handleIncomingNSHBytes(const char* bytes, int cBytes); - void _handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes); - void _loadParams(void); - void _handleHeartBeat(const mavlink_message_t& msg); - void _handleSetMode(const mavlink_message_t& msg); - void _handleParamRequestList(const mavlink_message_t& msg); - void _handleParamSet(const mavlink_message_t& msg); - void _handleParamRequestRead(const mavlink_message_t& msg); - void _handleFTP(const mavlink_message_t& msg); - void _handleCommandLong(const mavlink_message_t& msg); - void _handleManualControl(const mavlink_message_t& msg); - void _handlePreFlightCalibration(const mavlink_command_long_t& request); - void _handleLogRequestList(const mavlink_message_t& msg); - void _handleLogRequestData(const mavlink_message_t& msg); - float _floatUnionForParam(int componentId, const QString& paramName); - void _setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat); - void _sendHomePosition(void); - void _sendGpsRawInt(void); - void _sendVibration(void); - void _sendSysStatus(void); - void _sendStatusTextMessages(void); - void _respondWithAutopilotVersion(void); - void _sendRCChannels(void); - void _paramRequestListWorker(void); - void _logDownloadWorker(void); - void _sendADSBVehicles(void); - void _moveADSBVehicle(void); + void _sendHeartBeat (void); + void _sendHighLatency2 (void); + void _handleIncomingNSHBytes (const char* bytes, int cBytes); + void _handleIncomingMavlinkBytes (const uint8_t* bytes, int cBytes); + void _loadParams (void); + void _handleHeartBeat (const mavlink_message_t& msg); + void _handleSetMode (const mavlink_message_t& msg); + void _handleParamRequestList (const mavlink_message_t& msg); + void _handleParamSet (const mavlink_message_t& msg); + void _handleParamRequestRead (const mavlink_message_t& msg); + void _handleFTP (const mavlink_message_t& msg); + void _handleCommandLong (const mavlink_message_t& msg); + void _handleManualControl (const mavlink_message_t& msg); + void _handlePreFlightCalibration (const mavlink_command_long_t& request); + void _handleLogRequestList (const mavlink_message_t& msg); + void _handleLogRequestData (const mavlink_message_t& msg); + float _floatUnionForParam (int componentId, const QString& paramName); + void _setParamFloatUnionIntoMap (int componentId, const QString& paramName, float paramFloat); + void _sendHomePosition (void); + void _sendGpsRawInt (void); + void _sendVibration (void); + void _sendSysStatus (void); + void _sendStatusTextMessages (void); + void _sendChunkedStatusText (uint16_t chunkId, bool missingChunks); + void _respondWithAutopilotVersion (void); + void _sendRCChannels (void); + void _paramRequestListWorker (void); + void _logDownloadWorker (void); + void _sendADSBVehicles (void); + void _moveADSBVehicle (void); static MockLink* _startMockLinkWorker(QString configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode); static MockLink* _startMockLink(MockConfiguration* mockConfig); -- 2.22.0