Newer
Older
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Implementation of class QGCParamWidget
* @author Lorenz Meier <mail@qgroundcontrol.org>
*/
#include <QGridLayout>
#include <QPushButton>
#include <QFileDialog>
#include <QFile>
#include <QList>
#include "QGCParamWidget.h"
#include "UASInterface.h"
#include "UASParameterCommsMgr.h"
/**
* @param uas MAV to set the parameters on
* @param parent Parent widget
*/
QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) :
componentItems(new QMap<int, QTreeWidgetItem*>())
// Create tree widget
tree = new QTreeWidget(this);
tree->setColumnWidth(70, 30);
QGridLayout* horizontalLayout;
horizontalLayout = new QGridLayout(this);
Lorenz Meier
committed
horizontalLayout->setHorizontalSpacing(6);
horizontalLayout->setVerticalSpacing(6);
horizontalLayout->setSizeConstraint(QLayout::SetMinimumSize);
//horizontalLayout->setSizeConstraint( QLayout::SetFixedSize );
horizontalLayout->addWidget(tree, 0, 0, 1, 3);
horizontalLayout->addWidget(statusLabel, 1, 0, 1, 3);
// BUTTONS
QPushButton* refreshButton = new QPushButton(tr("Get"));
refreshButton->setToolTip(tr("Load parameters currently in non-permanent memory of aircraft."));
refreshButton->setWhatsThis(tr("Load parameters currently in non-permanent memory of aircraft."));
connect(refreshButton, SIGNAL(clicked()), this, SLOT(requestAllParamsUpdate()));
setButton->setToolTip(tr("Set current parameters in non-permanent onboard memory"));
setButton->setWhatsThis(tr("Set current parameters in non-permanent onboard memory"));
connect(setButton, SIGNAL(clicked()), this, SLOT(setParameters()));
QPushButton* writeButton = new QPushButton(tr("Write (ROM)"));
writeButton->setToolTip(tr("Copy current parameters in non-permanent memory of the aircraft to permanent memory. Transmit your parameters first to write these."));
writeButton->setWhatsThis(tr("Copy current parameters in non-permanent memory of the aircraft to permanent memory. Transmit your parameters first to write these."));
connect(writeButton, SIGNAL(clicked()), this, SLOT(writeParameters()));
QPushButton* loadFileButton = new QPushButton(tr("Load File"));
loadFileButton->setToolTip(tr("Load parameters from a file on this computer in the view. To write them to the aircraft, use transmit after loading them."));
loadFileButton->setWhatsThis(tr("Load parameters from a file on this computer in the view. To write them to the aircraft, use transmit after loading them."));
connect(loadFileButton, SIGNAL(clicked()), this, SLOT(loadParametersFromFile()));
QPushButton* saveFileButton = new QPushButton(tr("Save File"));
saveFileButton->setToolTip(tr("Save parameters in this view to a file on this computer."));
saveFileButton->setWhatsThis(tr("Save parameters in this view to a file on this computer."));
connect(saveFileButton, SIGNAL(clicked()), this, SLOT(saveParametersToFile()));
horizontalLayout->addWidget(saveFileButton, 3, 1);
QPushButton* readButton = new QPushButton(tr("Read (ROM)"));
readButton->setToolTip(tr("Copy parameters from permanent memory to non-permanent current memory of aircraft. DOES NOT update the parameters in this view, click refresh after copying them to get them."));
readButton->setWhatsThis(tr("Copy parameters from permanent memory to non-permanent current memory of aircraft. DOES NOT update the parameters in this view, click refresh after copying them to get them."));
connect(readButton, SIGNAL(clicked()), this, SLOT(readParameters()));
horizontalLayout->addWidget(readButton, 3, 2);
Lorenz Meier
committed
// Set correct vertical scaling
horizontalLayout->setRowStretch(0, 100);
horizontalLayout->setRowStretch(1, 10);
horizontalLayout->setRowStretch(2, 10);
horizontalLayout->setRowStretch(3, 10);
// Set layout
this->setLayout(horizontalLayout);
// Set header
QStringList headerItems;
headerItems.append("Parameter");
headerItems.append("Value");
tree->setHeaderLabels(headerItems);
tree->setColumnCount(2);
tree->setExpandsOnDoubleClick(true);
// Connect signals/slots
connect(this, SIGNAL(parameterChanged(int,QString,QVariant)), mav, SLOT(setParameter(int,QString,QVariant)));
connect(tree, SIGNAL(itemChanged(QTreeWidgetItem*,int)), this, SLOT(parameterItemChanged(QTreeWidgetItem*,int)));
// New parameters from UAS
connect(uas, SIGNAL(parameterChanged(int,int,int,int,QString,QVariant)), this, SLOT(receivedParameterUpdate(int,int,int,int,QString,QVariant)));
// connect(&retransmissionTimer, SIGNAL(timeout()), this, SLOT(retransmissionGuardTick()));
// connect(this, SIGNAL(requestParameter(int,QString)), uas, SLOT(requestParameter(int,QString)));
// connect(this, SIGNAL(requestParameter(int,int)), uas, SLOT(requestParameter(int,int)));
Lorenz Meier
committed
// Get parameters
Lorenz Meier
committed
void QGCParamWidget::addComponentItem( int compId, QString compName)
componentItems->value(compId)->setData(0, Qt::DisplayRole, QString("%1 (#%2)").arg(compName).arg(compId));
//components->value(component)->setData(1, Qt::DisplayRole, QString::number(component));
componentItems->value(compId)->setFirstColumnSpanned(true);
QStringList list(QString("%1 (#%2)").arg(compName).arg(compId));
QTreeWidgetItem* comp = new QTreeWidgetItem(list);
paramGroups.insert(compId, new QMap<QString, QTreeWidgetItem*>());
tree->addTopLevelItem(comp);
tree->update();
}
//TODO it seems unlikely that the UI would know about a component before the data model...
paramDataModel->addComponent(compId);
/**
* @param uas System which has the component
* @param component id of the component
* @param parameterName human friendly name of the parameter
*/
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
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
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
//void QGCParamWidget::receivedParameterUpdate(int uas, int component, int paramCount, int paramId, QString parameterName, QVariant value)
//{
// updateParameterDisplay(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
// if (transmissionListMode) {
// // Only accept the list size once on the first packet from
// // each component
// if (!transmissionListSizeKnown.contains(component))
// {
// // Mark list size as known
// transmissionListSizeKnown.insert(component, true);
// // Mark all parameters as missing
// for (int i = 0; i < paramCount; ++i)
// {
// if (!transmissionMissingPackets.value(component)->contains(i))
// {
// transmissionMissingPackets.value(component)->append(i);
// }
// }
// // There is only one transmission timeout for all components
// // since components do not manage their transmission,
// // the longest timeout is safe for all components.
// quint64 thisTransmissionTimeout = QGC::groundTimeMilliseconds() + ((paramCount)*retransmissionTimeout);
// if (thisTransmissionTimeout > transmissionTimeout)
// {
// transmissionTimeout = thisTransmissionTimeout;
// }
// }
// // Start retransmission guard
// // or reset timer
// paramCommsMgr->setRetransmissionGuardEnabled(true); //TODO
// }
// // Mark this parameter as received in read list
// int index = transmissionMissingPackets.value(component)->indexOf(paramId);
// // If the MAV sent the parameter without request, it wont be in missing list
// if (index != -1) transmissionMissingPackets.value(component)->removeAt(index);
// bool justWritten = false;
// bool writeMismatch = false;
// //bool lastWritten = false;
// // Mark this parameter as received in write ACK list
// QMap<QString, QVariant>* map = transmissionMissingWriteAckPackets.value(component);
// if (map && map->contains(parameterName))
// {
// justWritten = true;
// QVariant newval = map->value(parameterName);
// if (map->value(parameterName) != value)
// {
// writeMismatch = true;
// }
// map->remove(parameterName);
// }
// int missCount = 0;
// foreach (int key, transmissionMissingPackets.keys())
// {
// missCount += transmissionMissingPackets.value(key)->count();
// }
// int missWriteCount = 0;
// foreach (int key, transmissionMissingWriteAckPackets.keys())
// {
// missWriteCount += transmissionMissingWriteAckPackets.value(key)->count();
// }
// if (justWritten && !writeMismatch && missWriteCount == 0)
// {
// // Just wrote one and count went to 0 - this was the last missing write parameter
// statusLabel->setText(tr("SUCCESS: WROTE ALL PARAMETERS"));
// QPalette pal = statusLabel->palette();
// pal.setColor(backgroundRole(), QGC::colorGreen);
// statusLabel->setPalette(pal);
// } else if (justWritten && !writeMismatch)
// {
// statusLabel->setText(tr("SUCCESS: Wrote %2 (#%1/%4): %3").arg(paramId+1).arg(parameterName).arg(value.toDouble()).arg(paramCount));
// QPalette pal = statusLabel->palette();
// pal.setColor(backgroundRole(), QGC::colorGreen);
// statusLabel->setPalette(pal);
// } else if (justWritten && writeMismatch)
// {
// // Mismatch, tell user
// QPalette pal = statusLabel->palette();
// pal.setColor(backgroundRole(), QGC::colorRed);
// statusLabel->setPalette(pal);
// statusLabel->setText(tr("FAILURE: Wrote %1: sent %2 != onboard %3").arg(parameterName).arg(map->value(parameterName).toDouble()).arg(value.toDouble()));
// }
// else
// {
// if (missCount > 0)
// {
// QPalette pal = statusLabel->palette();
// pal.setColor(backgroundRole(), QGC::colorOrange);
// statusLabel->setPalette(pal);
// }
// else
// {
// QPalette pal = statusLabel->palette();
// pal.setColor(backgroundRole(), QGC::colorGreen);
// statusLabel->setPalette(pal);
// }
// QString val = QString("%1").arg(value.toFloat(), 5, 'f', 1, QChar(' '));
// //statusLabel->setText(tr("OK: %1 %2 #%3/%4, %5 miss").arg(parameterName).arg(val).arg(paramId+1).arg(paramCount).arg(missCount));
// if (missCount == 0)
// {
// // Transmission done
// QTime time = QTime::currentTime();
// QString timeString = time.toString();
// statusLabel->setText(tr("All received. (updated at %1)").arg(timeString));
// }
// else
// {
// // Transmission in progress
// statusLabel->setText(tr("OK: %1 %2 (%3/%4)").arg(parameterName).arg(val).arg(paramCount-missCount).arg(paramCount));
// }
// }
// // Check if last parameter was received
// if (missCount == 0 && missWriteCount == 0)
// {
// this->transmissionActive = false;
// this->transmissionListMode = false;
// transmissionListSizeKnown.clear();
// foreach (int key, transmissionMissingPackets.keys())
// {
// transmissionMissingPackets.value(key)->clear();
// }
// // Expand visual tree
// tree->expandItem(tree->topLevelItem(0));
// }
//}
void QGCParamWidget::handleParameterUpdate(int componentId, int paramCount, int paramId, const QString& paramName, QVariant value)
Q_UNUSED(paramCount);
Q_UNUSED(paramId);
updateParameterDisplay(componentId, paramName, value);
}
Lorenz Meier
committed
void QGCParamWidget::handleParameterListUpToDate(int component)
{
// Expand visual tree
tree->expandItem(tree->topLevelItem(0));
void QGCParamWidget::updateParameterDisplay(int componentId, QString parameterName, QVariant value)
{
// QString ptrStr;
// ptrStr.sprintf("%8p", this);
// qDebug() << "QGCParamWidget " << ptrStr << " got param" << parameterName;
QTreeWidgetItem* parameterItem = NULL;
if (!componentItems->contains(componentId)) {
QString componentName = tr("Component #%1").arg(componentId);
//TODO this should be bubbling up from the model, not vice-versa, right?
// // Replace value in data model
// paramDataModel->handleParameterUpdate(componentId,parameterName,value);
QString splitToken = "_";
// Check if auto-grouping can work
QString parent = parameterName.section(splitToken, 0, 0, QString::SectionSkipEmpty);
QMap<QString, QTreeWidgetItem*>* compParamGroups = paramGroups.value(componentId);
if (!compParamGroups->contains(parent))
{
// Insert group item
QStringList glist;
glist.append(parent);
QTreeWidgetItem* item = new QTreeWidgetItem(glist);
compParamGroups->insert(parent, item);
componentItems->value(componentId)->addChild(item);
// Append child to group
bool found = false;
QTreeWidgetItem* parentItem = compParamGroups->value(parent);
for (int i = 0; i < parentItem->childCount(); i++) {
QTreeWidgetItem* child = parentItem->child(i);
QString key = child->data(0, Qt::DisplayRole).toString();
//qDebug() << "UPDATED CHILD";
parameterItem = child;
parameterItem->setData(1, Qt::DisplayRole, value.toUInt());
}
parameterItem->setData(1, Qt::DisplayRole, value);
}
found = true;
}
}
// Insert parameter into map
QStringList plist;
plist.append(parameterName);
parameterItem = new QTreeWidgetItem(plist);
parameterItem->setData(1, Qt::DisplayRole, value.toUInt());
}
parameterItem->setData(1, Qt::DisplayRole, value);
}
compParamGroups->value(parent)->addChild(parameterItem);
parameterItem->setFlags(parameterItem->flags() | Qt::ItemIsEditable);
QTreeWidgetItem* parent = componentItems->value(componentId);
for (int i = 0; i < parent->childCount(); i++) {
QTreeWidgetItem* child = parent->child(i);
QString key = child->data(0, Qt::DisplayRole).toString();
parameterItem = child;
parameterItem->setData(1, Qt::DisplayRole, value);
// Insert parameter into map
QStringList plist;
plist.append(parameterName);
parameterItem = new QTreeWidgetItem(plist);
parameterItem->setData(1, Qt::DisplayRole, value);
componentItems->value(componentId)->addChild(parameterItem);
parameterItem->setFlags(parameterItem->flags() | Qt::ItemIsEditable);
Lorenz Meier
committed
parameterItem->setBackground(0, Qt::NoBrush);
parameterItem->setBackground(1, Qt::NoBrush);
QString paramDesc = paramDataModel->getParamDescription(parameterName);
if (paramDataModel->isParamDefaultKnown(parameterName)) {
double paramDefValue = paramDataModel->getParamDefault(parameterName);
tooltipFormat = tooltipFormat.arg(paramDefValue).arg(paramDesc);
else {
tooltipFormat = paramDesc;
}
parameterItem->setToolTip(0, tooltipFormat);
parameterItem->setToolTip(1, tooltipFormat);
//paramDataModel->handleParameterUpdate(componentId,parameterName,value);
void QGCParamWidget::parameterItemChanged(QTreeWidgetItem* current, int column)
QTreeWidgetItem* parent = current->parent();
while (parent->parent() != NULL) {
parent = parent->parent();
}
// Parent is now top-level component
int componentId = componentItems->key(parent);
QString key = current->data(0, Qt::DisplayRole).toString();
QVariant value = current->data(1, Qt::DisplayRole);
// Set parameter on changed list to be transmitted to MAV
QPalette pal = statusLabel->palette();
pal.setColor(backgroundRole(), QGC::colorOrange);
statusLabel->setPalette(pal);
statusLabel->setText(tr("Transmit pend. %1:%2: %3").arg(componentId).arg(key).arg(value.toFloat(), 5, 'f', 1, QChar(' ')));
//qDebug() << "PARAM CHANGED: COMP:" << key << "KEY:" << str << "VALUE:" << value;
// Changed values list
bool changed = paramDataModel->addPendingIfParameterChanged(componentId,key,value);
// If the value was numerically changed, display it differently
if (changed) {
current->setBackground(0, QBrush(QColor(QGC::colorOrange)));
current->setBackground(1, QBrush(QColor(QGC::colorOrange)));
//TODO this seems incorrect-- we're pre-updating the onboard value before we've received confirmation
//paramDataModel->setOnboardParameterWithType(componentId,key,value);
void QGCParamWidget::saveParametersToFile()
QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./parameters.txt", tr("Parameter File (*.txt)"));
QFile file(fileName);
if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
QTextStream outstream(&file);
paramDataModel->writeOnboardParametersToStream(outstream,mav->getUASName());
void QGCParamWidget::loadParametersFromFile()
QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Parameter file (*.txt)"));
QFile file(fileName);
if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
return;
QTextStream in(&file);
paramDataModel->readUpdateParametersFromStream(in);
void QGCParamWidget::setParameterStatusMsg(const QString& msg)
void QGCParamWidget::requestAllParamsUpdate()
// Clear view and request param list
clear();
requestParameterList();
/**
* Set all parameter in the parameter tree on the MAV
*/
void QGCParamWidget::setParameters()
{
lm
committed
/**
* Write the current onboard parameters from RAM into
* permanent storage, e.g. EEPROM or harddisk
lm
committed
*/
lm
committed
{
int changedParamCount = 0;
QMap<int, QMap<QString, QVariant>*>::iterator i;
QMap<int, QMap<QString, QVariant>*> changedValues = paramDataModel->getPendingParameters();
for (i = changedValues.begin(); i != changedValues.end() , (0 == changedParamCount); ++i) {
QMap<QString, QVariant>* compPending = i.value();
changedParamCount += compPending->count();
lm
committed
QMessageBox msgBox;
msgBox.setText(tr("There are locally changed parameters. Please transmit them first (<TRANSMIT>) or update them with the onboard values (<REFRESH>) before storing onboard from RAM to ROM."));
msgBox.exec();
}
else {
paramCommsMgr->writeParamsToPersistentStorage();
lm
committed
/**
* @param component the subsystem which has the parameter
* @param parameterName name of the parameter, as delivered by the system
* @param value value of the parameter
*/
void QGCParamWidget::setParameter(int component, QString parameterName, QVariant value)
{
if (parameterName.isEmpty()) {
return;
}
if (paramDataModel->isValueLessThanParamMin(parameterName,dblValue)) {
statusLabel->setText(tr("REJ. %1, %2 < min").arg(parameterName).arg(dblValue));
if (paramDataModel->isValueGreaterThanParamMax(parameterName,dblValue)) {
statusLabel->setText(tr("REJ. %1, %2 > max").arg(parameterName).arg(dblValue));
QVariant onboardVal;
paramDataModel->getOnboardParameterValue(component,parameterName,onboardVal);
if (onboardVal == value) {
statusLabel->setText(tr("REJ. %1 already %2").arg(parameterName).arg(dblValue));
//int paramType = (int)onboardParameters.value(component)->value(parameterName).type();
int paramType = (int)value.type();
switch (paramType)
case QVariant::Char:
{
QVariant fixedValue(QChar((unsigned char)value.toInt()));
emit parameterChanged(component, parameterName, fixedValue);
//qDebug() << "PARAM WIDGET SENT:" << fixedValue;
}
break;
{
QVariant fixedValue(value.toInt());
emit parameterChanged(component, parameterName, fixedValue);
//qDebug() << "PARAM WIDGET SENT:" << fixedValue;
}
{
QVariant fixedValue(value.toUInt());
emit parameterChanged(component, parameterName, fixedValue);
//qDebug() << "PARAM WIDGET SENT:" << fixedValue;
}
{
QVariant fixedValue(value.toFloat());
emit parameterChanged(component, parameterName, fixedValue);
//qDebug() << "PARAM WIDGET SENT:" << fixedValue;
}
break;
default:
qCritical() << "ABORTED PARAM SEND, NO VALID QVARIANT TYPE";
return;
}
// Wait for parameter to be written back
// mark it therefore as missing
if (!transmissionMissingWriteAckPackets.contains(component))
{
transmissionMissingWriteAckPackets.insert(component, new QMap<QString, QVariant>());
}
// Insert it in missing write ACK list
transmissionMissingWriteAckPackets.value(component)->insert(parameterName, value);
// Set timeouts
if (transmissionActive)
{
transmissionTimeout += rewriteTimeout;
}
else
{
quint64 newTransmissionTimeout = QGC::groundTimeMilliseconds() + rewriteTimeout;
if (newTransmissionTimeout > transmissionTimeout)
{
transmissionTimeout = newTransmissionTimeout;
}
transmissionActive = true;
paramCommsMgr->setRetransmissionGuardEnabled(true); //TODO
}
void QGCParamWidget::readParameters()
{
}
/**
* Clear all data in the parameter widget
*/
void QGCParamWidget::clear()
{
tree->clear();