Skip to content

Q
qgroundcontrol
Commit 8b18cf06 authored by DoinLakeFlyer's avatar DoinLakeFlyer
Browse files
Options

parent e28a6c9c
Hide whitespace changes
Inline Side-by-side
Showing with 214 additions and 121 deletions
src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
View file @ 8b18cf06
......@@ -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
}
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
......
src/FirmwarePlugin/APM/APMFirmwarePlugin.h
View file @ 8b18cf06
......@@ -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);
......
src/Vehicle/Vehicle.cc
View file @ 8b18cf06
......@@ -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<uint8_t> 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<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)
......
src/Vehicle/Vehicle.h
View file @ 8b18cf06
......@@ -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:
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:
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;
......
src/comm/MockLink.cc
View file @ 8b18cf06
......@@ -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<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 {
......@@ -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; i<sizeof(rgMessages)/sizeof(rgMessages[0]); i++) {
mavlink_message_t msg;
const struct StatusMessage* status = &rgMessages[i];
mavlink_msg_statustext_pack_chan(_vehicleSystemId,
......@@ -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
_mavlinkChannel,
&msg,
MAV_SEVERITY_INFO,
pCalMessage);
pCalMessage,
0, 0); // Not chunked
respondWithMavlinkMessage(msg);
}
......
src/comm/MockLink.h
View file @ 8b18cf06
......@@ -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);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment