Commit 6fbc9b42 authored by tstellanova's avatar tstellanova

Retransmit param list request; fix null data model

parent d8e54be4
......@@ -285,6 +285,7 @@ class BandMatrixWrapper : public BandMatrixBase<BandMatrixWrapper<_CoefficientsT
m_rows(rows), m_supers(supers), m_subs(subs)
{
//internal::assert(coeffs.cols()==cols() && (supers()+subs()+1)==coeffs.rows());
cols=0; //workaround for compiler warning
}
/** \returns the number of columns */
......
......@@ -234,6 +234,8 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
*/
UAS::~UAS()
{
delete paramCommsMgr;
delete paramDataModel;
writeSettings();
delete links;
delete statusTimeout;
......@@ -2029,8 +2031,10 @@ void UAS::forwardMessage(mavlink_message_t message)
{
if(serial != links->at(i))
{
qDebug()<<"Antenna tracking: Forwarding Over link: "<<serial->getName()<<" "<<serial;
sendMessage(serial, message);
if (link->isConnected()) {
qDebug()<<"Antenna tracking: Forwarding Over link: "<<serial->getName()<<" "<<serial;
sendMessage(serial, message);
}
}
}
}
......@@ -2541,7 +2545,7 @@ void UAS::setParameter(const int component, const QString& id, const QVariant& v
// TODO: This is a hack for MAV_AUTOPILOT_ARDUPILOTMEGA until the new version of MAVLink and a fix for their param handling.
if (getAutopilotType() == MAV_AUTOPILOT_ARDUPILOTMEGA)
{
switch (value.type())
switch ((int)value.type())
{
case QVariant::Char:
union_value.param_float = (unsigned char)value.toChar().toAscii();
......@@ -2566,7 +2570,7 @@ void UAS::setParameter(const int component, const QString& id, const QVariant& v
}
else
{
switch (value.type())
switch ((int)value.type())
{
case QVariant::Char:
union_value.param_int8 = (unsigned char)value.toChar().toAscii();
......
......@@ -10,6 +10,7 @@
UASParameterCommsMgr::UASParameterCommsMgr(QObject *parent, UASInterface *uas) :
QObject(parent),
mav(uas),
paramDataModel(NULL),
transmissionListMode(false),
transmissionActive(false),
transmissionTimeout(0),
......@@ -17,7 +18,7 @@ UASParameterCommsMgr::UASParameterCommsMgr(QObject *parent, UASInterface *uas) :
rewriteTimeout(500),
retransmissionBurstRequestSize(5)
{
mav = uas;
paramDataModel = mav->getParamDataModel();
loadParamCommsSettings();
......@@ -57,6 +58,8 @@ void UASParameterCommsMgr::loadParamCommsSettings()
settings.endGroup();
}
/**
* Send a request to deliver the list of onboard parameters
* from the MAV.
......@@ -82,7 +85,9 @@ void UASParameterCommsMgr::requestParameterList()
transmissionActive = true;
setParameterStatusMsg(tr("Requested param list.. waiting"));
listRecvTimeout = QGC::groundTimeMilliseconds() + 10000;
mav->requestParameters();
setRetransmissionGuardEnabled(true);
}
else {
qDebug() << __FILE__ << __LINE__ << "Ignoring requestParameterList because we're receiving params list";
......@@ -94,6 +99,17 @@ void UASParameterCommsMgr::requestParameterList()
void UASParameterCommsMgr::retransmissionGuardTick()
{
if (transmissionActive) {
if (transmissionListMode && transmissionListSizeKnown.isEmpty() ) {
//we are still waitin for the first parameter list response
if (QGC::groundTimeMilliseconds() > this->listRecvTimeout) {
//re-request parameters
setParameterStatusMsg(tr("TIMEOUT: Re-requesting param list"),ParamCommsStatusLevel_Warning);
listRecvTimeout = QGC::groundTimeMilliseconds() + 10000;
mav->requestParameters();
}
return;
}
qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD ACTIVE, CHECKING FOR DROPS..";
// Check for timeout
......@@ -466,11 +482,9 @@ void UASParameterCommsMgr::receivedParameterUpdate(int uas, int compId, int para
transmissionMissingPackets.value(key)->clear();
}
setRetransmissionGuardEnabled(false);
//all parameters have been received, broadcast to UI
emit parameterListUpToDate();
//TODO in UI
// Expand visual tree
//tree->expandItem(tree->topLevelItem(0));
}
}
......@@ -527,5 +541,10 @@ void UASParameterCommsMgr::sendPendingParameters()
UASParameterCommsMgr::~UASParameterCommsMgr()
{
setRetransmissionGuardEnabled(false);
QString ptrStr;
ptrStr.sprintf("%8p", this);
qDebug() << "UASParameterCommsMgr destructor: " << ptrStr ;
}
......@@ -98,6 +98,7 @@ protected:
int retransmissionTimeout; ///< Retransmission request timeout, in milliseconds
int rewriteTimeout; ///< Write request timeout, in milliseconds
int retransmissionBurstRequestSize; ///< Number of packets requested for retransmission per burst
quint64 listRecvTimeout; ///< How long to wait for first parameter list response before re-requesting
// Status
QString parameterStatusMsg;
......
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