Commit 6f920c2d authored by lm's avatar lm

Working on debug console auto hold feature, added param retransmission

parent a97c4405
......@@ -825,6 +825,8 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
// Iterate through all components / subsystems
int j = 0;
for (i = onboardParams.begin(); i != onboardParams.end(); ++i)
{
if (j != 5)
{
// Pack message and get size of encoded byte string
mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)i.key().toStdString().c_str(), i.value(), onboardParams.size(), j);
......@@ -833,6 +835,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
//add data into datastream
memcpy(stream+streampointer,buffer, bufferlength);
streampointer+=bufferlength;
}
j++;
}
......@@ -854,7 +857,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
onboardParams.insert(key, set.param_value);
// Pack message and get size of encoded byte string
mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)key.toStdString().c_str(), set.param_value, onboardParams.size(), 0);
mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)key.toStdString().c_str(), set.param_value, onboardParams.size(), onboardParams.keys().indexOf(key));
// Allocate buffer with packet data
bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
//add data into datastream
......@@ -863,15 +866,48 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
}
}
}
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
{
qDebug() << "SIMULATION RECEIVED COMMAND TO SEND PARAMETER";
mavlink_param_request_read_t read;
mavlink_msg_param_request_read_decode(&msg, &read);
QByteArray bytes((char*)read.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
QString key = QString(bytes);
if (onboardParams.contains(key))
{
float paramValue = onboardParams.value(key);
// 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));
// Allocate buffer with packet data
bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
//add data into datastream
memcpy(stream+streampointer,buffer, bufferlength);
streampointer+=bufferlength;
}
else if (read.param_index < onboardParams.size())
{
key = onboardParams.keys().at(read.param_index);
float paramValue = onboardParams.value(key);
// 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));
// Allocate buffer with packet data
bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
//add data into datastream
memcpy(stream+streampointer,buffer, bufferlength);
streampointer+=bufferlength;
}
}
break;
}
}
//unsigned char v=data[i];
//fprintf(stderr,"%02x ", v);
unsigned char v=data[i];
fprintf(stderr,"%02x ", v);
}
//fprintf(stderr,"\n");
fprintf(stderr,"\n");
readyBufferMutex.lock();
for (int i = 0; i < streampointer; i++)
......@@ -903,22 +939,22 @@ void MAVLinkSimulationLink::readBytes() {
readyBufferMutex.unlock();
// if (len > 0)
// {
// qDebug() << "Simulation sent " << len << " bytes to groundstation: ";
//
// /* Increase write counter */
// //bitsSentTotal += size * 8;
//
// //Output all bytes as hex digits
// int i;
// for (i=0; i<len; i++)
// {
// unsigned int v=data[i];
// fprintf(stderr,"%02x ", v);
// }
// fprintf(stderr,"\n");
// }
// if (len > 0)
// {
// qDebug() << "Simulation sent " << len << " bytes to groundstation: ";
// /* Increase write counter */
// //bitsSentTotal += size * 8;
// //Output all bytes as hex digits
// int i;
// for (i=0; i<len; i++)
// {
// unsigned int v=data[i];
// fprintf(stderr,"%02x ", v);
// }
// fprintf(stderr,"\n");
// }
}
/**
......
......@@ -535,9 +535,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit valueChanged(uasId, "latitude", "deg", latitude, time);
emit valueChanged(uasId, "longitude", "deg", longitude, time);
emit valueChanged(uasId, "altitude", "m", altitude, time);
emit valueChanged(uasId, "gps x speed", "m/s", speedX, time);
emit valueChanged(uasId, "gps y speed", "m/s", speedY, time);
emit valueChanged(uasId, "gps z speed", "m/s", speedZ, time);
double totalSpeed = sqrt(speedX*speedX + speedY*speedY + speedZ*speedZ);
emit valueChanged(uasId, "gps speed", "m/s", totalSpeed, time);
emit globalPositionChanged(this, longitude, latitude, altitude, time);
emit speedChanged(this, speedX, speedY, speedZ, time);
// Set internal state
......@@ -565,12 +564,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit valueChanged(uasId, "latitude", "deg", latitude, time);
emit valueChanged(uasId, "longitude", "deg", longitude, time);
emit valueChanged(uasId, "altitude", "m", altitude, time);
emit valueChanged(uasId, "gps x speed", "m/s", speedX, time);
emit valueChanged(uasId, "gps y speed", "m/s", speedY, time);
emit valueChanged(uasId, "gps z speed", "m/s", speedZ, time);
double totalSpeed = sqrt(speedX*speedX + speedY*speedY + speedZ*speedZ);
emit valueChanged(uasId, "gps speed", "m/s", totalSpeed, time);
emit globalPositionChanged(this, longitude, latitude, altitude, time);
emit speedChanged(this, speedX, speedY, speedZ, time);
emit valueChanged(uasId, "gpsspeed", "m/s", sqrt(speedX*speedX+speedY*speedY+speedZ*speedZ), time);
// Set internal state
if (!positionLock)
{
......@@ -600,7 +597,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
if (pos.fix_type > 0)
{
emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time);
emit valueChanged(uasId, "gpsspeed", "m/s", pos.v, time);
emit valueChanged(uasId, "gps speed", "m/s", pos.v, time);
// Check for NaN
int alt = pos.alt;
......@@ -1515,11 +1512,11 @@ void UAS::setParameter(const int component, const QString& id, const float value
{
p.param_id[i] = id.toAscii()[i];
}
// Null termination at end of string or end of buffer
else if ((int)i == id.length() || i == (sizeof(p.param_id) - 1))
{
p.param_id[i] = '\0';
}
// // Null termination at end of string or end of buffer
// else if ((int)i == id.length() || i == (sizeof(p.param_id) - 1))
// {
// p.param_id[i] = '\0';
// }
// Zero fill
else
{
......@@ -1531,6 +1528,18 @@ void UAS::setParameter(const int component, const QString& id, const float value
}
}
void UAS::requestParameter(int component, int parameter)
{
mavlink_message_t msg;
mavlink_param_request_read_t read;
read.param_index = parameter;
read.target_system = uasId;
read.target_component = component;
mavlink_msg_param_request_read_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &read);
sendMessage(msg);
qDebug() << __FILE__ << __LINE__ << "REQUESTING PARAM RETRANSMISSION FROM COMPONENT" << component << "FOR PARAM ID" << parameter;
}
void UAS::setSystemType(int systemType)
{
type = systemType;
......
......@@ -259,6 +259,9 @@ public slots:
/** @brief Request all parameters */
void requestParameters();
/** @brief Request a single parameter by index */
void requestParameter(int component, int parameter);
/** @brief Set a system parameter */
void setParameter(const int component, const QString& id, const float value);
......
......@@ -226,6 +226,8 @@ public slots:
virtual void setLocalOriginAtCurrentGPSPosition() = 0;
/** @brief Request all onboard parameters of all components */
virtual void requestParameters() = 0;
/** @brief Request one specific onboard parameter */
virtual void requestParameter(int component, int parameter) = 0;
/** @brief Write parameter to permanent storage */
virtual void writeParametersToStorage() = 0;
/** @brief Read parameter from permanent storage */
......
......@@ -85,10 +85,10 @@ DebugConsole::DebugConsole(QWidget *parent) :
snapShotTimer.setInterval(snapShotInterval);
snapShotTimer.start();
// Set hex checkbox checked
m_ui->hexCheckBox->setChecked(!convertToAscii);
m_ui->mavlinkCheckBox->setChecked(filterMAVLINK);
m_ui->holdCheckBox->setChecked(autoHold);
// // Set hex checkbox checked
// m_ui->hexCheckBox->setChecked(!convertToAscii);
// m_ui->mavlinkCheckBox->setChecked(filterMAVLINK);
// m_ui->holdCheckBox->setChecked(autoHold);
// Get a list of all existing links
links = QList<LinkInterface*>();
......@@ -120,8 +120,6 @@ DebugConsole::DebugConsole(QWidget *parent) :
// Allow to send via return
connect(m_ui->sendText, SIGNAL(returnPressed()), this, SLOT(sendBytes()));
hold(false);
loadSettings();
// Warn user about not activated hold
......@@ -146,8 +144,8 @@ void DebugConsole::loadSettings()
m_ui->specialComboBox->setCurrentIndex(settings.value("SPECIAL_SYMBOL", m_ui->specialComboBox->currentIndex()).toInt());
m_ui->specialCheckBox->setChecked(settings.value("SPECIAL_SYMBOL_CHECKBOX_STATE", m_ui->specialCheckBox->isChecked()).toBool());
hexModeEnabled(settings.value("HEX_MODE_ENABLED", m_ui->hexCheckBox->isChecked()).toBool());
MAVLINKfilterEnabled(settings.value("MAVLINK_FILTER_ENABLED", m_ui->mavlinkCheckBox->isChecked()).toBool());
setAutoHold(settings.value("AUTO_HOLD_ENABLED", m_ui->holdCheckBox->isChecked()).toBool());
MAVLINKfilterEnabled(settings.value("MAVLINK_FILTER_ENABLED", filterMAVLINK).toBool());
setAutoHold(settings.value("AUTO_HOLD_ENABLED", autoHold).toBool());
settings.endGroup();
// Update visibility settings
......@@ -166,8 +164,8 @@ void DebugConsole::storeSettings()
settings.setValue("SPECIAL_SYMBOL", m_ui->specialComboBox->currentIndex());
settings.setValue("SPECIAL_SYMBOL_CHECKBOX_STATE", m_ui->specialCheckBox->isChecked());
settings.setValue("HEX_MODE_ENABLED", m_ui->hexCheckBox->isChecked());
settings.setValue("MAVLINK_FILTER_ENABLED", m_ui->mavlinkCheckBox->isChecked());
settings.setValue("AUTO_HOLD_ENABLED", m_ui->holdCheckBox->isChecked());
settings.setValue("MAVLINK_FILTER_ENABLED", filterMAVLINK);
settings.setValue("AUTO_HOLD_ENABLED", autoHold);
settings.endGroup();
settings.sync();
//qDebug() << "Storing settings!";
......@@ -242,8 +240,16 @@ void DebugConsole::setAutoHold(bool hold)
this->hold(false);
m_ui->holdButton->setChecked(false);
}
// Set auto hold checkbox
if (m_ui->holdCheckBox->isChecked() != hold)
{
m_ui->holdCheckBox->setChecked(hold);
}
// Set new state
autoHold = hold;
// FIXME REMOVE THIS HERE
storeSettings();
}
/**
......@@ -699,6 +705,8 @@ void DebugConsole::hexModeEnabled(bool mode)
m_ui->sentText->clear();
commandHistory.clear();
}
// FIXME REMOVE THIS HERE
storeSettings();
}
/**
......@@ -715,6 +723,8 @@ void DebugConsole::MAVLINKfilterEnabled(bool filter)
m_ui->mavlinkCheckBox->setChecked(filter);
}
}
// FIXME REMOVE THIS HERE
storeSettings();
}
/**
* @param hold Freeze the input and thus any scrolling
......
......@@ -50,6 +50,9 @@
<property name="toolTip">
<string>Ignore MAVLINK protocol messages in display</string>
</property>
<property name="statusTip">
<string>Ignore MAVLINK protocol messages in display</string>
</property>
<property name="text">
<string>Hide MAVLink</string>
</property>
......@@ -58,7 +61,10 @@
<item>
<widget class="QCheckBox" name="hexCheckBox">
<property name="toolTip">
<string>Display and send bytes in HEX representation</string>
<string>Display and enter bytes in HEX representation (e.g. 0xAA)</string>
</property>
<property name="statusTip">
<string>Display and enter bytes in HEX representation (e.g. 0xAA)</string>
</property>
<property name="text">
<string>HEX</string>
......
......@@ -391,7 +391,7 @@ void MainWindow::buildCommonWidgets()
if (!dataplotWidget)
{
dataplotWidget = new QGCDataPlot2D(this);
addToCentralWidgetsMenu (dataplotWidget, "Data Plot", SLOT(showCentralWidget()),CENTRAL_DATA_PLOT);
addToCentralWidgetsMenu (dataplotWidget, "Logfile Plot", SLOT(showCentralWidget()),CENTRAL_DATA_PLOT);
}
}
......@@ -431,27 +431,27 @@ void MainWindow::buildPxWidgets()
{
// Center widgets
linechartWidget = new Linecharts(this);
addToCentralWidgetsMenu(linechartWidget, "Line Plots", SLOT(showCentralWidget()), CENTRAL_LINECHART);
addToCentralWidgetsMenu(linechartWidget, tr("Realtime Plot"), SLOT(showCentralWidget()), CENTRAL_LINECHART);
}
if (!hudWidget)
{
hudWidget = new HUD(320, 240, this);
addToCentralWidgetsMenu(hudWidget, "HUD", SLOT(showCentralWidget()), CENTRAL_HUD);
addToCentralWidgetsMenu(hudWidget, tr("Head Up Display"), SLOT(showCentralWidget()), CENTRAL_HUD);
}
if (!dataplotWidget)
{
dataplotWidget = new QGCDataPlot2D(this);
addToCentralWidgetsMenu(dataplotWidget, "Data Plots", SLOT(showCentralWidget()), CENTRAL_DATA_PLOT);
addToCentralWidgetsMenu(dataplotWidget, "Logfile Plot", SLOT(showCentralWidget()), CENTRAL_DATA_PLOT);
}
#ifdef QGC_OSG_ENABLED
if (!_3DWidget)
{
_3DWidget = Q3DWidgetFactory::get("PIXHAWK");
addToCentralWidgetsMenu(_3DWidget, "Local 3D", SLOT(showCentralWidget()), CENTRAL_3D_LOCAL);
addToCentralWidgetsMenu(_3DWidget, tr("Local 3D"), SLOT(showCentralWidget()), CENTRAL_3D_LOCAL);
}
#endif
......@@ -459,7 +459,7 @@ void MainWindow::buildPxWidgets()
if (!_3DMapWidget)
{
_3DMapWidget = Q3DWidgetFactory::get("MAP3D");
addToCentralWidgetsMenu(_3DMapWidget, "OSG Earth 3D", SLOT(showCentralWidget()), CENTRAL_OSGEARTH);
addToCentralWidgetsMenu(_3DMapWidget, tr("OSG Earth 3D"), SLOT(showCentralWidget()), CENTRAL_OSGEARTH);
}
#endif
......@@ -467,7 +467,7 @@ void MainWindow::buildPxWidgets()
if (!gEarthWidget)
{
gEarthWidget = new QGCGoogleEarthView(this);
addToCentralWidgetsMenu(gEarthWidget, "Google Earth", SLOT(showCentralWidget()), CENTRAL_GOOGLE_EARTH);
addToCentralWidgetsMenu(gEarthWidget, tr("Google Earth"), SLOT(showCentralWidget()), CENTRAL_GOOGLE_EARTH);
}
#endif
......@@ -503,7 +503,7 @@ void MainWindow::buildPxWidgets()
hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this);
hsiDockWidget->setWidget( new HSIDisplay(this) );
hsiDockWidget->setObjectName("HORIZONTAL_SITUATION_INDICATOR_DOCK_WIDGET");
addToToolsMenu (hsiDockWidget, tr("HSI"), SLOT(showToolWidget(bool)), MENU_HSI, Qt::BottomDockWidgetArea);
addToToolsMenu (hsiDockWidget, tr("Horizontal Situation"), SLOT(showToolWidget(bool)), MENU_HSI, Qt::BottomDockWidgetArea);
}
if (!headDown1DockWidget)
......@@ -572,6 +572,7 @@ void MainWindow::buildSlugsWidgets()
{
// Center widgets
linechartWidget = new Linecharts(this);
addToCentralWidgetsMenu(linechartWidget, tr("Realtime Plot"), SLOT(showCentralWidget()), CENTRAL_LINECHART);
}
if (!headUpDockWidget)
......@@ -580,7 +581,7 @@ void MainWindow::buildSlugsWidgets()
headUpDockWidget = new QDockWidget(tr("Control Indicator"), this);
headUpDockWidget->setWidget( new HUD(320, 240, this));
headUpDockWidget->setObjectName("HEAD_UP_DISPLAY_DOCK_WIDGET");
addToToolsMenu (headUpDockWidget, tr("HUD"), SLOT(showToolWidget(bool)), MENU_HUD, Qt::LeftDockWidgetArea);
addToToolsMenu (headUpDockWidget, tr("Head Up Display"), SLOT(showToolWidget(bool)), MENU_HUD, Qt::LeftDockWidgetArea);
}
if (!rcViewDockWidget)
......
......@@ -44,12 +44,6 @@ ParameterInterface::ParameterInterface(QWidget *parent) :
m_ui(new Ui::parameterWidget)
{
m_ui->setupUi(this);
// Make sure the combo box is empty
// because else indices get messed up
//m_ui->vehicleComboBox->clear();
// Setup UI connections
//connect(m_ui->vehicleComboBox, SIGNAL(activated(int)), this, SLOT(selectUAS(int)));
// Get current MAV list
QList<UASInterface*> systems = UASManager::instance()->getUASList();
......@@ -84,8 +78,6 @@ void ParameterInterface::selectUAS(int index)
*/
void ParameterInterface::addUAS(UASInterface* uas)
{
//m_ui->vehicleComboBox->addItem(uas->getUASName());
QGCParamWidget* param = new QGCParamWidget(uas, this);
paramWidgets->insert(uas->getUASID(), param);
m_ui->stackedWidget->addWidget(param);
......
......@@ -30,6 +30,7 @@ This file is part of the QGROUNDCONTROL project
#include <QPushButton>
#include <QFileDialog>
#include <QFile>
#include <QList>
#include "QGCParamWidget.h"
#include "UASInterface.h"
......@@ -127,6 +128,10 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) :
// New parameters from UAS
connect(uas, SIGNAL(parameterChanged(int,int,int,int,QString,float)), this, SLOT(addParameter(int,int,int,int,QString,float)));
// Connect retransmission guard
connect(this, SIGNAL(requestParameter(int,int)), uas, SLOT(requestParameter(int,int)));
connect(&retransmissionTimer, SIGNAL(timeout()), this, SLOT(retransmissionGuardTick()));
}
/**
......@@ -202,6 +207,7 @@ void QGCParamWidget::addParameter(int uas, int component, int paramCount, int pa
transmissionMissingPackets.insert(component, new QList<int>());
}
// Mark all parameters as missing
for (int i = 0; i < paramCount; ++i)
{
if (!transmissionMissingPackets.value(component)->contains(i))
......@@ -209,6 +215,11 @@ void QGCParamWidget::addParameter(int uas, int component, int paramCount, int pa
transmissionMissingPackets.value(component)->append(i);
}
}
// Start retransmission guard
setRetransmissionGuardEnabled(true);
// Mark list size as known
transmissionListSizeKnown.insert(component, true);
}
}
......@@ -532,6 +543,68 @@ void QGCParamWidget::loadParameters()
}
/**
* Enabling the retransmission guard enables the parameter widget to track
* dropped parameters and to re-request them. This works for both individual
* parameter reads as well for whole list requests.
*
* @param enabled True if retransmission checking should be enabled, false else
*/
void QGCParamWidget::setRetransmissionGuardEnabled(bool enabled)
{
if (enabled)
{
retransmissionTimer.start(retransmissionTimeout);
}
else
{
retransmissionTimer.stop();
}
}
void QGCParamWidget::retransmissionGuardTick()
{
if (transmissionActive)
{
qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD ACTIVE, CHECKING FOR DROPS..";
// Re-request at maximum five parameters at once
// to prevent link flooding
QMap<int, QMap<QString, float>*>::iterator i;
for (i = parameters.begin(); i != parameters.end(); ++i)
{
// Iterate through the parameters of the component
int component = i.key();
// Request five parameters from this component (at maximum)
QList<int> * paramList = transmissionMissingPackets.value(component, NULL);
if (paramList)
{
int count = 0;
int maxCount = 5;
foreach (int id, *paramList)
{
if (count < maxCount)
{
qDebug() << __FILE__ << __LINE__ << "RETRANSMISSION GUARD REQUESTS RETRANSMISSION OF PARAM #" << id << "FROM COMPONENT #" << component;
emit requestParameter(component, id);
statusLabel->setText(tr("Requ. retransmission of #%1").arg(id));
count++;
}
else
{
break;
}
}
}
}
}
else
{
qDebug() << __FILE__ << __LINE__ << "STOPPING RETRANSMISSION GUARD GRACEFULLY";
setRetransmissionGuardEnabled(false);
}
}
/**
* @param component the subsystem which has the parameter
......@@ -541,7 +614,23 @@ void QGCParamWidget::loadParameters()
void QGCParamWidget::setParameter(int component, QString parameterName, float value)
{
emit parameterChanged(component, parameterName, value);
// // Wait for parameter to be written back
// // mark it therefore as missing
// if (!transmissionMissingPackets.contains(component))
// {
// transmissionMissingPackets.insert(component, new QList<int>());
// }
// for (int i = 0; i < paramCount; ++i)
// {
// if (!transmissionMissingPackets.value(component)->contains(i))
// {
// transmissionMissingPackets.value(component)->append(i);
// }
// }
// transmissionActive = true;
// transmissionStarted = QGC::groundTimeUsecs();
// setRetransmissionGuardEnabled(true);
}
/**
......
......@@ -35,6 +35,7 @@ This file is part of the QGROUNDCONTROL project
#include <QTreeWidgetItem>
#include <QMap>
#include <QLabel>
#include <QTimer>
#include "UASInterface.h"
......@@ -51,6 +52,8 @@ public:
signals:
/** @brief A parameter was changed in the widget, NOT onboard */
void parameterChanged(int component, QString parametername, float value);
/** @brief Request a single parameter */
void requestParameter(int component, int parameter);
public slots:
/** @brief Add a component to the list */
void addComponent(int uas, int component, QString componentName);
......@@ -77,6 +80,10 @@ public slots:
void saveParameters();
/** @brief Load parameters from a file */
void loadParameters();
/** @brief Check for missing parameters */
void retransmissionGuardTick();
protected:
UASInterface* mav; ///< The MAV this widget is controlling
QTreeWidget* tree; ///< The parameter tree
......@@ -91,6 +98,11 @@ protected:
QMap<int, bool> transmissionListSizeKnown; ///< List size initialized?
bool transmissionActive; ///< Missing packets, working on list?
quint64 transmissionStarted; ///< Timeout
QTimer retransmissionTimer; ///< Timer handling parameter retransmission
const static int retransmissionTimeout = 100; ///< Retransmission request timeout, in milliseconds
/** @brief Activate / deactivate parameter retransmission */
void setRetransmissionGuardEnabled(bool enabled);
};
#endif // QGCPARAMWIDGET_H
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