Commit 53c54b15 authored by lm's avatar lm

Added write retransmission

parent 743b753e
...@@ -37,6 +37,15 @@ quint64 groundTimeUsecs() ...@@ -37,6 +37,15 @@ quint64 groundTimeUsecs()
return static_cast<quint64>(microseconds + (time.time().msec()*1000)); return static_cast<quint64>(microseconds + (time.time().msec()*1000));
} }
quint64 groundTimeMilliseconds()
{
QDateTime time = QDateTime::currentDateTime();
time = time.toUTC();
/* Return seconds and milliseconds, in milliseconds unit */
quint64 seconds = time.toTime_t() * static_cast<quint64>(1000);
return static_cast<quint64>(seconds + (time.time().msec()));
}
float limitAngleToPMPIf(float angle) float limitAngleToPMPIf(float angle)
{ {
while (angle > ((float)M_PI+FLT_EPSILON)) while (angle > ((float)M_PI+FLT_EPSILON))
......
...@@ -23,6 +23,8 @@ namespace QGC ...@@ -23,6 +23,8 @@ namespace QGC
/** @brief Get the current ground time in microseconds */ /** @brief Get the current ground time in microseconds */
quint64 groundTimeUsecs(); quint64 groundTimeUsecs();
/** @brief Get the current ground time in milliseconds */
quint64 groundTimeMilliseconds();
/** @brief Returns the angle limited to -pi - pi */ /** @brief Returns the angle limited to -pi - pi */
float limitAngleToPMPIf(float angle); float limitAngleToPMPIf(float angle);
/** @brief Returns the angle limited to -pi - pi */ /** @brief Returns the angle limited to -pi - pi */
......
...@@ -884,6 +884,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) ...@@ -884,6 +884,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
//add data into datastream //add data into datastream
memcpy(stream+streampointer,buffer, bufferlength); memcpy(stream+streampointer,buffer, bufferlength);
streampointer+=bufferlength; streampointer+=bufferlength;
//qDebug() << "Sending PARAM" << key;
} }
else if (read.param_index < onboardParams.size()) else if (read.param_index < onboardParams.size())
{ {
...@@ -891,12 +892,13 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size) ...@@ -891,12 +892,13 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
float paramValue = onboardParams.value(key); float paramValue = onboardParams.value(key);
// Pack message and get size of encoded byte string // Pack message and get size of encoded byte string
mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key)); mavlink_msg_param_value_pack(read.target_system, componentId, &msg, (int8_t*)key.toStdString().c_str(), paramValue, onboardParams.size(), onboardParams.keys().indexOf(key));
// Allocate buffer with packet data // Allocate buffer with packet data
bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
//add data into datastream //add data into datastream
memcpy(stream+streampointer,buffer, bufferlength); memcpy(stream+streampointer,buffer, bufferlength);
streampointer+=bufferlength; streampointer+=bufferlength;
//qDebug() << "Sending PARAM #ID" << (read.param_index) << "KEY:" << key;
} }
} }
break; break;
......
...@@ -44,7 +44,6 @@ This file is part of the QGROUNDCONTROL project ...@@ -44,7 +44,6 @@ This file is part of the QGROUNDCONTROL project
*/ */
QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) : QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) :
QWidget(parent), QWidget(parent),
mav(uas), mav(uas),
components(new QMap<int, QTreeWidgetItem*>()), components(new QMap<int, QTreeWidgetItem*>()),
paramGroups(), paramGroups(),
...@@ -52,9 +51,10 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) : ...@@ -52,9 +51,10 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) :
parameters(), parameters(),
transmissionListMode(false), transmissionListMode(false),
transmissionActive(false), transmissionActive(false),
transmissionStarted(0), transmissionTimeout(0),
retransmissionTimeout(350), retransmissionTimeout(350),
rewriteTimeout(500) rewriteTimeout(500),
retransmissionBurstRequestSize(2)
{ {
// Load settings // Load settings
loadSettings(); loadSettings();
...@@ -210,6 +210,12 @@ void QGCParamWidget::addParameter(int uas, int component, int paramCount, int pa ...@@ -210,6 +210,12 @@ void QGCParamWidget::addParameter(int uas, int component, int paramCount, int pa
{ {
addParameter(uas, component, parameterName, value); addParameter(uas, component, parameterName, value);
// Missing packets list has to be instantiated for all components
if (!transmissionMissingPackets.contains(component))
{
transmissionMissingPackets.insert(component, new QList<int>());
}
// List mode is different from single parameter transfers // List mode is different from single parameter transfers
if (transmissionListMode) if (transmissionListMode)
{ {
...@@ -217,13 +223,8 @@ void QGCParamWidget::addParameter(int uas, int component, int paramCount, int pa ...@@ -217,13 +223,8 @@ void QGCParamWidget::addParameter(int uas, int component, int paramCount, int pa
// each component // each component
if (!transmissionListSizeKnown.contains(component)) if (!transmissionListSizeKnown.contains(component))
{ {
// If this was the first packet to announce the list size // Mark list size as known
// set all packets not received yet as missing to enforce transmissionListSizeKnown.insert(component, true);
// retransmission until full list is received
if (!transmissionMissingPackets.contains(component))
{
transmissionMissingPackets.insert(component, new QList<int>());
}
// Mark all parameters as missing // Mark all parameters as missing
for (int i = 0; i < paramCount; ++i) for (int i = 0; i < paramCount; ++i)
...@@ -234,8 +235,14 @@ void QGCParamWidget::addParameter(int uas, int component, int paramCount, int pa ...@@ -234,8 +235,14 @@ void QGCParamWidget::addParameter(int uas, int component, int paramCount, int pa
} }
} }
// Mark list size as known // There is only one transmission timeout for all components
transmissionListSizeKnown.insert(component, true); // since components do not manage their transmission,
// the longest timeout is safe for all components.
quint64 thisTransmissionTimeout = QGC::groundTimeMilliseconds() + ((paramCount/retransmissionBurstRequestSize+5)*retransmissionTimeout);
if (thisTransmissionTimeout > transmissionTimeout)
{
transmissionTimeout = thisTransmissionTimeout;
}
} }
// Start retransmission guard // Start retransmission guard
...@@ -243,18 +250,44 @@ void QGCParamWidget::addParameter(int uas, int component, int paramCount, int pa ...@@ -243,18 +250,44 @@ void QGCParamWidget::addParameter(int uas, int component, int paramCount, int pa
setRetransmissionGuardEnabled(true); setRetransmissionGuardEnabled(true);
} }
// Mark this parameter as received // Mark this parameter as received in read list
int index = transmissionMissingPackets.value(component)->indexOf(paramId); int index = transmissionMissingPackets.value(component)->indexOf(paramId);
// If the MAV sent the parameter without request, it wont be in missing list // If the MAV sent the parameter without request, it wont be in missing list
if (index != -1) transmissionMissingPackets.value(component)->removeAt(index); if (index != -1) transmissionMissingPackets.value(component)->removeAt(index);
bool justWritten = false;
bool writeMismatch = false;
// Mark this parameter as received in write ACK list
QMap<QString, float>* map = transmissionMissingWriteAckPackets.value(component);
if (map && map->contains(parameterName))
{
justWritten = true;
if (map->value(parameterName) != value)
{
writeMismatch = true;
}
map->remove(parameterName);
}
int missCount = 0; int missCount = 0;
foreach (int key, transmissionMissingPackets.keys()) foreach (int key, transmissionMissingPackets.keys())
{ {
missCount += transmissionMissingPackets.value(key)->count(); missCount += transmissionMissingPackets.value(key)->count();
} }
statusLabel->setText(tr("Got Param (#%1 of %5) %2: %3 (%4 missing)").arg(paramId+1).arg(parameterName).arg(value).arg(missCount).arg(paramCount)); if (justWritten && !writeMismatch)
{
statusLabel->setText(tr("SUCCESS: Wrote %2 (#%1/%4): %3").arg(paramId+1).arg(parameterName).arg(value).arg(paramCount));
}
else if (justWritten && writeMismatch)
{
// Mismatch, tell user
statusLabel->setText(tr("FAILURE: Wrote %1: sent %2 != onboard %3").arg(parameterName).arg(map->value(parameterName), value));
}
else
{
statusLabel->setText(tr("Got %2 (#%1/%5): %3 (%4 missing)").arg(paramId+1).arg(parameterName).arg(value).arg(missCount).arg(paramCount));
}
// Check if last parameter was received // Check if last parameter was received
if (missCount == 0) if (missCount == 0)
...@@ -417,7 +450,13 @@ void QGCParamWidget::requestParameterList() ...@@ -417,7 +450,13 @@ void QGCParamWidget::requestParameterList()
transmissionMissingPackets.value(key)->clear(); transmissionMissingPackets.value(key)->clear();
} }
transmissionActive = true; transmissionActive = true;
transmissionStarted = QGC::groundTimeUsecs();
// Set status text
statusLabel->setText(tr("Requested param list.. waiting"));
// Request twice as mean of forward error correction
mav->requestParameters();
QGC::SLEEP::msleep(10);
mav->requestParameters(); mav->requestParameters();
} }
...@@ -456,8 +495,8 @@ void QGCParamWidget::parameterItemChanged(QTreeWidgetItem* current, int column) ...@@ -456,8 +495,8 @@ void QGCParamWidget::parameterItemChanged(QTreeWidgetItem* current, int column)
// Check if the value was numerically changed // Check if the value was numerically changed
if (!parameters.value(key)->contains(str) || parameters.value(key)->value(str, 0.0f) != value) if (!parameters.value(key)->contains(str) || parameters.value(key)->value(str, 0.0f) != value)
{ {
current->setBackground(0, QBrush(QColor(QGC::colorGreen))); current->setBackground(0, QBrush(QColor(QGC::colorOrange)));
current->setBackground(1, QBrush(QColor(QGC::colorGreen))); current->setBackground(1, QBrush(QColor(QGC::colorOrange)));
} }
// All parameters list // All parameters list
...@@ -593,27 +632,54 @@ void QGCParamWidget::retransmissionGuardTick() ...@@ -593,27 +632,54 @@ void QGCParamWidget::retransmissionGuardTick()
if (transmissionActive) if (transmissionActive)
{ {
qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD ACTIVE, CHECKING FOR DROPS.."; qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD ACTIVE, CHECKING FOR DROPS..";
// Re-request at maximum five parameters at once
// to prevent link flooding
// Check for timeout
// stop retransmission attempts on timeout
if (QGC::groundTimeMilliseconds() > transmissionTimeout)
{
setRetransmissionGuardEnabled(false);
transmissionActive = false;
// Empty read retransmission list
// Empty write retransmission list
int missingReadCount = 0;
QList<int> readKeys = transmissionMissingPackets.keys();
foreach (int component, readKeys)
{
missingReadCount += transmissionMissingPackets.value(component)->count();
transmissionMissingPackets.value(component)->clear();
}
// Empty write retransmission list
int missingWriteCount = 0;
QList<int> writeKeys = transmissionMissingWriteAckPackets.keys();
foreach (int component, writeKeys)
{
missingWriteCount += transmissionMissingWriteAckPackets.value(component)->count();
transmissionMissingWriteAckPackets.value(component)->clear();
}
statusLabel->setText(tr("TIMEOUT! MISSING: %1 read, %2 write.").arg(missingReadCount).arg(missingWriteCount));
}
// Re-request at maximum retransmissionBurstRequestSize parameters at once
// to prevent link flooding
QMap<int, QMap<QString, float>*>::iterator i; QMap<int, QMap<QString, float>*>::iterator i;
for (i = parameters.begin(); i != parameters.end(); ++i) for (i = parameters.begin(); i != parameters.end(); ++i)
{ {
// Iterate through the parameters of the component // Iterate through the parameters of the component
int component = i.key(); int component = i.key();
// Request five parameters from this component (at maximum) // Request n parameters from this component (at maximum)
QList<int> * paramList = transmissionMissingPackets.value(component, NULL); QList<int> * paramList = transmissionMissingPackets.value(component, NULL);
if (paramList) if (paramList)
{ {
int count = 0; int count = 0;
int maxCount = 1;
foreach (int id, *paramList) foreach (int id, *paramList)
{ {
if (count < maxCount) if (count < retransmissionBurstRequestSize)
{ {
qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD REQUESTS RETRANSMISSION OF PARAM #" << id << "FROM COMPONENT #" << component; qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD REQUESTS RETRANSMISSION OF PARAM #" << id << "FROM COMPONENT #" << component;
emit requestParameter(component, id); emit requestParameter(component, id);
statusLabel->setText(tr("Requ. retransmission of #%1").arg(id)); statusLabel->setText(tr("Requested retransmission of #%1").arg(id+1));
count++; count++;
} }
else else
...@@ -623,6 +689,30 @@ void QGCParamWidget::retransmissionGuardTick() ...@@ -623,6 +689,30 @@ void QGCParamWidget::retransmissionGuardTick()
} }
} }
} }
// Re-request at maximum retransmissionBurstRequestSize parameters at once
// to prevent write-request link flooding
// Empty write retransmission list
QList<int> writeKeys = transmissionMissingWriteAckPackets.keys();
foreach (int component, writeKeys)
{
int count = 0;
QMap <QString, float>* missingParams = transmissionMissingWriteAckPackets.value(component);
foreach (QString key, missingParams->keys())
{
if (count < retransmissionBurstRequestSize)
{
// Re-request write operation
emit parameterChanged(component, key, missingParams->value(key));
statusLabel->setText(tr("Re-requested write: %1: %2").arg(key).arg(missingParams->value(key)));
count++;
}
else
{
break;
}
}
}
} }
else else
{ {
...@@ -640,23 +730,25 @@ void QGCParamWidget::retransmissionGuardTick() ...@@ -640,23 +730,25 @@ void QGCParamWidget::retransmissionGuardTick()
void QGCParamWidget::setParameter(int component, QString parameterName, float value) void QGCParamWidget::setParameter(int component, QString parameterName, float value)
{ {
emit parameterChanged(component, parameterName, value); emit parameterChanged(component, parameterName, value);
// // Wait for parameter to be written back // Wait for parameter to be written back
// // mark it therefore as missing // mark it therefore as missing
// if (!transmissionMissingPackets.contains(component)) if (!transmissionMissingWriteAckPackets.contains(component))
// { {
// transmissionMissingPackets.insert(component, new QList<int>()); transmissionMissingWriteAckPackets.insert(component, new QMap<QString, float>());
// } }
// for (int i = 0; i < paramCount; ++i) // Insert it in missing write ACK list
// { transmissionMissingWriteAckPackets.value(component)->insert(parameterName, value);
// if (!transmissionMissingPackets.value(component)->contains(i))
// { // Set timeouts
// transmissionMissingPackets.value(component)->append(i); transmissionActive = true;
// } quint64 newTransmissionTimeout = QGC::groundTimeMilliseconds() + 5*rewriteTimeout;
// } if (newTransmissionTimeout > transmissionTimeout)
// transmissionActive = true; {
// transmissionStarted = QGC::groundTimeUsecs(); transmissionTimeout = newTransmissionTimeout;
// setRetransmissionGuardEnabled(true); }
// Enable guard / reset timeouts
setRetransmissionGuardEnabled(true);
} }
/** /**
...@@ -682,7 +774,7 @@ void QGCParamWidget::setParameters() ...@@ -682,7 +774,7 @@ void QGCParamWidget::setParameters()
} }
} }
// Update stats label // Change transmission status if necessary
if (parametersSent == 0) if (parametersSent == 0)
{ {
statusLabel->setText(tr("No transmission: No changed values.")); statusLabel->setText(tr("No transmission: No changed values."));
...@@ -690,6 +782,15 @@ void QGCParamWidget::setParameters() ...@@ -690,6 +782,15 @@ void QGCParamWidget::setParameters()
else else
{ {
statusLabel->setText(tr("Transmitting %1 parameters.").arg(parametersSent)); statusLabel->setText(tr("Transmitting %1 parameters.").arg(parametersSent));
// Set timeouts
transmissionActive = true;
quint64 newTransmissionTimeout = QGC::groundTimeMilliseconds() + (parametersSent/retransmissionBurstRequestSize+5)*rewriteTimeout;
if (newTransmissionTimeout > transmissionTimeout)
{
transmissionTimeout = newTransmissionTimeout;
}
// Enable guard
setRetransmissionGuardEnabled(true);
} }
changedValues.clear(); changedValues.clear();
......
...@@ -94,14 +94,15 @@ protected: ...@@ -94,14 +94,15 @@ protected:
QMap<int, QMap<QString, float>* > parameters; ///< All parameters QMap<int, QMap<QString, float>* > parameters; ///< All parameters
QVector<bool> received; ///< Successfully received parameters QVector<bool> received; ///< Successfully received parameters
QMap<int, QList<int>* > transmissionMissingPackets; ///< Missing packets QMap<int, QList<int>* > transmissionMissingPackets; ///< Missing packets
QMap<int, QList<float>* > transmissionMissingWriteAckPackets; ///< Missing write ACK packets QMap<int, QMap<QString, float>* > transmissionMissingWriteAckPackets; ///< Missing write ACK packets
bool transmissionListMode; ///< Currently requesting list bool transmissionListMode; ///< Currently requesting list
QMap<int, bool> transmissionListSizeKnown; ///< List size initialized? QMap<int, bool> transmissionListSizeKnown; ///< List size initialized?
bool transmissionActive; ///< Missing packets, working on list? bool transmissionActive; ///< Missing packets, working on list?
quint64 transmissionStarted; ///< Timeout quint64 transmissionTimeout; ///< Timeout
QTimer retransmissionTimer; ///< Timer handling parameter retransmission QTimer retransmissionTimer; ///< Timer handling parameter retransmission
int retransmissionTimeout; ///< Retransmission request timeout, in milliseconds int retransmissionTimeout; ///< Retransmission request timeout, in milliseconds
int rewriteTimeout; ///< Write request timeout, in milliseconds int rewriteTimeout; ///< Write request timeout, in milliseconds
int retransmissionBurstRequestSize; ///< Number of packets requested for retransmission per burst
/** @brief Activate / deactivate parameter retransmission */ /** @brief Activate / deactivate parameter retransmission */
void setRetransmissionGuardEnabled(bool enabled); void setRetransmissionGuardEnabled(bool enabled);
......
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