Newer
Older
/*===================================================================
======================================================================*/
/**
* @file
* @brief Implementation of class MAVLinkProtocol
* @author Lorenz Meier <mail@qgroundcontrol.org>
*/
#include <inttypes.h>
#include <iostream>
#include <QDebug>
#include <QTime>
#include <QSettings>
#include <QDesktopServices>
#include "MAVLinkProtocol.h"
#include "UASInterface.h"
#include "UASManager.h"
#include "UASInterface.h"
#include "UAS.h"
#include "SlugsMAV.h"
#include "PxQuadMAV.h"
#include "QGCMAVLink.h"
#include "QGCMAVLinkUASFactory.h"
#ifdef QGC_PROTOBUF_ENABLED
#include <google/protobuf/descriptor.h>
#endif
/**
* The default constructor will create a new MAVLink object sending heartbeats at
* the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links.
*/
MAVLinkProtocol::MAVLinkProtocol() :
heartbeatTimer(new QTimer(this)),
heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
m_heartbeatsEnabled(false),
m_multiplexingEnabled(false),
m_authEnabled(false),
m_loggingEnabled(false),
m_logfile(NULL),
m_enable_version_check(true),
m_paramRetransmissionTimeout(350),
m_paramRewriteTimeout(500),
m_paramGuardEnabled(true),
m_actionGuardEnabled(false),
m_actionRetransmissionTimeout(100),
versionMismatchIgnore(false),
systemId(QGC::defaultSystemId)
m_authKey = "xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx";
loadSettings();
//start(QThread::LowPriority);
// Start heartbeat timer, emitting a heartbeat at the configured rate
connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(sendHeartbeat()));
heartbeatTimer->start(1000/heartbeatRate);
currReceiveCounter = 0;
currLossCounter = 0;
LM
committed
for (int i = 0; i < 256; i++)
{
for (int j = 0; j < 256; j++)
{
void MAVLinkProtocol::loadSettings()
{
// Load defaults from settings
QSettings settings;
settings.sync();
settings.beginGroup("QGC_MAVLINK_PROTOCOL");
enableHeartbeats(settings.value("HEARTBEATS_ENABLED", m_heartbeatsEnabled).toBool());
enableVersionCheck(settings.value("VERSION_CHECK_ENABLED", m_enable_version_check).toBool());
enableMultiplexing(settings.value("MULTIPLEXING_ENABLED", m_multiplexingEnabled).toBool());
// Only set logfile if there is a name present in settings
LM
committed
if (settings.contains("LOGFILE_NAME") && m_logfile == NULL)
{
m_logfile = new QFile(settings.value("LOGFILE_NAME").toString());
LM
committed
}
else if (m_logfile == NULL)
{
m_logfile = new QFile(QDesktopServices::storageLocation(QDesktopServices::HomeLocation) + "/qgroundcontrol_packetlog.mavlink");
}
// Enable logging
enableLogging(settings.value("LOGGING_ENABLED", m_loggingEnabled).toBool());
// Only set system id if it was valid
int temp = settings.value("GCS_SYSTEM_ID", systemId).toInt();
LM
committed
if (temp > 0 && temp < 256)
{
systemId = temp;
}
// Set auth key
m_authKey = settings.value("GCS_AUTH_KEY", m_authKey).toString();
enableAuth(settings.value("GCS_AUTH_ENABLED", m_authEnabled).toBool());
// Parameter interface settings
bool ok;
temp = settings.value("PARAMETER_RETRANSMISSION_TIMEOUT", m_paramRetransmissionTimeout).toInt(&ok);
if (ok) m_paramRetransmissionTimeout = temp;
temp = settings.value("PARAMETER_REWRITE_TIMEOUT", m_paramRewriteTimeout).toInt(&ok);
if (ok) m_paramRewriteTimeout = temp;
m_paramGuardEnabled = settings.value("PARAMETER_TRANSMISSION_GUARD_ENABLED", m_paramGuardEnabled).toBool();
settings.endGroup();
}
void MAVLinkProtocol::storeSettings()
{
// Store settings
QSettings settings;
settings.beginGroup("QGC_MAVLINK_PROTOCOL");
settings.setValue("HEARTBEATS_ENABLED", m_heartbeatsEnabled);
settings.setValue("LOGGING_ENABLED", m_loggingEnabled);
settings.setValue("VERSION_CHECK_ENABLED", m_enable_version_check);
settings.setValue("MULTIPLEXING_ENABLED", m_multiplexingEnabled);
settings.setValue("GCS_SYSTEM_ID", systemId);
settings.setValue("GCS_AUTH_KEY", m_authKey);
settings.setValue("GCS_AUTH_ENABLED", m_authEnabled);
LM
committed
if (m_logfile)
{
// Logfile exists, store the name
settings.setValue("LOGFILE_NAME", m_logfile->fileName());
}
// Parameter interface settings
settings.setValue("PARAMETER_RETRANSMISSION_TIMEOUT", m_paramRetransmissionTimeout);
settings.setValue("PARAMETER_REWRITE_TIMEOUT", m_paramRewriteTimeout);
settings.setValue("PARAMETER_TRANSMISSION_GUARD_ENABLED", m_paramGuardEnabled);
settings.endGroup();
settings.sync();
//qDebug() << "Storing settings!";
}
storeSettings();
LM
committed
if (m_logfile)
{
if (m_logfile->isOpen())
{
m_logfile->flush();
m_logfile->close();
}
LM
committed
m_logfile = NULL;
LM
committed
if (m_logfile)
{
return m_logfile->fileName();
LM
committed
}
else
{
return QDesktopServices::storageLocation(QDesktopServices::HomeLocation) + "/qgroundcontrol_packetlog.mavlink";
}
/**
* The bytes are copied by calling the LinkInterface::readBytes() method.
* This method parses all incoming bytes and constructs a MAVLink packet.
* It can handle multiple links in parallel, as each link has it's own buffer/
* parsing state machine.
* @param link The interface to read from
* @see LinkInterface
**/
void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
// receiveMutex.lock();
for (int position = 0; position < b.size(); position++) {
unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)(b.at(position)), &message, &status);
LM
committed
if (decodeState == 1)
{
//#ifdef MAVLINK_MESSAGE_LENGTHS
// const uint8_t message_lengths[] = MAVLINK_MESSAGE_LENGTHS;
// if (message.msgid >= sizeof(message_lengths) ||
// message.len != message_lengths[message.msgid]) {
// qDebug() << "MAVLink message " << message.msgid << " length incorrect (was " << message.len << " expected " << message_lengths[message.msgid] << ")";
// continue;
// }
//#endif
if (message.msgid == MAVLINK_MSG_ID_EXTENDED_MESSAGE)
{
Lionel Heng
committed
mavlink_extended_message_t extended_message;
extended_message.base_msg = message;
// read extended header
uint8_t* payload = reinterpret_cast<uint8_t*>(message.payload64);
memcpy(&extended_message.extended_payload_len, payload + 3, 4);
const uint8_t* extended_payload = reinterpret_cast<const uint8_t*>(b.constData()) + MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_EXTENDED_HEADER_LEN;
// copy extended payload data
memcpy(extended_message.extended_payload, extended_payload, extended_message.extended_payload_len);
if (protobufManager.cacheFragment(extended_message))
{
std::tr1::shared_ptr<google::protobuf::Message> protobuf_msg;
if (protobufManager.getMessage(protobuf_msg))
{
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
const google::protobuf::Descriptor* descriptor = protobuf_msg->GetDescriptor();
if (!descriptor)
{
continue;
}
const google::protobuf::FieldDescriptor* headerField = descriptor->FindFieldByName("header");
if (!headerField)
{
continue;
}
const google::protobuf::Descriptor* headerDescriptor = headerField->message_type();
if (!headerDescriptor)
{
continue;
}
const google::protobuf::FieldDescriptor* sourceSysIdField = headerDescriptor->FindFieldByName("source_sysid");
if (!sourceSysIdField)
{
continue;
}
const google::protobuf::Reflection* reflection = protobuf_msg->GetReflection();
const google::protobuf::Message& headerMsg = reflection->GetMessage(*protobuf_msg, headerField);
const google::protobuf::Reflection* headerReflection = headerMsg.GetReflection();
int source_sysid = headerReflection->GetInt32(headerMsg, sourceSysIdField);
UASInterface* uas = UASManager::instance()->getUASForId(source_sysid);
if (uas != NULL)
{
emit extendedMessageReceived(link, protobuf_msg);
}
Lionel Heng
committed
}
}
position += extended_message.extended_payload_len;
continue;
LM
committed
if (m_loggingEnabled && m_logfile)
{
uint8_t buf[len];
quint64 time = QGC::groundTimeUsecs();
// Write message to buffer
mavlink_msg_to_send_buffer(buf+sizeof(quint64), &message);
QByteArray b((const char*)buf, len);
LM
committed
if(m_logfile->write(b) < static_cast<qint64>(MAVLINK_MAX_PACKET_LEN+sizeof(quint64)))
{
emit protocolStatusMessage(tr("MAVLink Logging failed"), tr("Could not write to file %1, disabling logging.").arg(m_logfile->fileName()));
// Stop logging
enableLogging(false);
}
// ORDER MATTERS HERE!
// If the matching UAS object does not yet exist, it has to be created
// before emitting the packetReceived signal
UASInterface* uas = UASManager::instance()->getUASForId(message.sysid);
// Check and (if necessary) create UAS object
LM
committed
if (uas == NULL && message.msgid == MAVLINK_MSG_ID_HEARTBEAT)
{
// ORDER MATTERS HERE!
// The UAS object has first to be created and connected,
// only then the rest of the application can be made aware
// of its existence, as it only then can send and receive
// it's first messages.
// Check if the UAS has the same id like this system
LM
committed
if (message.sysid == getSystemId())
{
emit protocolStatusMessage(tr("SYSTEM ID CONFLICT!"), tr("Warning: A second system is using the same system id (%1)").arg(getSystemId()));
// Create a new UAS based on the heartbeat received
// Todo dynamically load plugin at run-time for MAV
// WIKISEARCH:AUTOPILOT_TYPE_INSTANTIATION
// Decode heartbeat message
mavlink_heartbeat_t heartbeat;
// Reset version field to 0
heartbeat.mavlink_version = 0;
mavlink_msg_heartbeat_decode(&message, &heartbeat);
// Check if the UAS has a different protocol version
LM
committed
if (m_enable_version_check && (heartbeat.mavlink_version != MAVLINK_VERSION))
{
LM
committed
if (!versionMismatchIgnore)
{
emit protocolStatusMessage(tr("The MAVLink protocol version on the MAV and QGroundControl mismatch!"),
tr("It is unsafe to use different MAVLink versions. QGroundControl therefore refuses to connect to system %1, which sends MAVLink version %2 (QGroundControl uses version %3).").arg(message.sysid).arg(heartbeat.mavlink_version).arg(MAVLINK_VERSION));
versionMismatchIgnore = true;
}
// Ignore this message and continue gracefully
continue;
}
// Create a new UAS object
uas = QGCMAVLinkUASFactory::createUAS(this, link, message.sysid, &heartbeat);
// Only count message if UAS exists for this message
LM
committed
if (uas != NULL)
{
// Increase receive counter
totalReceiveCounter++;
currReceiveCounter++;
// Update last packet index
LM
committed
if (lastIndex[message.sysid][message.compid] == -1)
{
lastIndex[message.sysid][message.compid] = message.seq;
LM
committed
}
else
{
uint8_t expectedIndex = lastIndex[message.sysid][message.compid];
// Now increase to the expected index
expectedIndex++;
//qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "MSGID" << message.msgid << "EXPECTED INDEX:" << expectedIndex << "SEQ" << message.seq;
while(expectedIndex != message.seq)
LM
committed
{
totalLossCounter++;
currLossCounter++;
// Set new lastindex
lastIndex[message.sysid][message.compid] = message.seq;
}
// if (lastIndex.contains(message.sysid))
// {
// QMap<int, int>* lastCompIndex = lastIndex.value(message.sysid);
// if (lastCompIndex->contains(message.compid))
// while (lastCompIndex->value(message.compid, 0)+1 )
// }
//if ()
// Update on every 32th packet
if (totalReceiveCounter % 32 == 0)
LM
committed
{
// Calculate new loss ratio
// Receive loss
float receiveLoss = (double)currLossCounter/(double)(currReceiveCounter+currLossCounter);
receiveLoss *= 100.0f;
// qDebug() << "LOSSCHANGED" << receiveLoss;
currLossCounter = 0;
currReceiveCounter = 0;
//qDebug() << "LOSSCHANGED" << message.sysid<<" "<<receiveLoss;
// 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.
// It buys as reentrancy for the whole code over all threads
emit messageReceived(link, message);
// Multiplex message if enabled
LM
committed
if (m_multiplexingEnabled)
{
// Get all links connected to this unit
QList<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(this);
// Emit message on all links that are currently connected
LM
committed
foreach (LinkInterface* currLink, links)
{
// Only forward this message to the other links,
// not the link the message was received on
if (currLink != link) sendMessage(currLink, message);
}
}
// receiveMutex.unlock();
}
/**
* @return The name of this protocol
**/
QString MAVLinkProtocol::getName()
{
return QString(tr("MAVLink protocol"));
}
/** @return System id of this application */
int MAVLinkProtocol::getSystemId()
return systemId;
}
void MAVLinkProtocol::setSystemId(int id)
{
systemId = id;
}
/** @return Component id of this application */
int MAVLinkProtocol::getComponentId()
return QGC::defaultComponentId;
/**
* @param message message to send
*/
void MAVLinkProtocol::sendMessage(mavlink_message_t message)
{
// Get all links connected to this unit
QList<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(this);
// Emit message on all links that are currently connected
QList<LinkInterface*>::iterator i;
LM
committed
for (i = links.begin(); i != links.end(); ++i)
{
//qDebug() << __FILE__ << __LINE__ << "SENT MESSAGE OVER" << ((LinkInterface*)*i)->getName() << "LIST SIZE:" << links.size();
}
}
/**
* @param link the link to send the message over
* @param message message to send
*/
void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message)
{
// Create buffer
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
// Rewriting header to ensure correct link ID is set
static uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS;
if (link->getId() != 0) mavlink_finalize_message_chan(&message, this->getSystemId(), this->getComponentId(), link->getId(), message.len, messageKeys[message.msgid]);
int len = mavlink_msg_to_send_buffer(buffer, &message);
LM
committed
if (link->isConnected())
{
// Send the portion of the buffer now occupied by the message
link->writeBytes((const char*)buffer, len);
}
}
/**
* The heartbeat is sent out of order and does not reset the
* periodic heartbeat emission. It will be just sent in addition.
* @return mavlink_message_t heartbeat message sent on serial link
*/
void MAVLinkProtocol::sendHeartbeat()
{
mavlink_msg_heartbeat_pack(getSystemId(), getComponentId(),&beat, MAV_TYPE_GCS, MAV_AUTOPILOT_INVALID, MAV_MODE_MANUAL_ARMED, 0, MAV_STATE_ACTIVE);
if (m_authEnabled) {
mavlink_message_t msg;
mavlink_auth_key_t auth;
if (m_authKey.length() != MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN) m_authKey.resize(MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN);
strcpy(auth.key, m_authKey.toStdString().c_str());
mavlink_msg_auth_key_encode(getSystemId(), getComponentId(), &msg, &auth);
sendMessage(msg);
}
}
/** @param enabled true to enable heartbeats emission at heartbeatRate, false to disable */
void MAVLinkProtocol::enableHeartbeats(bool enabled)
{
m_heartbeatsEnabled = enabled;
emit heartbeatChanged(enabled);
}
void MAVLinkProtocol::enableMultiplexing(bool enabled)
{
bool changed = false;
if (enabled != m_multiplexingEnabled) changed = true;
m_multiplexingEnabled = enabled;
if (changed) emit multiplexingChanged(m_multiplexingEnabled);
}
void MAVLinkProtocol::enableAuth(bool enable)
{
bool changed = false;
m_authEnabled = enable;
changed = true;
}
if (changed) emit authChanged(m_authEnabled);
}
void MAVLinkProtocol::enableParamGuard(bool enabled)
{
if (enabled != m_paramGuardEnabled) {
m_paramGuardEnabled = enabled;
emit paramGuardChanged(m_paramGuardEnabled);
}
}
lm
committed
void MAVLinkProtocol::enableActionGuard(bool enabled)
{
if (enabled != m_actionGuardEnabled) {
lm
committed
m_actionGuardEnabled = enabled;
emit actionGuardChanged(m_actionGuardEnabled);
}
}
void MAVLinkProtocol::setParamRetransmissionTimeout(int ms)
{
if (ms != m_paramRetransmissionTimeout) {
m_paramRetransmissionTimeout = ms;
emit paramRetransmissionTimeoutChanged(m_paramRetransmissionTimeout);
}
}
void MAVLinkProtocol::setParamRewriteTimeout(int ms)
{
if (ms != m_paramRewriteTimeout) {
m_paramRewriteTimeout = ms;
emit paramRewriteTimeoutChanged(m_paramRewriteTimeout);
}
}
lm
committed
void MAVLinkProtocol::setActionRetransmissionTimeout(int ms)
{
if (ms != m_actionRetransmissionTimeout) {
lm
committed
m_actionRetransmissionTimeout = ms;
emit actionRetransmissionTimeoutChanged(m_actionRetransmissionTimeout);
}
}
bool changed = false;
if (enabled != m_loggingEnabled) changed = true;
LM
committed
if (enabled)
{
if (m_logfile && m_logfile->isOpen())
{
m_logfile->flush();
m_logfile->close();
}
LM
committed
if (m_logfile)
{
if (!m_logfile->open(QIODevice::WriteOnly | QIODevice::Append))
{
emit protocolStatusMessage(tr("Opening MAVLink logfile for writing failed"), tr("MAVLink cannot log to the file %1, please choose a different file. Stopping logging.").arg(m_logfile->fileName()));
m_loggingEnabled = false;
}
LM
committed
else
{
emit protocolStatusMessage(tr("Opening MAVLink logfile for writing failed"), tr("MAVLink cannot start logging, no logfile selected."));
}
}
else if (!enabled)
{
if (m_logfile)
{
if (m_logfile->isOpen())
{
m_logfile->flush();
m_logfile->close();
}
}
if (changed) emit loggingChanged(enabled);
}
void MAVLinkProtocol::setLogfileName(const QString& filename)
{
LM
committed
if (!m_logfile)
{
m_logfile = new QFile(filename);
LM
committed
}
else
{
m_logfile->flush();
m_logfile->close();
}
m_logfile->setFileName(filename);
enableLogging(m_loggingEnabled);
void MAVLinkProtocol::enableVersionCheck(bool enabled)
{
m_enable_version_check = enabled;
emit versionCheckChanged(enabled);
/**
* The default rate is 1 Hertz.
*
* @param rate heartbeat rate in hertz (times per second)
*/
void MAVLinkProtocol::setHeartbeatRate(int rate)
{
heartbeatRate = rate;
heartbeatTimer->setInterval(1000/heartbeatRate);
}
/** @return heartbeat rate in Hertz */
int MAVLinkProtocol::getHeartbeatRate()
{
return heartbeatRate;
}