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

Add support for chunked STATUSTEXT

QGC4.4
DoinLakeFlyer 5 years ago
parent
e28a6c9c89
commit
8b18cf066f
6 changed files with 214 additions and 121 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. 42
      src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
  2. 6
      src/FirmwarePlugin/APM/APMFirmwarePlugin.h
  3. 111
      src/Vehicle/Vehicle.cc
  4. 13
      src/Vehicle/Vehicle.h
  5. 68
      src/comm/MockLink.cc
  6. 1
      src/comm/MockLink.h

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

@ -339,20 +339,15 @@ void APMFirmwarePlugin::_handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* @@ -339,20 +339,15 @@ void APMFirmwarePlugin::_handleOutgoingParamSet(Vehicle* vehicle, LinkInterface*
mavlink_msg_param_set_encode_chan(message->sysid, message->compid, outgoingLink->mavlinkChannel(), message, &paramSet);
}
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<APMFirmwarePluginInstanceData*>(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 @@ -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 @@ -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 @@ -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 @@ -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());
}
// Ensure NUL-termination
b[b.length()-1] = '\0';
@ -595,23 +583,12 @@ void APMFirmwarePlugin::_adjustSeverity(mavlink_message_t* message) const @@ -595,23 +583,12 @@ 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);
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);
@ -622,7 +599,6 @@ void APMFirmwarePlugin::_setInfoSeverity(mavlink_message_t* message, bool longVe @@ -622,7 +599,6 @@ void APMFirmwarePlugin::_setInfoSeverity(mavlink_message_t* message, bool longVe
message,
&statusText);
}
}
void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* message) const
{

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

@ -122,10 +122,10 @@ private: @@ -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);

111
src/Vehicle/Vehicle.cc
Unescape View File

@ -257,6 +257,11 @@ Vehicle::Vehicle(LinkInterface* link, @@ -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 @@ -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) @@ -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;
// Spit out all incomplete chunks
QList<uint8_t> rgCompId = _chunkedStatusTextInfoMap.keys();
for (uint8_t compId : rgCompId) {
_chunkedStatusTextInfoMap[compId].rgMessageChunks.append(QString());
_chunkedStatusTextCompleted(compId);
}
}
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;
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;
for (const QString& chunk : rgChunks) {
if (chunk.isEmpty()) {
// Indicates missing chunk
messageText += tr(" ... ", "Indicates missing chunk from chunked STATUS_TEXT");
} 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;
messageText += chunk;
}
b[b.length()-1] = '\0';
messageText = QString(b);
}
_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) @@ -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) @@ -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<statustext.chunk_seq; i++) {
chunks.append(QString());
}
}
chunks.append(messageText);
} else {
// Starting a new chunk sequence
ChunkedStatusTextInfo_t chunkedInfo;
chunkedInfo.chunkId = statustext.id;
chunkedInfo.severity = statustext.severity;
chunkedInfo.rgMessageChunks.append(messageText);
_chunkedStatusTextInfoMap[compId] = chunkedInfo;
}
_chunkedStatusTextTimer.start();
}
if (statustext.id == 0 || includesNullTerminator) {
_chunkedStatusTextTimer.stop();
_chunkedStatusTextCompleted(message.compid);
}
}
void Vehicle::_handleVfrHud(mavlink_message_t& message)

13
src/Vehicle/Vehicle.h
Unescape View File

@ -1314,7 +1314,7 @@ private: @@ -1314,7 +1314,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, bool longVersion);
void _handleStatusText (mavlink_message_t& message);
void _handleOrbitExecutionStatus (const mavlink_message_t& message);
void _handleMessageInterval (const mavlink_message_t& message);
void _handleGimbalOrientation (const mavlink_message_t& message);
@ -1351,6 +1351,8 @@ private: @@ -1351,6 +1351,8 @@ private:
void _flightTimerStart ();
void _flightTimerStop ();
void _batteryStatusWorker (int batteryId, double voltage, double current, double batteryRemainingPct);
void _chunkedStatusTextTimeout (void);
void _chunkedStatusTextCompleted (uint8_t compId);
int _id; ///< Mavlink system id
int _defaultComponentId;
@ -1550,6 +1552,15 @@ private: @@ -1550,6 +1552,15 @@ private:
QList<int> _pidTuningMessages;
QMap<int, int> _pidTuningMessageRatesUsecs;
// Chunked status text support
typedef struct {
uint16_t chunkId;
uint8_t severity;
QStringList rgMessageChunks;
} ChunkedStatusTextInfo_t;
QMap<uint8_t /* compId */, ChunkedStatusTextInfo_t> _chunkedStatusTextInfoMap;
QTimer _chunkedStatusTextTimer;
// FactGroup facts
Fact _rollFact;

68
src/comm/MockLink.cc
Unescape View File

@ -1059,16 +1059,55 @@ void MockLink::_sendGpsRawInt(void) @@ -1059,16 +1059,55 @@ void MockLink::_sendGpsRawInt(void)
UINT16_MAX, UINT16_MAX, // HDOP/VDOP not known
UINT16_MAX, // velocity not known
UINT16_MAX, // course over ground not known
8, // satellite count
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<cChunks; i++) {
if (missingChunks && (i & 1)) {
continue;
}
int cBuf = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
char msgBuf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
memset(msgBuf, 0, sizeof(msgBuf));
if (i == cChunks - 1) {
// Last chunk is partial
cBuf /= 2;
}
for (int j=0; j<cBuf-1; j++) {
msgBuf[j] = '0' + num++;
if (num > 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 {
@ -1088,8 +1127,9 @@ void MockLink::_sendStatusTextMessages(void) @@ -1088,8 +1127,9 @@ void MockLink::_sendStatusTextMessages(void)
{ MAV_SEVERITY_DEBUG, "Status text debug" },
};
for (size_t i=0; i<sizeof(rgMessages)/sizeof(rgMessages[0]); i++) {
mavlink_message_t msg;
for (size_t i=0; i<sizeof(rgMessages)/sizeof(rgMessages[0]); i++) {
const struct StatusMessage* status = &rgMessages[i];
mavlink_msg_statustext_pack_chan(_vehicleSystemId,
@ -1097,17 +1137,16 @@ void MockLink::_sendStatusTextMessages(void) @@ -1097,17 +1137,16 @@ void MockLink::_sendStatusTextMessages(void)
_mavlinkChannel,
&msg,
status->severity,
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 @@ -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);
}

1
src/comm/MockLink.h
Unescape View File

@ -199,6 +199,7 @@ private: @@ -199,6 +199,7 @@ private:
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);

Write Preview
Loading…
Cancel
Save