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
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 62
      src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
  2. 6
      src/FirmwarePlugin/APM/APMFirmwarePlugin.h
  3. 115
      src/Vehicle/Vehicle.cc
  4. 13
      src/Vehicle/Vehicle.h
  5. 80
      src/comm/MockLink.cc
  6. 59
      src/comm/MockLink.h

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

@ -339,20 +339,15 @@ void APMFirmwarePlugin::_handleOutgoingParamSet(Vehicle* vehicle, LinkInterface*
mavlink_msg_param_set_encode_chan(message->sysid, message->compid, outgoingLink->mavlinkChannel(), message, &paramSet); 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; QString messageText;
APMFirmwarePluginInstanceData* instanceData = qobject_cast<APMFirmwarePluginInstanceData*>(vehicle->firmwarePluginInstanceData()); APMFirmwarePluginInstanceData* instanceData = qobject_cast<APMFirmwarePluginInstanceData*>(vehicle->firmwarePluginInstanceData());
int severity; int severity = mavlink_msg_statustext_get_severity(message);
if (longVersion) {
severity = mavlink_msg_statustext_long_get_severity(message);
} else {
severity = mavlink_msg_statustext_get_severity(message);
}
if (vehicle->firmwareMajorVersion() == Vehicle::versionNotSetValue || severity < MAV_SEVERITY_NOTICE) { if (vehicle->firmwareMajorVersion() == Vehicle::versionNotSetValue || severity < MAV_SEVERITY_NOTICE) {
messageText = _getMessageText(message, longVersion); messageText = _getMessageText(message);
qCDebug(APMFirmwarePluginLog) << messageText; qCDebug(APMFirmwarePluginLog) << messageText;
if (!messageText.contains(APM_SOLO_REXP)) { if (!messageText.contains(APM_SOLO_REXP)) {
@ -423,14 +418,14 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess
} }
if (messageText.isEmpty()) { if (messageText.isEmpty()) {
messageText = _getMessageText(message, longVersion); messageText = _getMessageText(message);
} }
// The following messages are incorrectly labeled as warning message. // The following messages are incorrectly labeled as warning message.
// Fixed in newer firmware (unreleased at this point), but still in older firmware. // 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) || 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)) { 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)) { if (messageText.contains(APM_SOLO_REXP)) {
@ -438,7 +433,7 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess
vehicle->setSoloFirmware(true); vehicle->setSoloFirmware(true);
// Fix up severity // Fix up severity
_setInfoSeverity(message, longVersion); _setInfoSeverity(message);
// Start TCP video handshake with ARTOO // Start TCP video handshake with ARTOO
_soloVideoHandshake(vehicle, true /* originalSoloFirmware */); _soloVideoHandshake(vehicle, true /* originalSoloFirmware */);
@ -499,9 +494,7 @@ bool APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_m
_handleIncomingParamValue(vehicle, message); _handleIncomingParamValue(vehicle, message);
break; break;
case MAVLINK_MSG_ID_STATUSTEXT: case MAVLINK_MSG_ID_STATUSTEXT:
return _handleIncomingStatusText(vehicle, message, false /* longVersion */); return _handleIncomingStatusText(vehicle, message);
case MAVLINK_MSG_ID_STATUSTEXT_LONG:
return _handleIncomingStatusText(vehicle, message, true /* longVersion */);
case MAVLINK_MSG_ID_RC_CHANNELS: case MAVLINK_MSG_ID_RC_CHANNELS:
_handleRCChannels(vehicle, message); _handleRCChannels(vehicle, message);
break; 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; QByteArray b;
if (longVersion) { b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
b.resize(MAVLINK_MSG_STATUSTEXT_LONG_FIELD_TEXT_LEN+1); mavlink_msg_statustext_get_text(message, b.data());
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 // Ensure NUL-termination
b[b.length()-1] = '\0'; b[b.length()-1] = '\0';
@ -595,33 +583,21 @@ void APMFirmwarePlugin::_adjustSeverity(mavlink_message_t* message) const
&statusText); &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 // Re-Encoding is always done using mavlink 1.0
mavlink_status_t* mavlinkStatusReEncode = mavlink_get_channel_status(0); mavlink_status_t* mavlinkStatusReEncode = mavlink_get_channel_status(0);
mavlinkStatusReEncode->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1; mavlinkStatusReEncode->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1;
if (longVersion) { mavlink_statustext_t statusText;
mavlink_statustext_long_t statusTextLong; mavlink_msg_statustext_decode(message, &statusText);
mavlink_msg_statustext_long_decode(message, &statusTextLong);
statusTextLong.severity = MAV_SEVERITY_INFO; statusText.severity = MAV_SEVERITY_INFO;
mavlink_msg_statustext_long_encode_chan(message->sysid, mavlink_msg_statustext_encode_chan(message->sysid,
message->compid, message->compid,
0, // Re-encoding uses reserved channel 0 0, // Re-encoding uses reserved channel 0
message, message,
&statusTextLong); &statusText);
} 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 void APMFirmwarePlugin::_adjustCalibrationMessageSeverity(mavlink_message_t* message) const

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

@ -122,10 +122,10 @@ private:
void _adjustSeverity(mavlink_message_t* message) const; void _adjustSeverity(mavlink_message_t* message) const;
void _adjustCalibrationMessageSeverity(mavlink_message_t* message) const; void _adjustCalibrationMessageSeverity(mavlink_message_t* message) const;
static bool _isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion); static bool _isTextSeverityAdjustmentNeeded(const APMFirmwareVersion& firmwareVersion);
void _setInfoSeverity(mavlink_message_t* message, bool longVersion) const; void _setInfoSeverity(mavlink_message_t* message) const;
QString _getMessageText(mavlink_message_t* message, bool longVersion) const; QString _getMessageText(mavlink_message_t* message) const;
void _handleIncomingParamValue(Vehicle* vehicle, mavlink_message_t* message); 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 _handleIncomingHeartbeat(Vehicle* vehicle, mavlink_message_t* message);
void _handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message); void _handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message);
void _soloVideoHandshake(Vehicle* vehicle, bool originalSoloFirmware); void _soloVideoHandshake(Vehicle* vehicle, bool originalSoloFirmware);

115
src/Vehicle/Vehicle.cc
Unescape View File

@ -257,6 +257,11 @@ Vehicle::Vehicle(LinkInterface* link,
_mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs); _mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs);
connect(&_mavCommandAckTimer, &QTimer::timeout, this, &Vehicle::_sendMavCommandAgain); 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(); _mav = uas();
// Listen for system messages // Listen for system messages
@ -843,10 +848,7 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
_handleEstimatorStatus(message); _handleEstimatorStatus(message);
break; break;
case MAVLINK_MSG_ID_STATUSTEXT: case MAVLINK_MSG_ID_STATUSTEXT:
_handleStatusText(message, false /* longVersion */); _handleStatusText(message);
break;
case MAVLINK_MSG_ID_STATUSTEXT_LONG:
_handleStatusText(message, true /* longVersion */);
break; break;
case MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS: case MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS:
_handleOrbitExecutionStatus(message); _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; // Spit out all incomplete chunks
QString messageText; QList<uint8_t> rgCompId = _chunkedStatusTextInfoMap.keys();
int severity; for (uint8_t compId : rgCompId) {
_chunkedStatusTextInfoMap[compId].rgMessageChunks.append(QString());
if (longVersion) { _chunkedStatusTextCompleted(compId);
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;
} }
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 skipSpoken = false;
bool ardupilotPrearm = messageText.startsWith(QStringLiteral("PreArm")); 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 '#', // If the message is NOTIFY or higher severity, or starts with a '#',
// then read it aloud. // then read it aloud.
if (messageText.startsWith("#") || severity <= MAV_SEVERITY_NOTICE) { 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); 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) void Vehicle::_handleVfrHud(mavlink_message_t& message)

13
src/Vehicle/Vehicle.h
Unescape View File

@ -1314,7 +1314,7 @@ private:
void _handleAttitudeTarget (mavlink_message_t& message); void _handleAttitudeTarget (mavlink_message_t& message);
void _handleDistanceSensor (mavlink_message_t& message); void _handleDistanceSensor (mavlink_message_t& message);
void _handleEstimatorStatus (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 _handleOrbitExecutionStatus (const mavlink_message_t& message);
void _handleMessageInterval (const mavlink_message_t& message); void _handleMessageInterval (const mavlink_message_t& message);
void _handleGimbalOrientation (const mavlink_message_t& message); void _handleGimbalOrientation (const mavlink_message_t& message);
@ -1351,6 +1351,8 @@ private:
void _flightTimerStart (); void _flightTimerStart ();
void _flightTimerStop (); void _flightTimerStop ();
void _batteryStatusWorker (int batteryId, double voltage, double current, double batteryRemainingPct); void _batteryStatusWorker (int batteryId, double voltage, double current, double batteryRemainingPct);
void _chunkedStatusTextTimeout (void);
void _chunkedStatusTextCompleted (uint8_t compId);
int _id; ///< Mavlink system id int _id; ///< Mavlink system id
int _defaultComponentId; int _defaultComponentId;
@ -1550,6 +1552,15 @@ private:
QList<int> _pidTuningMessages; QList<int> _pidTuningMessages;
QMap<int, int> _pidTuningMessageRatesUsecs; 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 // FactGroup facts
Fact _rollFact; Fact _rollFact;

80
src/comm/MockLink.cc
Unescape View File

@ -1051,24 +1051,63 @@ void MockLink::_sendGpsRawInt(void)
_vehicleComponentId, _vehicleComponentId,
_mavlinkChannel, _mavlinkChannel,
&msg, &msg,
timeTick++, // time since boot timeTick++, // time since boot
3, // 3D fix 3, // 3D fix
(int32_t)(_vehicleLatitude * 1E7), (int32_t)(_vehicleLatitude * 1E7),
(int32_t)(_vehicleLongitude * 1E7), (int32_t)(_vehicleLongitude * 1E7),
(int32_t)(_vehicleAltitude * 1000), (int32_t)(_vehicleAltitude * 1000),
UINT16_MAX, UINT16_MAX, // HDOP/VDOP not known UINT16_MAX, UINT16_MAX, // HDOP/VDOP not known
UINT16_MAX, // velocity not known UINT16_MAX, // velocity not known
UINT16_MAX, // course over ground not known UINT16_MAX, // course over ground not known
8, // satellite count 8, // satellites visible
//-- Extension //-- Extension
0, // Altitude (above WGS84, EGM96 ellipsoid), in meters * 1000 (positive for up). 0, // Altitude (above WGS84, EGM96 ellipsoid), in meters * 1000 (positive for up).
0, // Position uncertainty 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, // Altitude uncertainty in meters * 1000 (positive for up).
0, // Speed 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); 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) void MockLink::_sendStatusTextMessages(void)
{ {
struct StatusMessage { struct StatusMessage {
@ -1086,10 +1125,11 @@ void MockLink::_sendStatusTextMessages(void)
{ MAV_SEVERITY_NOTICE, "Status text notice" }, { MAV_SEVERITY_NOTICE, "Status text notice" },
{ MAV_SEVERITY_INFO, "Status text info" }, { MAV_SEVERITY_INFO, "Status text info" },
{ MAV_SEVERITY_DEBUG, "Status text debug" }, { MAV_SEVERITY_DEBUG, "Status text debug" },
}; };
mavlink_message_t msg;
for (size_t i=0; i<sizeof(rgMessages)/sizeof(rgMessages[0]); i++) { for (size_t i=0; i<sizeof(rgMessages)/sizeof(rgMessages[0]); i++) {
mavlink_message_t msg;
const struct StatusMessage* status = &rgMessages[i]; const struct StatusMessage* status = &rgMessages[i];
mavlink_msg_statustext_pack_chan(_vehicleSystemId, mavlink_msg_statustext_pack_chan(_vehicleSystemId,
@ -1097,17 +1137,16 @@ void MockLink::_sendStatusTextMessages(void)
_mavlinkChannel, _mavlinkChannel,
&msg, &msg,
status->severity, status->severity,
status->msg); status->msg,
respondWithMavlinkMessage(msg); 0, // Not a chunked sequence
0); // Not a chunked sequence
mavlink_msg_statustext_long_pack_chan(_vehicleSystemId, //respondWithMavlinkMessage(msg);
_vehicleComponentId,
_mavlinkChannel,
&msg,
status->severity,
status->msg);
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) MockConfiguration::MockConfiguration(const QString& name)
@ -1280,7 +1319,8 @@ void MockLink::_handlePreFlightCalibration(const mavlink_command_long_t& request
_mavlinkChannel, _mavlinkChannel,
&msg, &msg,
MAV_SEVERITY_INFO, MAV_SEVERITY_INFO,
pCalMessage); pCalMessage,
0, 0); // Not chunked
respondWithMavlinkMessage(msg); respondWithMavlinkMessage(msg);
} }

59
src/comm/MockLink.h
Unescape View File

@ -176,35 +176,36 @@ private:
virtual void run(void); virtual void run(void);
// MockLink methods // MockLink methods
void _sendHeartBeat(void); void _sendHeartBeat (void);
void _sendHighLatency2(void); void _sendHighLatency2 (void);
void _handleIncomingNSHBytes(const char* bytes, int cBytes); void _handleIncomingNSHBytes (const char* bytes, int cBytes);
void _handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes); void _handleIncomingMavlinkBytes (const uint8_t* bytes, int cBytes);
void _loadParams(void); void _loadParams (void);
void _handleHeartBeat(const mavlink_message_t& msg); void _handleHeartBeat (const mavlink_message_t& msg);
void _handleSetMode(const mavlink_message_t& msg); void _handleSetMode (const mavlink_message_t& msg);
void _handleParamRequestList(const mavlink_message_t& msg); void _handleParamRequestList (const mavlink_message_t& msg);
void _handleParamSet(const mavlink_message_t& msg); void _handleParamSet (const mavlink_message_t& msg);
void _handleParamRequestRead(const mavlink_message_t& msg); void _handleParamRequestRead (const mavlink_message_t& msg);
void _handleFTP(const mavlink_message_t& msg); void _handleFTP (const mavlink_message_t& msg);
void _handleCommandLong(const mavlink_message_t& msg); void _handleCommandLong (const mavlink_message_t& msg);
void _handleManualControl(const mavlink_message_t& msg); void _handleManualControl (const mavlink_message_t& msg);
void _handlePreFlightCalibration(const mavlink_command_long_t& request); void _handlePreFlightCalibration (const mavlink_command_long_t& request);
void _handleLogRequestList(const mavlink_message_t& msg); void _handleLogRequestList (const mavlink_message_t& msg);
void _handleLogRequestData(const mavlink_message_t& msg); void _handleLogRequestData (const mavlink_message_t& msg);
float _floatUnionForParam(int componentId, const QString& paramName); float _floatUnionForParam (int componentId, const QString& paramName);
void _setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat); void _setParamFloatUnionIntoMap (int componentId, const QString& paramName, float paramFloat);
void _sendHomePosition(void); void _sendHomePosition (void);
void _sendGpsRawInt(void); void _sendGpsRawInt (void);
void _sendVibration(void); void _sendVibration (void);
void _sendSysStatus(void); void _sendSysStatus (void);
void _sendStatusTextMessages(void); void _sendStatusTextMessages (void);
void _respondWithAutopilotVersion(void); void _sendChunkedStatusText (uint16_t chunkId, bool missingChunks);
void _sendRCChannels(void); void _respondWithAutopilotVersion (void);
void _paramRequestListWorker(void); void _sendRCChannels (void);
void _logDownloadWorker(void); void _paramRequestListWorker (void);
void _sendADSBVehicles(void); void _logDownloadWorker (void);
void _moveADSBVehicle(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* _startMockLinkWorker(QString configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode);
static MockLink* _startMockLink(MockConfiguration* mockConfig); static MockLink* _startMockLink(MockConfiguration* mockConfig);

Write Preview
Loading…
Cancel
Save