Commit 012de5a5 authored by Gus Grubba's avatar Gus Grubba

Merge branch 'master' of https://github.com/mavlink/qgroundcontrol into Airmap

# Conflicts:
#	src/Vehicle/Vehicle.cc
parents a1f8de63 15f188a0
...@@ -138,7 +138,7 @@ void ...@@ -138,7 +138,7 @@ void
AirMapFlightPlanManager::setFlightStartTime(QDateTime start) AirMapFlightPlanManager::setFlightStartTime(QDateTime start)
{ {
if(start < QDateTime::currentDateTime()) { if(start < QDateTime::currentDateTime()) {
start = QDateTime::currentDateTime().addSecs(60); start = QDateTime::currentDateTime().addSecs(1);
} }
if(_flightStartTime != start) { if(_flightStartTime != start) {
_flightStartTime = start; _flightStartTime = start;
...@@ -478,14 +478,8 @@ AirMapFlightPlanManager::_updateRulesAndFeatures(std::vector<RuleSet::Id>& rules ...@@ -478,14 +478,8 @@ AirMapFlightPlanManager::_updateRulesAndFeatures(std::vector<RuleSet::Id>& rules
void void
AirMapFlightPlanManager::_updateFlightStartEndTime(DateTime& start_time, DateTime& end_time) AirMapFlightPlanManager::_updateFlightStartEndTime(DateTime& start_time, DateTime& end_time)
{ {
if(_flightStartsNow) { if(_flightStartsNow || _flightStartTime < QDateTime::currentDateTime()) {
setFlightStartTime(QDateTime::currentDateTime().addSecs(60)); setFlightStartTime(QDateTime::currentDateTime().addSecs(1));
} else {
if(_flightStartTime < QDateTime::currentDateTime()) {
//-- Can't start in the past
_flightStartTime = QDateTime::currentDateTime();
emit flightStartTimeChanged();
}
} }
quint64 startt = static_cast<quint64>(_flightStartTime.toUTC().toMSecsSinceEpoch()); quint64 startt = static_cast<quint64>(_flightStartTime.toUTC().toMSecsSinceEpoch());
start_time = airmap::from_milliseconds_since_epoch(airmap::milliseconds(static_cast<qint64>(startt))); start_time = airmap::from_milliseconds_since_epoch(airmap::milliseconds(static_cast<qint64>(startt)));
......
...@@ -208,7 +208,8 @@ Vehicle::Vehicle(LinkInterface* link, ...@@ -208,7 +208,8 @@ Vehicle::Vehicle(LinkInterface* link,
_mavlink = _toolbox->mavlinkProtocol(); _mavlink = _toolbox->mavlinkProtocol();
connect(_mavlink, &MAVLinkProtocol::messageReceived, this, &Vehicle::_mavlinkMessageReceived); connect(_mavlink, &MAVLinkProtocol::messageReceived, this, &Vehicle::_mavlinkMessageReceived);
connect(_mavlink, &MAVLinkProtocol::mavlinkMessageStatus, this, &Vehicle::_mavlinkMessageStatus);
_addLink(link); _addLink(link);
...@@ -1845,7 +1846,7 @@ void Vehicle::_updatePriorityLink(bool updateActive, bool sendCommand) ...@@ -1845,7 +1846,7 @@ void Vehicle::_updatePriorityLink(bool updateActive, bool sendCommand)
// This routine specifically does not clear _priorityLink when there are no links remaining. // This routine specifically does not clear _priorityLink when there are no links remaining.
// By doing this we hold a reference on the last link as the Vehicle shuts down. Thus preventing shutdown // By doing this we hold a reference on the last link as the Vehicle shuts down. Thus preventing shutdown
// ordering NULL pointer crashes where priorityLink() is still called during shutdown sequence. // ordering nullptr pointer crashes where priorityLink() is still called during shutdown sequence.
if (_links.count() == 0) { if (_links.count() == 0) {
return; return;
} }
...@@ -3652,6 +3653,17 @@ void Vehicle::_adsbTimerTimeout() ...@@ -3652,6 +3653,17 @@ void Vehicle::_adsbTimerTimeout()
} }
} }
void Vehicle::_mavlinkMessageStatus(int uasId, uint64_t totalSent, uint64_t totalReceived, uint64_t totalLoss, float lossPercent)
{
if(uasId == _id) {
_mavlinkSentCount = totalSent;
_mavlinkReceivedCount = totalReceived;
_mavlinkLossCount = totalLoss;
_mavlinkLossPercent = lossPercent;
emit mavlinkStatusChanged();
}
}
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
......
...@@ -625,6 +625,10 @@ public: ...@@ -625,6 +625,10 @@ public:
Q_PROPERTY(QString priorityLinkName READ priorityLinkName WRITE setPriorityLinkByName NOTIFY priorityLinkNameChanged) Q_PROPERTY(QString priorityLinkName READ priorityLinkName WRITE setPriorityLinkByName NOTIFY priorityLinkNameChanged)
Q_PROPERTY(QVariantList links READ links NOTIFY linksChanged) Q_PROPERTY(QVariantList links READ links NOTIFY linksChanged)
Q_PROPERTY(LinkInterface* priorityLink READ priorityLink NOTIFY priorityLinkNameChanged) Q_PROPERTY(LinkInterface* priorityLink READ priorityLink NOTIFY priorityLinkNameChanged)
Q_PROPERTY(quint64 mavlinkSentCount READ mavlinkSentCount NOTIFY mavlinkStatusChanged)
Q_PROPERTY(quint64 mavlinkReceivedCount READ mavlinkReceivedCount NOTIFY mavlinkStatusChanged)
Q_PROPERTY(quint64 mavlinkLossCount READ mavlinkLossCount NOTIFY mavlinkStatusChanged)
Q_PROPERTY(float mavlinkLossPercent READ mavlinkLossPercent NOTIFY mavlinkStatusChanged)
// Vehicle state used for guided control // Vehicle state used for guided control
Q_PROPERTY(bool flying READ flying NOTIFY flyingChanged) ///< Vehicle is flying Q_PROPERTY(bool flying READ flying NOTIFY flyingChanged) ///< Vehicle is flying
...@@ -1047,6 +1051,11 @@ public: ...@@ -1047,6 +1051,11 @@ public:
/// Vehicle is about to be deleted /// Vehicle is about to be deleted
void prepareDelete(); void prepareDelete();
quint64 mavlinkSentCount () { return _mavlinkSentCount; } /// Calculated total number of messages sent to us
quint64 mavlinkReceivedCount () { return _mavlinkReceivedCount; } /// Total number of sucessful messages received
quint64 mavlinkLossCount () { return _mavlinkLossCount; } /// Total number of lost messages
float mavlinkLossPercent () { return _mavlinkLossPercent; } /// Running loss rate
signals: signals:
void allLinksInactive(Vehicle* vehicle); void allLinksInactive(Vehicle* vehicle);
void coordinateChanged(QGeoCoordinate coordinate); void coordinateChanged(QGeoCoordinate coordinate);
...@@ -1150,6 +1159,7 @@ signals: ...@@ -1150,6 +1159,7 @@ signals:
// MAVLink protocol version // MAVLink protocol version
void requestProtocolVersion(unsigned version); void requestProtocolVersion(unsigned version);
void mavlinkStatusChanged();
private slots: private slots:
void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message); void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
...@@ -1182,6 +1192,7 @@ private slots: ...@@ -1182,6 +1192,7 @@ private slots:
void _updateHobbsMeter(void); void _updateHobbsMeter(void);
void _vehicleParamLoaded(bool ready); void _vehicleParamLoaded(bool ready);
void _sendQGCTimeToVehicle(void); void _sendQGCTimeToVehicle(void);
void _mavlinkMessageStatus(int uasId, uint64_t totalSent, uint64_t totalReceived, uint64_t totalLoss, float lossPercent);
void _trafficUpdate (bool alert, QString traffic_id, QString vehicle_id, QGeoCoordinate location, float heading); void _trafficUpdate (bool alert, QString traffic_id, QString vehicle_id, QGeoCoordinate location, float heading);
void _adsbTimerTimeout (); void _adsbTimerTimeout ();
...@@ -1417,6 +1428,11 @@ private: ...@@ -1417,6 +1428,11 @@ private:
SharedLinkInterfacePointer _priorityLink; // We always keep a reference to the priority link to manage shutdown ordering SharedLinkInterfacePointer _priorityLink; // We always keep a reference to the priority link to manage shutdown ordering
bool _priorityLinkCommanded; bool _priorityLinkCommanded;
uint64_t _mavlinkSentCount = 0;
uint64_t _mavlinkReceivedCount = 0;
uint64_t _mavlinkLossCount = 0;
float _mavlinkLossPercent = 0.0f;
// FactGroup facts // FactGroup facts
Fact _rollFact; Fact _rollFact;
......
...@@ -61,14 +61,13 @@ MAVLinkProtocol::MAVLinkProtocol(QGCApplication* app, QGCToolbox* toolbox) ...@@ -61,14 +61,13 @@ MAVLinkProtocol::MAVLinkProtocol(QGCApplication* app, QGCToolbox* toolbox)
, _logSuspendReplay(false) , _logSuspendReplay(false)
, _vehicleWasArmed(false) , _vehicleWasArmed(false)
, _tempLogFile(QString("%2.%3").arg(_tempLogFileTemplate).arg(_logFileExtension)) , _tempLogFile(QString("%2.%3").arg(_tempLogFileTemplate).arg(_logFileExtension))
, _linkMgr(NULL) , _linkMgr(nullptr)
, _multiVehicleManager(NULL) , _multiVehicleManager(nullptr)
{ {
memset(&totalReceiveCounter, 0, sizeof(totalReceiveCounter)); memset(totalReceiveCounter, 0, sizeof(totalReceiveCounter));
memset(&totalLossCounter, 0, sizeof(totalLossCounter)); memset(totalLossCounter, 0, sizeof(totalLossCounter));
memset(&totalErrorCounter, 0, sizeof(totalErrorCounter)); memset(runningLossPercent, 0, sizeof(runningLossPercent));
memset(&currReceiveCounter, 0, sizeof(currReceiveCounter)); memset(firstMessage, 1, sizeof(firstMessage));
memset(&currLossCounter, 0, sizeof(currLossCounter));
} }
MAVLinkProtocol::~MAVLinkProtocol() MAVLinkProtocol::~MAVLinkProtocol()
...@@ -109,15 +108,6 @@ void MAVLinkProtocol::setToolbox(QGCToolbox *toolbox) ...@@ -109,15 +108,6 @@ void MAVLinkProtocol::setToolbox(QGCToolbox *toolbox)
// All the *Counter variables are not initialized here, as they should be initialized // All the *Counter variables are not initialized here, as they should be initialized
// on a per-link basis before those links are used. @see resetMetadataForLink(). // on a per-link basis before those links are used. @see resetMetadataForLink().
// Initialize the list for tracking dropped messages to invalid.
for (int i = 0; i < 256; i++)
{
for (int j = 0; j < 256; j++)
{
lastIndex[i][j] = -1;
}
}
connect(this, &MAVLinkProtocol::protocolStatusMessage, _app, &QGCApplication::criticalMessageBoxOnMainThread); connect(this, &MAVLinkProtocol::protocolStatusMessage, _app, &QGCApplication::criticalMessageBoxOnMainThread);
connect(this, &MAVLinkProtocol::saveTelemetryLog, _app, &QGCApplication::saveTelemetryLogOnMainThread); connect(this, &MAVLinkProtocol::saveTelemetryLog, _app, &QGCApplication::saveTelemetryLogOnMainThread);
connect(this, &MAVLinkProtocol::checkTelemetrySavePath, _app, &QGCApplication::checkTelemetrySavePathOnMainThread); connect(this, &MAVLinkProtocol::checkTelemetrySavePath, _app, &QGCApplication::checkTelemetrySavePathOnMainThread);
...@@ -157,10 +147,11 @@ void MAVLinkProtocol::resetMetadataForLink(LinkInterface *link) ...@@ -157,10 +147,11 @@ void MAVLinkProtocol::resetMetadataForLink(LinkInterface *link)
{ {
int channel = link->mavlinkChannel(); int channel = link->mavlinkChannel();
totalReceiveCounter[channel] = 0; totalReceiveCounter[channel] = 0;
totalLossCounter[channel] = 0; totalLossCounter[channel] = 0;
totalErrorCounter[channel] = 0; runningLossPercent[channel] = 0.0f;
currReceiveCounter[channel] = 0; for(int i = 0; i < 256; i++) {
currLossCounter[channel] = 0; firstMessage[channel][i] = 1;
}
link->setDecodedFirstMavlinkPacket(false); link->setDecodedFirstMavlinkPacket(false);
} }
...@@ -171,6 +162,7 @@ void MAVLinkProtocol::resetMetadataForLink(LinkInterface *link) ...@@ -171,6 +162,7 @@ void MAVLinkProtocol::resetMetadataForLink(LinkInterface *link)
* @param link The interface to read from * @param link The interface to read from
* @see LinkInterface * @see LinkInterface
**/ **/
void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
{ {
// Since receiveBytes signals cross threads we can end up with signals in the queue // Since receiveBytes signals cross threads we can end up with signals in the queue
...@@ -180,56 +172,67 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) ...@@ -180,56 +172,67 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
return; return;
} }
// receiveMutex.lock(); uint8_t mavlinkChannel = link->mavlinkChannel();
mavlink_message_t message;
mavlink_status_t status;
int mavlinkChannel = link->mavlinkChannel();
static int nonmavlinkCount = 0; static int nonmavlinkCount = 0;
static bool checkedUserNonMavlink = false; static bool checkedUserNonMavlink = false;
static bool warnedUserNonMavlink = false; static bool warnedUserNonMavlink = false;
for (int position = 0; position < b.size(); position++) { for (int position = 0; position < b.size(); position++) {
unsigned int decodeState = mavlink_parse_char(mavlinkChannel, (uint8_t)(b[position]), &message, &status); if (mavlink_parse_char(mavlinkChannel, static_cast<uint8_t>(b[position]), &_message, &_status)) {
// Got a valid message
if (decodeState == 0 && !link->decodedFirstMavlinkPacket())
{
nonmavlinkCount++;
if (nonmavlinkCount > 1000 && !warnedUserNonMavlink)
{
// 500 bytes with no mavlink message. Are we connected to a mavlink capable device?
if (!checkedUserNonMavlink)
{
link->requestReset();
checkedUserNonMavlink = true;
}
else
{
warnedUserNonMavlink = true;
// Disconnect the link since its some other device and
// QGC clinging on to it and feeding it data might have unintended
// side effects (e.g. if its a modem)
qDebug() << "disconnected link" << link->getName() << "as it contained no MAVLink data";
QMetaObject::invokeMethod(_linkMgr, "disconnectLink", Q_ARG( LinkInterface*, link ) );
return;
}
}
}
if (decodeState == 1)
{
if (!link->decodedFirstMavlinkPacket()) { if (!link->decodedFirstMavlinkPacket()) {
link->setDecodedFirstMavlinkPacket(true); link->setDecodedFirstMavlinkPacket(true);
mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel); mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel);
if (!(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) && (mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) { if (!(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) && (mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) {
qDebug() << "Switching outbound to mavlink 2.0 due to incoming mavlink 2.0 packet:" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags; qDebug() << "Switching outbound to mavlink 2.0 due to incoming mavlink 2.0 packet:" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags;
mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
// Set all links to v2 // Set all links to v2
setVersion(200); setVersion(200);
} }
} }
//-----------------------------------------------------------------
// MAVLink Status
uint8_t lastSeq = lastIndex[_message.sysid][_message.compid];
uint8_t expectedSeq = lastSeq + 1;
// Increase receive counter
totalReceiveCounter[mavlinkChannel]++;
// Determine what the next expected sequence number is, accounting for
// never having seen a message for this system/component pair.
if(firstMessage[_message.sysid][_message.compid]) {
firstMessage[_message.sysid][_message.compid] = 0;
lastSeq = _message.seq;
expectedSeq = _message.seq;
}
// And if we didn't encounter that sequence number, record the error
//int foo = 0;
if (_message.seq != expectedSeq)
{
//foo = 1;
int lostMessages = 0;
//-- Account for overflow during packet loss
if(_message.seq < expectedSeq) {
lostMessages = (_message.seq + 255) - expectedSeq;
} else {
lostMessages = _message.seq - expectedSeq;
}
// Log how many were lost
totalLossCounter[mavlinkChannel] += static_cast<uint64_t>(lostMessages);
}
// And update the last sequence number for this system/component pair
lastIndex[_message.sysid][_message.compid] = _message.seq;;
// Calculate new loss ratio
uint64_t totalSent = totalReceiveCounter[mavlinkChannel] + totalLossCounter[mavlinkChannel];
float receiveLossPercent = static_cast<float>(static_cast<double>(totalLossCounter[mavlinkChannel]) / static_cast<double>(totalSent));
receiveLossPercent *= 100.0f;
receiveLossPercent = (receiveLossPercent * 0.5f) + (runningLossPercent[mavlinkChannel] * 0.5f);
runningLossPercent[mavlinkChannel] = receiveLossPercent;
//qDebug() << foo << _message.seq << expectedSeq << lastSeq << totalLossCounter[mavlinkChannel] << totalReceiveCounter[mavlinkChannel] << totalSentCounter[mavlinkChannel] << "(" << _message.sysid << _message.compid << ")";
//-----------------------------------------------------------------
// Log data // Log data
if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) { if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) {
uint8_t buf[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)]; uint8_t buf[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)];
...@@ -237,17 +240,17 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) ...@@ -237,17 +240,17 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
// Write the uint64 time in microseconds in big endian format before the message. // Write the uint64 time in microseconds in big endian format before the message.
// This timestamp is saved in UTC time. We are only saving in ms precision because // This timestamp is saved in UTC time. We are only saving in ms precision because
// getting more than this isn't possible with Qt without a ton of extra code. // getting more than this isn't possible with Qt without a ton of extra code.
quint64 time = (quint64)QDateTime::currentMSecsSinceEpoch() * 1000; quint64 time = static_cast<quint64>(QDateTime::currentMSecsSinceEpoch() * 1000);
qToBigEndian(time, buf); qToBigEndian(time, buf);
// Then write the message to the buffer // Then write the message to the buffer
int len = mavlink_msg_to_send_buffer(buf + sizeof(quint64), &message); int len = mavlink_msg_to_send_buffer(buf + sizeof(quint64), &_message);
// Determine how many bytes were written by adding the timestamp size to the message size // Determine how many bytes were written by adding the timestamp size to the message size
len += sizeof(quint64); len += sizeof(quint64);
// Now write this timestamp/message pair to the log. // Now write this timestamp/message pair to the log.
QByteArray b((const char*)buf, len); QByteArray b(reinterpret_cast<const char*>(buf), len);
if(_tempLogFile.write(b) != len) if(_tempLogFile.write(b) != len)
{ {
// If there's an error logging data, raise an alert and stop logging. // If there's an error logging data, raise an alert and stop logging.
...@@ -257,32 +260,32 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) ...@@ -257,32 +260,32 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
} }
// Check for the vehicle arming going by. This is used to trigger log save. // Check for the vehicle arming going by. This is used to trigger log save.
if (!_vehicleWasArmed && message.msgid == MAVLINK_MSG_ID_HEARTBEAT) { if (!_vehicleWasArmed && _message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
mavlink_heartbeat_t state; mavlink_heartbeat_t state;
mavlink_msg_heartbeat_decode(&message, &state); mavlink_msg_heartbeat_decode(&_message, &state);
if (state.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY) { if (state.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY) {
_vehicleWasArmed = true; _vehicleWasArmed = true;
} }
} }
} }
if (message.msgid == MAVLINK_MSG_ID_HEARTBEAT) { if (_message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
_startLogging(); _startLogging();
mavlink_heartbeat_t heartbeat; mavlink_heartbeat_t heartbeat;
mavlink_msg_heartbeat_decode(&message, &heartbeat); mavlink_msg_heartbeat_decode(&_message, &heartbeat);
emit vehicleHeartbeatInfo(link, message.sysid, message.compid, heartbeat.autopilot, heartbeat.type); emit vehicleHeartbeatInfo(link, _message.sysid, _message.compid, heartbeat.autopilot, heartbeat.type);
} }
if (message.msgid == MAVLINK_MSG_ID_HIGH_LATENCY2) { if (_message.msgid == MAVLINK_MSG_ID_HIGH_LATENCY2) {
_startLogging(); _startLogging();
mavlink_high_latency2_t highLatency2; mavlink_high_latency2_t highLatency2;
mavlink_msg_high_latency2_decode(&message, &highLatency2); mavlink_msg_high_latency2_decode(&_message, &highLatency2);
emit vehicleHeartbeatInfo(link, message.sysid, message.compid, highLatency2.autopilot, highLatency2.type); emit vehicleHeartbeatInfo(link, _message.sysid, _message.compid, highLatency2.autopilot, highLatency2.type);
} }
// Detect if we are talking to an old radio not supporting v2 // Detect if we are talking to an old radio not supporting v2
mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel); mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel);
if (message.msgid == MAVLINK_MSG_ID_RADIO_STATUS) { if (_message.msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
if ((mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) if ((mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1)
&& !(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) { && !(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) {
...@@ -300,53 +303,36 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) ...@@ -300,53 +303,36 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
} }
// Increase receive counter // Update MAVLink status on every 32th packet
totalReceiveCounter[mavlinkChannel]++; if ((totalReceiveCounter[mavlinkChannel] & 0x1F) == 0) {
currReceiveCounter[mavlinkChannel]++; emit mavlinkMessageStatus(_message.sysid, totalSent, totalReceiveCounter[mavlinkChannel], totalLossCounter[mavlinkChannel], receiveLossPercent);
// Determine what the next expected sequence number is, accounting for
// never having seen a message for this system/component pair.
int lastSeq = lastIndex[message.sysid][message.compid];
int expectedSeq = (lastSeq == -1) ? message.seq : (lastSeq + 1);
// And if we didn't encounter that sequence number, record the error
if (message.seq != expectedSeq)
{
// Determine how many messages were skipped
int lostMessages = message.seq - expectedSeq;
// Out of order messages or wraparound can cause this, but we just ignore these conditions for simplicity
if (lostMessages < 0)
{
lostMessages = 0;
}
// And log how many were lost for all time and just this timestep
totalLossCounter[mavlinkChannel] += lostMessages;
currLossCounter[mavlinkChannel] += lostMessages;
}
// And update the last sequence number for this system/component pair
lastIndex[message.sysid][message.compid] = expectedSeq;
// Update on every 32th packet
if ((totalReceiveCounter[mavlinkChannel] & 0x1F) == 0)
{
// Calculate new loss ratio
// Receive loss
float receiveLossPercent = (double)currLossCounter[mavlinkChannel]/(double)(currReceiveCounter[mavlinkChannel]+currLossCounter[mavlinkChannel]);
receiveLossPercent *= 100.0f;
currLossCounter[mavlinkChannel] = 0;
currReceiveCounter[mavlinkChannel] = 0;
emit receiveLossPercentChanged(message.sysid, receiveLossPercent);
emit receiveLossTotalChanged(message.sysid, totalLossCounter[mavlinkChannel]);
} }
// The packet is emitted as a whole, as it is only 255 - 261 bytes short // The packet is emitted as a whole, as it is only 255 - 261 bytes short
// kind of inefficient, but no issue for a groundstation pc. // kind of inefficient, but no issue for a groundstation pc.
// It buys as reentrancy for the whole code over all threads // It buys as reentrancy for the whole code over all threads
emit messageReceived(link, message); emit messageReceived(link, _message);
// Reset message parsing
memset(&_status, 0, sizeof(_status));
memset(&_message, 0, sizeof(_message));
} else if (!link->decodedFirstMavlinkPacket()) {
// No formed message yet
nonmavlinkCount++;
if (nonmavlinkCount > 1000 && !warnedUserNonMavlink) {
// 1000 bytes with no mavlink message. Are we connected to a mavlink capable device?
if (!checkedUserNonMavlink) {
link->requestReset();
checkedUserNonMavlink = true;
} else {
warnedUserNonMavlink = true;
// Disconnect the link since it's some other device and
// QGC clinging on to it and feeding it data might have unintended
// side effects (e.g. if its a modem)
qDebug() << "disconnected link" << link->getName() << "as it contained no MAVLink data";
QMetaObject::invokeMethod(_linkMgr, "disconnectLink", Q_ARG( LinkInterface*, link ) );
return;
}
}
} }
} }
} }
......
...@@ -65,27 +65,6 @@ public: ...@@ -65,27 +65,6 @@ public:
unsigned getCurrentVersion() { unsigned getCurrentVersion() {
return _current_version; return _current_version;
} }
/**
* Retrieve a total of all successfully parsed packets for the specified link.
* @returns -1 if this is not available for this protocol, # of packets otherwise.
*/
qint32 getReceivedPacketCount(const LinkInterface *link) const {
return totalReceiveCounter[link->mavlinkChannel()];
}
/**
* Retrieve a total of all parsing errors for the specified link.
* @returns -1 if this is not available for this protocol, # of errors otherwise.
*/
qint32 getParsingErrorCount(const LinkInterface *link) const {
return totalErrorCounter[link->mavlinkChannel()];
}
/**
* Retrieve a total of all dropped packets for the specified link.
* @returns -1 if this is not available for this protocol, # of packets otherwise.
*/
qint32 getDroppedPacketCount(const LinkInterface *link) const {
return totalLossCounter[link->mavlinkChannel()];
}
/** /**
* Reset the counters for all metadata for this link. * Reset the counters for all metadata for this link.
*/ */
...@@ -122,18 +101,20 @@ public slots: ...@@ -122,18 +101,20 @@ public slots:
void checkForLostLogFiles(void); void checkForLostLogFiles(void);
protected: protected:
bool m_enable_version_check; ///< Enable checking of version match of MAV and QGC bool m_enable_version_check; ///< Enable checking of version match of MAV and QGC
QMutex receiveMutex; ///< Mutex to protect receiveBytes function uint8_t lastIndex[256][256]; ///< Store the last received sequence ID for each system/componenet pair
int lastIndex[256][256]; ///< Store the last received sequence ID for each system/componenet pair uint8_t firstMessage[256][256]; ///< First message flag
int totalReceiveCounter[MAVLINK_COMM_NUM_BUFFERS]; ///< The total number of successfully received messages uint64_t totalReceiveCounter[MAVLINK_COMM_NUM_BUFFERS]; ///< The total number of successfully received messages
int totalLossCounter[MAVLINK_COMM_NUM_BUFFERS]; ///< Total messages lost during transmission. uint64_t totalLossCounter[MAVLINK_COMM_NUM_BUFFERS]; ///< Total messages lost during transmission.
int totalErrorCounter[MAVLINK_COMM_NUM_BUFFERS]; ///< Total count of all parsing errors. Generally <= totalLossCounter. float runningLossPercent[MAVLINK_COMM_NUM_BUFFERS]; ///< Loss rate
int currReceiveCounter[MAVLINK_COMM_NUM_BUFFERS]; ///< Received messages during this sample time window. Used for calculating loss %.
int currLossCounter[MAVLINK_COMM_NUM_BUFFERS]; ///< Lost messages during this sample time window. Used for calculating loss %. mavlink_message_t _message = {};
bool versionMismatchIgnore; mavlink_status_t _status = {};
int systemId;
unsigned _current_version; bool versionMismatchIgnore;
unsigned _radio_version_mismatch_count; int systemId;
unsigned _current_version;
unsigned _radio_version_mismatch_count;
signals: signals:
/// Heartbeat received on link /// Heartbeat received on link
...@@ -148,8 +129,7 @@ signals: ...@@ -148,8 +129,7 @@ signals:
/** @brief Emitted if a new system ID was set */ /** @brief Emitted if a new system ID was set */
void systemIdChanged(int systemId); void systemIdChanged(int systemId);
void receiveLossPercentChanged(int uasId, float lossPercent); void mavlinkMessageStatus(int uasId, uint64_t totalSent, uint64_t totalReceived, uint64_t totalLoss, float lossPercent);
void receiveLossTotalChanged(int uasId, int totalLoss);
/** /**
* @brief Emitted if a new radio status packet received * @brief Emitted if a new radio status packet received
......
...@@ -16,8 +16,7 @@ ...@@ -16,8 +16,7 @@
* *
*/ */
#ifndef _PROTOCOLINTERFACE_H_ #pragma once
#define _PROTOCOLINTERFACE_H_
#include <QThread> #include <QThread>
#include <QString> #include <QString>
...@@ -37,26 +36,8 @@ class ProtocolInterface : public QThread ...@@ -37,26 +36,8 @@ class ProtocolInterface : public QThread
{ {
Q_OBJECT Q_OBJECT
public: public:
virtual ~ProtocolInterface () {} virtual ~ProtocolInterface () {}
virtual QString getName() = 0; virtual QString getName () = 0;
/**
* Retrieve a total of all successfully parsed packets for the specified link.
* @param link The link to return metadata about.
* @returns -1 if this is not available for this protocol, # of packets otherwise.
*/
virtual qint32 getReceivedPacketCount(const LinkInterface *link) const = 0;
/**
* Retrieve a total of all parsing errors for the specified link.
* @param link The link to return metadata about.
* @returns -1 if this is not available for this protocol, # of errors otherwise.
*/
virtual qint32 getParsingErrorCount(const LinkInterface *link) const = 0;
/**
* Retrieve a total of all dropped packets for the specified link.
* @param link The link to return metadata about.
* @returns -1 if this is not available for this protocol, # of packets otherwise.
*/
virtual qint32 getDroppedPacketCount(const LinkInterface *link) const = 0;
/** /**
* Reset the received, error, and dropped counts for the given link. Useful for * Reset the received, error, and dropped counts for the given link. Useful for
* when reconnecting a link. * when reconnecting a link.
...@@ -65,13 +46,11 @@ public: ...@@ -65,13 +46,11 @@ public:
virtual void resetMetadataForLink(LinkInterface *link) = 0; virtual void resetMetadataForLink(LinkInterface *link) = 0;
public slots: public slots:
virtual void receiveBytes(LinkInterface *link, QByteArray b) = 0; virtual void receiveBytes (LinkInterface *link, QByteArray b) = 0;
virtual void linkStatusChanged(bool connected) = 0; virtual void linkStatusChanged (bool connected) = 0;
signals: signals:
/** @brief Update the packet loss from one system */ /** @brief Update the packet loss from one system */
void receiveLossChanged(int uasId, float loss); void receiveLossChanged (int uasId, float loss);
}; };
#endif // _PROTOCOLINTERFACE_H_
...@@ -30,6 +30,7 @@ Rectangle { ...@@ -30,6 +30,7 @@ Rectangle {
property int _selectedCount: 0 property int _selectedCount: 0
property real _columnSpacing: ScreenTools.defaultFontPixelHeight * 0.25 property real _columnSpacing: ScreenTools.defaultFontPixelHeight * 0.25
property bool _uploadedSelected: false property bool _uploadedSelected: false
property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
QGCPalette { id: qgcPal } QGCPalette { id: qgcPal }
...@@ -152,6 +153,92 @@ Rectangle { ...@@ -152,6 +153,92 @@ Rectangle {
} }
} }
//----------------------------------------------------------------- //-----------------------------------------------------------------
//-- Mavlink Status
Item {
width: __mavlinkRoot.width * 0.8
height: mavStatusLabel.height
anchors.margins: ScreenTools.defaultFontPixelWidth
anchors.horizontalCenter: parent.horizontalCenter
QGCLabel {
id: mavStatusLabel
text: qsTr("MAVLink Link Status (Current Vehicle)")
font.family: ScreenTools.demiboldFontFamily
}
}
Rectangle {
height: mavStatusColumn.height + (ScreenTools.defaultFontPixelHeight * 2)
width: __mavlinkRoot.width * 0.8
color: qgcPal.windowShade
anchors.margins: ScreenTools.defaultFontPixelWidth
anchors.horizontalCenter: parent.horizontalCenter
Column {
id: mavStatusColumn