From 0e3addac7c7bb1fe15d80d6066ac73b7243cb3ed Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Fri, 18 Jan 2019 10:53:42 -0800 Subject: [PATCH] Support STATUSTEXT_LONG --- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 68 ++++++++++++++------- src/FirmwarePlugin/APM/APMFirmwarePlugin.h | 6 +- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 2 +- src/Vehicle/Vehicle.cc | 25 +++++--- src/Vehicle/Vehicle.h | 2 +- src/comm/MockLink.cc | 8 +++ src/ui/MAVLinkDecoder.cc | 1 + 7 files changed, 78 insertions(+), 34 deletions(-) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index df059a4fe..68119e7ff 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 3abab25ee..ddf754ed5 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 44eb73ff1..9b5f3f568 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/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 56a2762fc..487052565 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 7bb9072a0..cba6240ea 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 9eee0deed..9edc2f30c 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 53381f8dd..9c2152396 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); -- 2.22.0