Commit 36b4938c authored by LM's avatar LM

Fixed roll angle in Google earth, still not smooth. Fixed a number of...

Fixed roll angle in Google earth, still not smooth. Fixed a number of threading issues. Fixed all custom widget related bugs. Improved MAVLink decoder. Reduced CPU usage substantially across all views
parent 49cebb45
...@@ -683,11 +683,17 @@ function updateFollowAircraft() ...@@ -683,11 +683,17 @@ function updateFollowAircraft()
if (viewMode != 3) // VIEW_MODE_CHASE_FREE if (viewMode != 3) // VIEW_MODE_CHASE_FREE
{ {
currView.setTilt(currFollowTilt); ge.getView().setAbstractView(currView);
currView.setHeading(currFollowHeading); var camera = ge.getView().copyAsCamera(ge.ALTITUDE_ABSOLUTE);
camera.setTilt(currFollowTilt);
camera.setRoll(0);
camera.setHeading(currFollowHeading);
ge.getView().setAbstractView(camera);
}
else
{
ge.getView().setAbstractView(currView);
} }
ge.getView().setAbstractView(currView);
} }
function failureCallback(object) function failureCallback(object)
......
...@@ -147,13 +147,6 @@ MAVLinkProtocol::~MAVLinkProtocol() ...@@ -147,13 +147,6 @@ MAVLinkProtocol::~MAVLinkProtocol()
} }
} }
void MAVLinkProtocol::run()
{
exec();
}
QString MAVLinkProtocol::getLogfileName() QString MAVLinkProtocol::getLogfileName()
{ {
if (m_logfile) { if (m_logfile) {
...@@ -173,7 +166,7 @@ QString MAVLinkProtocol::getLogfileName() ...@@ -173,7 +166,7 @@ QString MAVLinkProtocol::getLogfileName()
**/ **/
void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
{ {
receiveMutex.lock(); // receiveMutex.lock();
mavlink_message_t message; mavlink_message_t message;
mavlink_status_t status; mavlink_status_t status;
for (int position = 0; position < b.size(); position++) { for (int position = 0; position < b.size(); position++) {
...@@ -330,7 +323,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) ...@@ -330,7 +323,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
} }
} }
} }
receiveMutex.unlock(); // receiveMutex.unlock();
} }
/** /**
......
...@@ -57,7 +57,6 @@ public: ...@@ -57,7 +57,6 @@ public:
MAVLinkProtocol(); MAVLinkProtocol();
~MAVLinkProtocol(); ~MAVLinkProtocol();
void run();
/** @brief Get the human-friendly name of this protocol */ /** @brief Get the human-friendly name of this protocol */
QString getName(); QString getName();
/** @brief Get the system id of this application */ /** @brief Get the system id of this application */
......
...@@ -46,7 +46,7 @@ This file is part of the PIXHAWK project ...@@ -46,7 +46,7 @@ This file is part of the PIXHAWK project
* @see LinkManager. * @see LinkManager.
* *
**/ **/
class ProtocolInterface : public QThread class ProtocolInterface : public QObject
{ {
Q_OBJECT Q_OBJECT
public: public:
......
...@@ -1733,6 +1733,20 @@ void UAS::setParameter(const int component, const QString& id, const QVariant& v ...@@ -1733,6 +1733,20 @@ void UAS::setParameter(const int component, const QString& id, const QVariant& v
} }
} }
void UAS::requestParameter(int component, int id)
{
// Request parameter, use parameter name to request it
mavlink_message_t msg;
mavlink_param_request_read_t read;
read.param_index = id;
read.param_id[0] = '\0'; // Enforce null termination
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" << id;
}
void UAS::requestParameter(int component, const QString& parameter) void UAS::requestParameter(int component, const QString& parameter)
{ {
// Request parameter, use parameter name to request it // Request parameter, use parameter name to request it
...@@ -1750,7 +1764,7 @@ void UAS::requestParameter(int component, const QString& parameter) ...@@ -1750,7 +1764,7 @@ void UAS::requestParameter(int component, const QString& parameter)
read.target_component = component; read.target_component = component;
mavlink_msg_param_request_read_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &read); mavlink_msg_param_request_read_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &read);
sendMessage(msg); sendMessage(msg);
qDebug() << __FILE__ << __LINE__ << "REQUESTING PARAM RETRANSMISSION FROM COMPONENT" << component << "FOR PARAM ID" << parameter; qDebug() << __FILE__ << __LINE__ << "REQUESTING PARAM RETRANSMISSION FROM COMPONENT" << component << "FOR PARAM NAME" << parameter;
} }
void UAS::setSystemType(int systemType) void UAS::setSystemType(int systemType)
...@@ -1840,13 +1854,9 @@ void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float ...@@ -1840,13 +1854,9 @@ void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float
**/ **/
void UAS::launch() void UAS::launch()
{ {
// FIXME MAVLINKV10PORTINGNEEDED mavlink_message_t msg;
// mavlink_message_t msg; mavlink_msg_command_short_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, MAV_CMD_NAV_TAKEOFF, 1, 0, 0, 0, 0);
// // TODO Replace MG System ID with static function call and allow to change ID in GUI sendMessage(msg);
// mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_TAKEOFF);
// // Send message twice to increase chance of reception
// sendMessage(msg);
// sendMessage(msg);
} }
/** /**
......
...@@ -465,8 +465,10 @@ public slots: ...@@ -465,8 +465,10 @@ public slots:
/** @brief Request all parameters */ /** @brief Request all parameters */
void requestParameters(); void requestParameters();
/** @brief Request a single parameter by index */ /** @brief Request a single parameter by name */
void requestParameter(int component, const QString& parameter); void requestParameter(int component, const QString& parameter);
/** @brief Request a single parameter by index */
void requestParameter(int component, int id);
/** @brief Set a system parameter */ /** @brief Set a system parameter */
void setParameter(const int component, const QString& id, const QVariant& value); void setParameter(const int component, const QString& id, const QVariant& value);
......
...@@ -216,7 +216,6 @@ UASManager::UASManager() : ...@@ -216,7 +216,6 @@ UASManager::UASManager() :
homeLon(8.549444), homeLon(8.549444),
homeAlt(470.0) homeAlt(470.0)
{ {
start(QThread::LowPriority);
loadSettings(); loadSettings();
setLocalNEDSafetyBorders(1, -1, 0, -1, 1, -1); setLocalNEDSafetyBorders(1, -1, 0, -1, 1, -1);
} }
...@@ -230,16 +229,6 @@ UASManager::~UASManager() ...@@ -230,16 +229,6 @@ UASManager::~UASManager()
} }
} }
void UASManager::run()
{
// forever
// {
// QGC::SLEEP::msleep(5000);
// }
exec();
}
void UASManager::addUAS(UASInterface* uas) void UASManager::addUAS(UASInterface* uas)
{ {
// WARNING: The active uas is set here // WARNING: The active uas is set here
......
...@@ -44,7 +44,7 @@ This file is part of the QGROUNDCONTROL project ...@@ -44,7 +44,7 @@ This file is part of the QGROUNDCONTROL project
* This class keeps a list of all connected / configured UASs. It also stores which * This class keeps a list of all connected / configured UASs. It also stores which
* UAS is currently select with respect to user input or manual controls. * UAS is currently select with respect to user input or manual controls.
**/ **/
class UASManager : public QThread class UASManager : public QObject
{ {
Q_OBJECT Q_OBJECT
...@@ -52,7 +52,6 @@ public: ...@@ -52,7 +52,6 @@ public:
static UASManager* instance(); static UASManager* instance();
~UASManager(); ~UASManager();
void run();
/** /**
* @brief Get the currently selected UAS * @brief Get the currently selected UAS
* *
......
...@@ -50,7 +50,8 @@ HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) : ...@@ -50,7 +50,8 @@ HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) :
acceptUnitList(new QStringList()), acceptUnitList(new QStringList()),
lastPaintTime(0), lastPaintTime(0),
columns(3), columns(3),
m_ui(new Ui::HDDisplay) valuesChanged(true)/*,
m_ui(new Ui::HDDisplay)*/
{ {
setWindowTitle(title); setWindowTitle(title);
//m_ui->setupUi(this); //m_ui->setupUi(this);
...@@ -65,9 +66,6 @@ HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) : ...@@ -65,9 +66,6 @@ HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) :
} }
restoreState(); restoreState();
// Set minimum size
setMinimumSize(60, 60);
// Set preferred size // Set preferred size
setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
...@@ -109,6 +107,7 @@ HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) : ...@@ -109,6 +107,7 @@ HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) :
// setCursor(Qt::OpenHandCursor); // setCursor(Qt::OpenHandCursor);
// Set minimum size
this->setMinimumHeight(125); this->setMinimumHeight(125);
this->setMinimumWidth(100); this->setMinimumWidth(100);
...@@ -231,6 +230,8 @@ void HDDisplay::restoreState() ...@@ -231,6 +230,8 @@ void HDDisplay::restoreState()
QSettings settings; QSettings settings;
settings.sync(); settings.sync();
acceptList->clear();
QStringList instruments = settings.value(windowTitle()+"_gauges").toString().split('|'); QStringList instruments = settings.value(windowTitle()+"_gauges").toString().split('|');
for (int i = 0; i < instruments.count(); i++) { for (int i = 0; i < instruments.count(); i++) {
addGauge(instruments.at(i)); addGauge(instruments.at(i));
...@@ -385,6 +386,8 @@ void HDDisplay::setTitle() ...@@ -385,6 +386,8 @@ void HDDisplay::setTitle()
void HDDisplay::renderOverlay() void HDDisplay::renderOverlay()
{ {
if (!valuesChanged || !isVisible()) return;
#if (QGC_EVENTLOOP_DEBUG) #if (QGC_EVENTLOOP_DEBUG)
qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__; qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__;
#endif #endif
...@@ -666,7 +669,7 @@ void HDDisplay::drawSystemIndicator(float xRef, float yRef, int maxNum, float ma ...@@ -666,7 +669,7 @@ void HDDisplay::drawSystemIndicator(float xRef, float yRef, int maxNum, float ma
// x speed: 2.54 // x speed: 2.54
// One column per value // One column per value
QMapIterator<QString, float> value(values); QMapIterator<QString, double> value(values);
float x = xRef; float x = xRef;
float y = yRef; float y = yRef;
...@@ -820,6 +823,7 @@ void HDDisplay::updateValue(const int uasId, const QString& name, const QString& ...@@ -820,6 +823,7 @@ void HDDisplay::updateValue(const int uasId, const QString& name, const QString&
valuesMean.insert(name, (oldMean * meanCount + value) / (meanCount + 1)); valuesMean.insert(name, (oldMean * meanCount + value) / (meanCount + 1));
valuesCount.insert(name, meanCount + 1); valuesCount.insert(name, meanCount + 1);
valuesDot.insert(name, (value - values.value(name, 0.0f)) / ((msec - lastUpdate.value(name, 0))/1000.0f)); valuesDot.insert(name, (value - values.value(name, 0.0f)) / ((msec - lastUpdate.value(name, 0))/1000.0f));
if (values.value(name, 0.0) != value) valuesChanged = true;
values.insert(name, value); values.insert(name, value);
units.insert(name, unit); units.insert(name, unit);
lastUpdate.insert(name, msec); lastUpdate.insert(name, msec);
......
...@@ -139,7 +139,7 @@ protected: ...@@ -139,7 +139,7 @@ protected:
// virtual void resizeEvent(QResizeEvent* event); // virtual void resizeEvent(QResizeEvent* event);
UASInterface* uas; ///< The uas currently monitored UASInterface* uas; ///< The uas currently monitored
QMap<QString, float> values; ///< The variables this HUD displays QMap<QString, double> values; ///< The variables this HUD displays
QMap<QString, QString> units; ///< The units QMap<QString, QString> units; ///< The units
QMap<QString, float> valuesDot; ///< First derivative of the variable QMap<QString, float> valuesDot; ///< First derivative of the variable
QMap<QString, float> valuesMean; ///< Mean since system startup for this variable QMap<QString, float> valuesMean; ///< Mean since system startup for this variable
...@@ -172,7 +172,7 @@ protected: ...@@ -172,7 +172,7 @@ protected:
int warningBlinkRate; ///< Blink rate of warning messages, will be rounded to the refresh rate int warningBlinkRate; ///< Blink rate of warning messages, will be rounded to the refresh rate
QTimer* refreshTimer; ///< The main timer, controls the update rate QTimer* refreshTimer; ///< The main timer, controls the update rate
static const int updateInterval = 120; ///< Update interval in milliseconds static const int updateInterval = 300; ///< Update interval in milliseconds
QPainter* hudPainter; QPainter* hudPainter;
QFont font; ///< The HUD font, per default the free Bitstream Vera SANS, which is very close to actual HUD fonts QFont font; ///< The HUD font, per default the free Bitstream Vera SANS, which is very close to actual HUD fonts
QFontDatabase fontDatabase;///< Font database, only used to load the TrueType font file (the HUD font is directly loaded from file rather than from the system) QFontDatabase fontDatabase;///< Font database, only used to load the TrueType font file (the HUD font is directly loaded from file rather than from the system)
...@@ -191,6 +191,7 @@ protected: ...@@ -191,6 +191,7 @@ protected:
QAction* addGaugeAction; ///< Action adding a gauge QAction* addGaugeAction; ///< Action adding a gauge
QAction* setTitleAction; ///< Action setting the title QAction* setTitleAction; ///< Action setting the title
QAction* setColumnsAction; ///< Action setting the number of columns QAction* setColumnsAction; ///< Action setting the number of columns
bool valuesChanged;
private: private:
Ui::HDDisplay *m_ui; Ui::HDDisplay *m_ui;
......
...@@ -179,6 +179,7 @@ void HSIDisplay::paintEvent(QPaintEvent * event) ...@@ -179,6 +179,7 @@ void HSIDisplay::paintEvent(QPaintEvent * event)
void HSIDisplay::renderOverlay() void HSIDisplay::renderOverlay()
{ {
if (!isVisible()) return;
#if (QGC_EVENTLOOP_DEBUG) #if (QGC_EVENTLOOP_DEBUG)
qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__; qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__;
#endif #endif
......
...@@ -79,6 +79,7 @@ public slots: ...@@ -79,6 +79,7 @@ public slots:
/** @brief Ultrasound/Infrared localization changed */ /** @brief Ultrasound/Infrared localization changed */
void updateInfraredUltrasoundLocalization(UASInterface* uas, int fix); void updateInfraredUltrasoundLocalization(UASInterface* uas, int fix);
/** @brief Repaint the widget */
void paintEvent(QPaintEvent * event); void paintEvent(QPaintEvent * event);
/** @brief Update state from joystick */ /** @brief Update state from joystick */
void updateJoystick(double roll, double pitch, double yaw, double thrust, int xHat, int yHat); void updateJoystick(double roll, double pitch, double yaw, double thrust, int xHat, int yHat);
......
...@@ -21,6 +21,11 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) : ...@@ -21,6 +21,11 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) :
messageFilter.insert(MAVLINK_MSG_ID_MISSION_COUNT, false); messageFilter.insert(MAVLINK_MSG_ID_MISSION_COUNT, false);
messageFilter.insert(MAVLINK_MSG_ID_MISSION_ACK, false); messageFilter.insert(MAVLINK_MSG_ID_MISSION_ACK, false);
textMessageFilter.insert(MAVLINK_MSG_ID_DEBUG, false);
textMessageFilter.insert(MAVLINK_MSG_ID_DEBUG_VECT, false);
textMessageFilter.insert(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, false);
textMessageFilter.insert(MAVLINK_MSG_ID_NAMED_VALUE_INT, false);
connect(protocol, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), this, SLOT(receiveMessage(LinkInterface*,mavlink_message_t))); connect(protocol, SIGNAL(messageReceived(LinkInterface*,mavlink_message_t)), this, SLOT(receiveMessage(LinkInterface*,mavlink_message_t)));
} }
...@@ -81,7 +86,7 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64 ...@@ -81,7 +86,7 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
// Enforce null termination // Enforce null termination
str[messageInfo[msgid].fields[fieldid].array_length-1] = '\0'; str[messageInfo[msgid].fields[fieldid].array_length-1] = '\0';
QString string(name + ": " + str); QString string(name + ": " + str);
emit textMessageReceived(msg->sysid, msg->compid, 0, string); if (!textMessageFilter.contains(msgid)) emit textMessageReceived(msg->sysid, msg->compid, 0, string);
} }
else else
{ {
......
...@@ -29,6 +29,7 @@ protected: ...@@ -29,6 +29,7 @@ protected:
mavlink_message_t receivedMessages[256]; ///< Available / known messages mavlink_message_t receivedMessages[256]; ///< Available / known messages
mavlink_message_info_t messageInfo[256]; ///< Message information mavlink_message_info_t messageInfo[256]; ///< Message information
QMap<uint16_t, bool> messageFilter; ///< Message/field names not to emit QMap<uint16_t, bool> messageFilter; ///< Message/field names not to emit
QMap<uint16_t, bool> textMessageFilter; ///< Message/field names not to emit in text mode
}; };
......
...@@ -1249,11 +1249,8 @@ void MainWindow::UASCreated(UASInterface* uas) ...@@ -1249,11 +1249,8 @@ void MainWindow::UASCreated(UASInterface* uas)
if (!ui.menuConnected_Systems->isEnabled()) ui.menuConnected_Systems->setEnabled(true); if (!ui.menuConnected_Systems->isEnabled()) ui.menuConnected_Systems->setEnabled(true);
// // Restore the mainwindow size // Reload view state in case new widgets were added
// if (settings.contains(getWindowGeometryKey())) loadViewState();
// {
// restoreGeometry(settings.value(getWindowGeometryKey()).toByteArray());
// }
} }
/** /**
......
...@@ -131,6 +131,7 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) : ...@@ -131,6 +131,7 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) :
connect(uas, SIGNAL(parameterChanged(int,int,int,int,QString,QVariant)), this, SLOT(addParameter(int,int,int,int,QString,QVariant))); connect(uas, SIGNAL(parameterChanged(int,int,int,int,QString,QVariant)), this, SLOT(addParameter(int,int,int,int,QString,QVariant)));
// Connect retransmission guard // Connect retransmission guard
connect(this, SIGNAL(requestParameter(int,QString)), uas, SLOT(requestParameter(int,QString)));
connect(this, SIGNAL(requestParameter(int,int)), uas, SLOT(requestParameter(int,int))); connect(this, SIGNAL(requestParameter(int,int)), uas, SLOT(requestParameter(int,int)));
connect(&retransmissionTimer, SIGNAL(timeout()), this, SLOT(retransmissionGuardTick())); connect(&retransmissionTimer, SIGNAL(timeout()), this, SLOT(retransmissionGuardTick()));
} }
...@@ -474,7 +475,7 @@ void QGCParamWidget::addParameter(int uas, int component, QString parameterName, ...@@ -474,7 +475,7 @@ void QGCParamWidget::addParameter(int uas, int component, QString parameterName,
// componentName = tr("Component #").arg(component); // componentName = tr("Component #").arg(component);
// break; // break;
// } // }
QString componentName = tr("Component #").arg(component); QString componentName = tr("Component #%1").arg(component);
addComponent(uas, component, componentName); addComponent(uas, component, componentName);
} }
......
...@@ -53,8 +53,10 @@ public: ...@@ -53,8 +53,10 @@ public:
signals: signals:
/** @brief A parameter was changed in the widget, NOT onboard */ /** @brief A parameter was changed in the widget, NOT onboard */
void parameterChanged(int component, QString parametername, QVariant value); void parameterChanged(int component, QString parametername, QVariant value);
/** @brief Request a single parameter */ /** @brief Request a single parameter by index */
void requestParameter(int component, int parameter); void requestParameter(int component, int parameter);
/** @brief Request a single parameter by name */
void requestParameter(int component, const QString& parameter);
public slots: public slots:
/** @brief Add a component to the list */ /** @brief Add a component to the list */
void addComponent(int uas, int component, QString componentName); void addComponent(int uas, int component, QString componentName);
......
...@@ -214,7 +214,7 @@ void QGCToolBar::setActiveUAS(UASInterface* active) ...@@ -214,7 +214,7 @@ void QGCToolBar::setActiveUAS(UASInterface* active)
disconnect(mav, SIGNAL(batteryChanged(UASInterface*,double,double,int)), this, SLOT(updateBatteryRemaining(UASInterface*,double,double,int))); disconnect(mav, SIGNAL(batteryChanged(UASInterface*,double,double,int)), this, SLOT(updateBatteryRemaining(UASInterface*,double,double,int)));
if (mav->getWaypointManager()) if (mav->getWaypointManager())
{ {
disconnect(mav->getWaypointManager(), SIGNAL(currentWaypointChanged(int)), this, SLOT(updateCurrentWaypoint(int))); disconnect(mav->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(updateCurrentWaypoint(quint16)));
disconnect(mav->getWaypointManager(), SIGNAL(waypointDistanceChanged(double)), this, SLOT(updateWaypointDistance(double))); disconnect(mav->getWaypointManager(), SIGNAL(waypointDistanceChanged(double)), this, SLOT(updateWaypointDistance(double)));
} }
} }
...@@ -234,6 +234,7 @@ void QGCToolBar::setActiveUAS(UASInterface* active) ...@@ -234,6 +234,7 @@ void QGCToolBar::setActiveUAS(UASInterface* active)
} }
// Update all values once // Update all values once
systemName = mav->getUASName();
toolBarNameLabel->setText(mav->getUASName()); toolBarNameLabel->setText(mav->getUASName());
toolBarNameLabel->setStyleSheet(QString("QLabel { font: bold 16px; color: %1; }").arg(mav->getColor().name())); toolBarNameLabel->setStyleSheet(QString("QLabel { font: bold 16px; color: %1; }").arg(mav->getColor().name()));
symbolButton->setStyleSheet(QString("QWidget { background-color: %1; color: #DDDDDF; background-clip: border; } QToolButton { font-weight: bold; font-size: 12px; border: 0px solid #999999; border-radius: 5px; min-width:22px; max-width: 22px; min-height: 22px; max-height: 22px; padding: 0px; margin: 0px 4px 0px 20px; background-color: none; }").arg(mav->getColor().name())); symbolButton->setStyleSheet(QString("QWidget { background-color: %1; color: #DDDDDF; background-clip: border; } QToolButton { font-weight: bold; font-size: 12px; border: 0px solid #999999; border-radius: 5px; min-width:22px; max-width: 22px; min-height: 22px; max-height: 22px; padding: 0px; margin: 0px 4px 0px 20px; background-color: none; }").arg(mav->getColor().name()));
......
...@@ -170,12 +170,15 @@ void QGCCommandButton::readSettings(const QSettings& settings) ...@@ -170,12 +170,15 @@ void QGCCommandButton::readSettings(const QSettings& settings)
} }
ui->editParamsVisibleCheckBox->setChecked(settings.value("QGC_COMMAND_BUTTON_PARAMS_VISIBLE").toBool()); ui->editParamsVisibleCheckBox->setChecked(settings.value("QGC_COMMAND_BUTTON_PARAMS_VISIBLE").toBool());
if (ui->editParamsVisibleCheckBox->isChecked()) { if (ui->editParamsVisibleCheckBox->isChecked())
{
ui->editParam1SpinBox->show(); ui->editParam1SpinBox->show();
ui->editParam2SpinBox->show(); ui->editParam2SpinBox->show();
ui->editParam3SpinBox->show(); ui->editParam3SpinBox->show();
ui->editParam4SpinBox->show(); ui->editParam4SpinBox->show();
} else { }
else
{
ui->editParam1SpinBox->hide(); ui->editParam1SpinBox->hide();
ui->editParam2SpinBox->hide(); ui->editParam2SpinBox->hide();
ui->editParam3SpinBox->hide(); ui->editParam3SpinBox->hide();
......
...@@ -20,6 +20,7 @@ QGCParamSlider::QGCParamSlider(QWidget *parent) : ...@@ -20,6 +20,7 @@ QGCParamSlider::QGCParamSlider(QWidget *parent) :
ui(new Ui::QGCParamSlider) ui(new Ui::QGCParamSlider)
{ {
ui->setupUi(this); ui->setupUi(this);
ui->intValueSpinBox->hide();
uas = NULL; uas = NULL;
scaledInt = ui->valueSlider->maximum() - ui->valueSlider->minimum(); scaledInt = ui->valueSlider->maximum() - ui->valueSlider->minimum();
...@@ -45,12 +46,14 @@ QGCParamSlider::QGCParamSlider(QWidget *parent) : ...@@ -45,12 +46,14 @@ QGCParamSlider::QGCParamSlider(QWidget *parent) :
connect(ui->editSelectComponentComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectComponent(int))); connect(ui->editSelectComponentComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectComponent(int)));
connect(ui->editSelectParamComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectParameter(int))); connect(ui->editSelectParamComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectParameter(int)));
connect(ui->valueSlider, SIGNAL(valueChanged(int)), this, SLOT(setSliderValue(int))); connect(ui->valueSlider, SIGNAL(valueChanged(int)), this, SLOT(setSliderValue(int)));
connect(ui->valueSpinBox, SIGNAL(valueChanged(double)), this, SLOT(setParamValue(double))); connect(ui->doubleValueSpinBox, SIGNAL(valueChanged(double)), this, SLOT(setParamValue(double)));
connect(ui->intValueSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setParamValue(int)));
connect(ui->editNameLabel, SIGNAL(textChanged(QString)), ui->nameLabel, SLOT(setText(QString))); connect(ui->editNameLabel, SIGNAL(textChanged(QString)), ui->nameLabel, SLOT(setText(QString)));
connect(ui->readButton, SIGNAL(clicked()), this, SLOT(requestParameter())); connect(ui->readButton, SIGNAL(clicked()), this, SLOT(requestParameter()));
connect(ui->editRefreshParamsButton, SIGNAL(clicked()), this, SLOT(refreshParamList())); connect(ui->editRefreshParamsButton, SIGNAL(clicked()), this, SLOT(refreshParamList()));
// Set the current UAS if present // Set the current UAS if present
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
setActiveUAS(UASManager::instance()->getActiveUAS()); setActiveUAS(UASManager::instance()->getActiveUAS());
// Get param value // Get param value
...@@ -66,21 +69,26 @@ void QGCParamSlider::refreshParamList() ...@@ -66,21 +69,26 @@ void QGCParamSlider::refreshParamList()
{ {
ui->editSelectParamComboBox->setEnabled(true); ui->editSelectParamComboBox->setEnabled(true);
ui->editSelectComponentComboBox->setEnabled(true); ui->editSelectComponentComboBox->setEnabled(true);
if (uas) { if (uas)
{
uas->getParamManager()->requestParameterList(); uas->getParamManager()->requestParameterList();
} }
} }
void QGCParamSlider::setActiveUAS(UASInterface* activeUas) void QGCParamSlider::setActiveUAS(UASInterface* activeUas)
{ {
if (activeUas) { if (activeUas)
if (uas) { {
disconnect(uas, SIGNAL(parameterChanged(int,int,int,int,QString,float)), this, SLOT(setParameterValue(int,int,int,int,QString,float))); if (uas)
{
disconnect(uas, SIGNAL(parameterChanged(int,int,int,int,QString,QVariant)), this, SLOT(setParameterValue(int,int,int,int,QString,QVariant)));
} }
// Connect buttons and signals // Connect buttons and signals
connect(activeUas, SIGNAL(parameterChanged(int,int,int,int,QString,float)), this, SLOT(setParameterValue(int,int,int,int,QString,float)), Qt::UniqueConnection); connect(activeUas, SIGNAL(parameterChanged(int,int,int,int,QString,QVariant)), this, SLOT(setParameterValue(int,int,int,int,QString,QVariant)), Qt::UniqueConnection);
uas = activeUas; uas = activeUas;
// Update current param value
requestParameter();
} }
} }
...@@ -98,6 +106,12 @@ void QGCParamSlider::setParamValue(double value) ...@@ -98,6 +106,12 @@ void QGCParamSlider::setParamValue(double value)
ui->valueSlider->setValue(floatToScaledInt(value)); ui->valueSlider->setValue(floatToScaledInt(value));
} }
void QGCParamSlider::setParamValue(int value)
{
parameterValue = value;
ui->valueSlider->setValue(floatToScaledInt(value));
}
void QGCParamSlider::selectComponent(int componentIndex) void QGCParamSlider::selectComponent(int componentIndex)
{ {
this->component = ui->editSelectComponentComboBox->itemData(componentIndex).toInt(); this->component = ui->editSelectComponentComboBox->itemData(componentIndex).toInt();
...@@ -111,7 +125,8 @@ void QGCParamSlider::selectParameter(int paramIndex) ...@@ -111,7 +125,8 @@ void QGCParamSlider::selectParameter(int paramIndex)
void QGCParamSlider::startEditMode() void QGCParamSlider::startEditMode()
{ {
ui->valueSlider->hide(); ui->valueSlider->hide();
ui->valueSpinBox->hide(); ui->doubleValueSpinBox->hide();
ui->intValueSpinBox->hide();
ui->nameLabel->hide(); ui->nameLabel->hide();
ui->writeButton->hide(); ui->writeButton->hide();
ui->readButton->hide(); ui->readButton->hide();
...@@ -156,7 +171,21 @@ void QGCParamSlider::endEditMode() ...@@ -156,7 +171,21 @@ void QGCParamSlider::endEditMode()
ui->writeButton->show(); ui->writeButton->show();
ui->readButton->show(); ui->readButton->show();
ui->valueSlider->show(); ui->valueSlider->show();
ui->valueSpinBox->show(); switch (parameterValue.type())
{
case QVariant::Int:
ui->intValueSpinBox->show();
break;
case QVariant::UInt:
ui->intValueSpinBox->show();
break;
case QMetaType::Float:
ui->doubleValueSpinBox->show();
break;
default:
qCritical() << "ERROR: NO VALID PARAM TYPE";
return;
}
ui->nameLabel->show(); ui->nameLabel->show();
isInEditMode = false; isInEditMode = false;
emit editingFinished(); emit editingFinished();
...@@ -164,10 +193,20 @@ void QGCParamSlider::endEditMode() ...@@ -164,10 +193,20 @@ void QGCParamSlider::endEditMode()
void QGCParamSlider::sendParameter() void QGCParamSlider::sendParameter()
{ {
if (uas) { if (uas)
{
// Set value, param manager handles retransmission // Set value, param manager handles retransmission
uas->getParamManager()->setParameter(component, parameterName, parameterValue); if (uas->getParamManager())
} else { {
uas->getParamManager()->setParameter(component, parameterName, parameterValue);
}
else
{
qDebug() << "UAS HAS NO PARAM MANAGER, DOING NOTHING";
}
}
else
{
qDebug() << __FILE__ << __LINE__ << "NO UAS SET, DOING NOTHING"; qDebug() << __FILE__ << __LINE__ << "NO UAS SET, DOING NOTHING";
} }
} }
...@@ -175,9 +214,21 @@ void QGCParamSlider::sendParameter() ...@@ -175,9 +214,21 @@ void QGCParamSlider::sendParameter()
void QGCParamSlider::setSliderValue(int sliderValue) void QGCParamSlider::setSliderValue(int sliderValue)
{ {
parameterValue = scaledIntToFloat(sliderValue); parameterValue = scaledIntToFloat(sliderValue);
ui->valueSpinBox->setValue(parameterValue); switch (parameterValue.type())
// QString unit(""); {
// ui->valueLabel->setText(QString("%1 %2").arg(parameterValue, 6, 'f', 6, ' ').arg(unit)); case QVariant::Int:
ui->intValueSpinBox->setValue(parameterValue.toInt());
break;
case QVariant::UInt:
ui->intValueSpinBox->setValue(parameterValue.toUInt());
break;
case QMetaType::Float:
ui->doubleValueSpinBox->setValue(parameterValue.toFloat());
break;
default:
qCritical() << "ERROR: NO VALID PARAM TYPE";
return;
}
} }
/** /**
...@@ -186,38 +237,67 @@ void QGCParamSlider::setSliderValue(int sliderValue) ...@@ -186,38 +237,67 @@ void QGCParamSlider::setSliderValue(int sliderValue)
* @brief parameterName Key/name of the parameter * @brief parameterName Key/name of the parameter
* @brief value Value of the parameter * @brief value Value of the parameter
*/ */
void QGCParamSlider::setParameterValue(int uas, int component, int paramCount, int paramIndex, QString parameterName, float value) void QGCParamSlider::setParameterValue(int uas, int component, int paramCount, int paramIndex, QString parameterName, QVariant value)
{ {
Q_UNUSED(paramCount); Q_UNUSED(paramCount);
// Check if this component and parameter are part of the list // Check if this component and parameter are part of the list
bool found = false; bool found = false;
for (int i = 0; i< ui->editSelectComponentComboBox->count(); ++i) { for (int i = 0; i< ui->editSelectComponentComboBox->count(); ++i)
if (component == ui->editSelectComponentComboBox->itemData(i).toInt()) { {
if (component == ui->editSelectComponentComboBox->itemData(i).toInt())
{
found = true; found = true;
} }
} }
if (!found) { if (!found)
{
ui->editSelectComponentComboBox->addItem(tr("Component #%1").arg(component), component); ui->editSelectComponentComboBox->addItem(tr("Component #%1").arg(component), component);
} }
// Parameter checking // Parameter checking
found = false; found = false;
for (int i = 0; i < ui->editSelectParamComboBox->count(); ++i) { for (int i = 0; i < ui->editSelectParamComboBox->count(); ++i)
if (parameterName == ui->editSelectParamComboBox->itemText(i)) { {
if (parameterName == ui->editSelectParamComboBox->itemText(i))
{
found = true; found = true;
} }
} }
if (!found) { if (!found)
{
ui->editSelectParamComboBox->addItem(parameterName, paramIndex); ui->editSelectParamComboBox->addItem(parameterName, paramIndex);
} }
Q_UNUSED(uas); Q_UNUSED(uas);
if (component == this->component && parameterName == this->parameterName) { if (component == this->component && parameterName == this->parameterName)
{
parameterValue = value; parameterValue = value;
ui->valueSpinBox->setValue(value); switch (value.type())
ui->valueSlider->setValue(floatToScaledInt(value)); {
case QVariant::Int:
ui->intValueSpinBox->show();
ui->doubleValueSpinBox->hide();
ui->intValueSpinBox->setValue(value.toDouble());
ui->intValueSpinBox->setMinimum(-ui->intValueSpinBox->maximum());
break;
case QVariant::UInt:
ui->intValueSpinBox->show();
ui->doubleValueSpinBox->hide();
ui->intValueSpinBox->setValue(value.toDouble());
ui->intValueSpinBox->setMinimum(0);
break;
case QMetaType::Float:
ui->doubleValueSpinBox->setValue(value.toDouble());
ui->doubleValueSpinBox->show();
ui->intValueSpinBox->hide();
break;
default:
qCritical() << "ERROR: NO VALID PARAM TYPE";
return;
}
ui->valueSlider->setValue(floatToScaledInt(value.toDouble()));
} }
} }
......
...@@ -28,14 +28,17 @@ public slots: ...@@ -28,14 +28,17 @@ public slots:
/** @brief Set the slider value as parameter value */ /** @brief Set the slider value as parameter value */
void setSliderValue(int sliderValue); void setSliderValue(int sliderValue);
/** @brief Update the UI with the new parameter value */ /** @brief Update the UI with the new parameter value */
void setParameterValue(int uas, int component, int paramCount, int paramIndex, QString parameterName, float value); void setParameterValue(int uas, int component, int paramCount, int paramIndex, QString parameterName, const QVariant value);
void writeSettings(QSettings& settings); void writeSettings(QSettings& settings);
void readSettings(const QSettings& settings); void readSettings(const QSettings& settings);
void refreshParamList(); void refreshParamList();
void setActiveUAS(UASInterface *uas); void setActiveUAS(UASInterface *uas);
void selectComponent(int componentIndex); void selectComponent(int componentIndex);
void selectParameter(int paramIndex); void selectParameter(int paramIndex);
/** @brief Set a double parameter value */
void setParamValue(double value); void setParamValue(double value);
/** @brief Set an integer parameter value */
void setParamValue(int value);
protected slots: protected slots:
/** @brief Request the parameter of this widget from the MAV */ /** @brief Request the parameter of this widget from the MAV */
...@@ -43,7 +46,7 @@ protected slots: ...@@ -43,7 +46,7 @@ protected slots:
protected: protected:
QString parameterName; ///< Key/Name of the parameter QString parameterName; ///< Key/Name of the parameter
float parameterValue; ///< Value of the parameter QVariant parameterValue; ///< Value of the parameter
double parameterScalingFactor; ///< Factor to scale the parameter between slider and true value double parameterScalingFactor; ///< Factor to scale the parameter between slider and true value
float parameterMin; float parameterMin;
float parameterMax; float parameterMax;
......
...@@ -7,7 +7,7 @@ ...@@ -7,7 +7,7 @@
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>789</width> <width>789</width>
<height>155</height> <height>158</height>
</rect> </rect>
</property> </property>
<property name="windowTitle"> <property name="windowTitle">
...@@ -147,7 +147,7 @@ ...@@ -147,7 +147,7 @@
</widget> </widget>
</item> </item>
<item row="2" column="2"> <item row="2" column="2">
<widget class="QDoubleSpinBox" name="valueSpinBox"> <widget class="QDoubleSpinBox" name="doubleValueSpinBox">
<property name="minimum"> <property name="minimum">
<double>-999999999.000000000000000</double> <double>-999999999.000000000000000</double>
</property> </property>
...@@ -246,6 +246,16 @@ ...@@ -246,6 +246,16 @@
</property> </property>
</widget> </widget>
</item> </item>
<item row="2" column="4">
<widget class="QSpinBox" name="intValueSpinBox">
<property name="minimum">
<number>-999999999</number>
</property>
<property name="maximum">
<number>999999999</number>
</property>
</widget>
</item>
</layout> </layout>
</widget> </widget>
<resources/> <resources/>
......
...@@ -47,7 +47,6 @@ QGCToolWidget::QGCToolWidget(const QString& title, QWidget *parent) : ...@@ -47,7 +47,6 @@ QGCToolWidget::QGCToolWidget(const QString& title, QWidget *parent) :
} }
this->setWindowTitle(title); this->setWindowTitle(title);
//setObjectName(title+"WIDGET");
QList<UASInterface*> systems = UASManager::instance()->getUASList(); QList<UASInterface*> systems = UASManager::instance()->getUASList();
foreach (UASInterface* uas, systems) foreach (UASInterface* uas, systems)
...@@ -64,8 +63,8 @@ QGCToolWidget::QGCToolWidget(const QString& title, QWidget *parent) : ...@@ -64,8 +63,8 @@ QGCToolWidget::QGCToolWidget(const QString& title, QWidget *parent) :
QGCToolWidget::~QGCToolWidget() QGCToolWidget::~QGCToolWidget()
{ {
if (mainMenuAction) delete mainMenuAction; if (mainMenuAction) mainMenuAction->deleteLater();
QGCToolWidget::instances()->remove(widgetTitle); if (QGCToolWidget::instances()) QGCToolWidget::instances()->remove(widgetTitle);
delete ui; delete ui;
} }
...@@ -99,6 +98,7 @@ QList<QGCToolWidget*> QGCToolWidget::createWidgetsFromSettings(QWidget* parent, ...@@ -99,6 +98,7 @@ QList<QGCToolWidget*> QGCToolWidget::createWidgetsFromSettings(QWidget* parent,
else else
{ {
settings = new QSettings(); settings = new QSettings();
qDebug() << "LOADING SETTINGS FROM DEFAULT" << settings->fileName();
} }
QList<QGCToolWidget*> newWidgets; QList<QGCToolWidget*> newWidgets;
...@@ -206,10 +206,12 @@ void QGCToolWidget::storeWidgetsToSettings(QString settingsFile) ...@@ -206,10 +206,12 @@ void QGCToolWidget::storeWidgetsToSettings(QString settingsFile)
if (!settingsFile.isEmpty()) if (!settingsFile.isEmpty())
{ {
settings = new QSettings(settingsFile, QSettings::IniFormat); settings = new QSettings(settingsFile, QSettings::IniFormat);
qDebug() << "STORING SETTINGS TO" << settings->fileName();
} }
else else
{ {
settings = new QSettings(); settings = new QSettings();
qDebug() << "STORING SETTINGS TO DEFAULT" << settings->fileName();
} }
settings->beginWriteArray("QGC_TOOL_WIDGET_NAMES"); settings->beginWriteArray("QGC_TOOL_WIDGET_NAMES");
...@@ -228,6 +230,12 @@ void QGCToolWidget::storeWidgetsToSettings(QString settingsFile) ...@@ -228,6 +230,12 @@ void QGCToolWidget::storeWidgetsToSettings(QString settingsFile)
delete settings; delete settings;
} }
void QGCToolWidget::storeSettings()
{
QSettings settings;
storeSettings(settings);
}
void QGCToolWidget::storeSettings(const QString& settingsFile) void QGCToolWidget::storeSettings(const QString& settingsFile)
{ {
QSettings settings(settingsFile, QSettings::IniFormat); QSettings settings(settingsFile, QSettings::IniFormat);
...@@ -271,6 +279,7 @@ void QGCToolWidget::contextMenuEvent (QContextMenuEvent* event) ...@@ -271,6 +279,7 @@ void QGCToolWidget::contextMenuEvent (QContextMenuEvent* event)
QMenu menu(this); QMenu menu(this);
menu.addAction(addParamAction); menu.addAction(addParamAction);
menu.addAction(addCommandAction); menu.addAction(addCommandAction);
menu.addSeparator();
menu.addAction(setTitleAction); menu.addAction(setTitleAction);
menu.addAction(exportAction); menu.addAction(exportAction);
menu.addAction(importAction); menu.addAction(importAction);
...@@ -281,7 +290,6 @@ void QGCToolWidget::contextMenuEvent (QContextMenuEvent* event) ...@@ -281,7 +290,6 @@ void QGCToolWidget::contextMenuEvent (QContextMenuEvent* event)
void QGCToolWidget::hideEvent(QHideEvent* event) void QGCToolWidget::hideEvent(QHideEvent* event)
{ {
// Store settings // Store settings
storeWidgetsToSettings();
QWidget::hideEvent(event); QWidget::hideEvent(event);
} }
...@@ -340,6 +348,7 @@ QList<QGCToolWidgetItem*>* QGCToolWidget::itemList() ...@@ -340,6 +348,7 @@ QList<QGCToolWidgetItem*>* QGCToolWidget::itemList()
void QGCToolWidget::addParam() void QGCToolWidget::addParam()
{ {
QGCParamSlider* slider = new QGCParamSlider(this); QGCParamSlider* slider = new QGCParamSlider(this);
connect(slider, SIGNAL(destroyed()), this, SLOT(storeSettings()));
if (ui->hintLabel) if (ui->hintLabel)
{ {
ui->hintLabel->deleteLater(); ui->hintLabel->deleteLater();
...@@ -352,6 +361,7 @@ void QGCToolWidget::addParam() ...@@ -352,6 +361,7 @@ void QGCToolWidget::addParam()
void QGCToolWidget::addCommand() void QGCToolWidget::addCommand()
{ {
QGCCommandButton* button = new QGCCommandButton(this); QGCCommandButton* button = new QGCCommandButton(this);
connect(button, SIGNAL(destroyed()), this, SLOT(storeSettings()));
if (ui->hintLabel) if (ui->hintLabel)
{ {
ui->hintLabel->deleteLater(); ui->hintLabel->deleteLater();
...@@ -368,6 +378,7 @@ void QGCToolWidget::addToolWidget(QGCToolWidgetItem* widget) ...@@ -368,6 +378,7 @@ void QGCToolWidget::addToolWidget(QGCToolWidgetItem* widget)
ui->hintLabel->deleteLater(); ui->hintLabel->deleteLater();
ui->hintLabel = NULL; ui->hintLabel = NULL;
} }
connect(widget, SIGNAL(destroyed()), this, SLOT(storeSettings()));
toolLayout->addWidget(widget); toolLayout->addWidget(widget);
} }
...@@ -436,7 +447,6 @@ void QGCToolWidget::setTitle(QString title) ...@@ -436,7 +447,6 @@ void QGCToolWidget::setTitle(QString title)
QWidget::setWindowTitle(title); QWidget::setWindowTitle(title);
if (parent) parent->setWindowTitle(title); if (parent) parent->setWindowTitle(title);
storeWidgetsToSettings();
emit titleChanged(title); emit titleChanged(title);
if (mainMenuAction) mainMenuAction->setText(title); if (mainMenuAction) mainMenuAction->setText(title);
} }
......
...@@ -53,6 +53,8 @@ public slots: ...@@ -53,6 +53,8 @@ public slots:
void storeSettings(QSettings& settings); void storeSettings(QSettings& settings);
/** @brief Store this widget to a settings file */ /** @brief Store this widget to a settings file */
void storeSettings(const QString& settingsFile); void storeSettings(const QString& settingsFile);
/** @brief Store this widget to a settings file */
void storeSettings();
/** @brief Store the view id and dock widget area */ /** @brief Store the view id and dock widget area */
void setViewVisibilityAndDockWidgetArea(int view, bool visible, Qt::DockWidgetArea area); void setViewVisibilityAndDockWidgetArea(int view, bool visible, Qt::DockWidgetArea area);
......
...@@ -610,7 +610,8 @@ void QGCGoogleEarthView::updateState() ...@@ -610,7 +610,8 @@ void QGCGoogleEarthView::updateState()
// Update all MAVs // Update all MAVs
QList<UASInterface*> mavs = UASManager::instance()->getUASList(); QList<UASInterface*> mavs = UASManager::instance()->getUASList();
foreach (UASInterface* currMav, mavs) { foreach (UASInterface* currMav, mavs)
{
uasId = currMav->getUASID(); uasId = currMav->getUASID();
lat = currMav->getLatitude(); lat = currMav->getLatitude();
lon = currMav->getLongitude(); lon = currMav->getLongitude();
...@@ -637,9 +638,9 @@ void QGCGoogleEarthView::updateState() ...@@ -637,9 +638,9 @@ void QGCGoogleEarthView::updateState()
// Microsoft API available in Qt - improvements wanted // Microsoft API available in Qt - improvements wanted
// First check if a new WP should be created // First check if a new WP should be created
// bool newWaypointPending = .to
bool newWaypointPending = documentElement("newWaypointPending").toBool(); bool newWaypointPending = documentElement("newWaypointPending").toBool();
if (newWaypointPending) { if (newWaypointPending)
{
bool coordsOk = true; bool coordsOk = true;
bool ok; bool ok;
double latitude = documentElement("newWaypointLatitude").toDouble(&ok); double latitude = documentElement("newWaypointLatitude").toDouble(&ok);
...@@ -648,15 +649,14 @@ void QGCGoogleEarthView::updateState() ...@@ -648,15 +649,14 @@ void QGCGoogleEarthView::updateState()
coordsOk &= ok; coordsOk &= ok;
double altitude = documentElement("newWaypointAltitude").toDouble(&ok); double altitude = documentElement("newWaypointAltitude").toDouble(&ok);
coordsOk &= ok; coordsOk &= ok;
if (coordsOk) { if (coordsOk)
{
// Add new waypoint // Add new waypoint
if (mav) { if (mav)
{
int nextIndex = mav->getWaypointManager()->getWaypointList().count(); int nextIndex = mav->getWaypointManager()->getWaypointList().count();
Waypoint* wp = new Waypoint(nextIndex, latitude, longitude, altitude, true); Waypoint* wp = new Waypoint(nextIndex, latitude, longitude, altitude, true);
wp->setFrame(MAV_FRAME_GLOBAL); wp->setFrame(MAV_FRAME_GLOBAL);
// wp.setLatitude(latitude);
// wp.setLongitude(longitude);
// wp.setAltitude(altitude);
mav->getWaypointManager()->addWaypoint(wp); mav->getWaypointManager()->addWaypoint(wp);
} }
} }
...@@ -666,7 +666,8 @@ void QGCGoogleEarthView::updateState() ...@@ -666,7 +666,8 @@ void QGCGoogleEarthView::updateState()
// Check if a waypoint should be moved // Check if a waypoint should be moved
bool dragWaypointPending = documentElement("dragWaypointPending").toBool(); bool dragWaypointPending = documentElement("dragWaypointPending").toBool();
if (dragWaypointPending) { if (dragWaypointPending)
{
bool coordsOk = true; bool coordsOk = true;
bool ok; bool ok;
double latitude = documentElement("dragWaypointLatitude").toDouble(&ok); double latitude = documentElement("dragWaypointLatitude").toDouble(&ok);
...@@ -677,21 +678,27 @@ void QGCGoogleEarthView::updateState() ...@@ -677,21 +678,27 @@ void QGCGoogleEarthView::updateState()
coordsOk &= ok; coordsOk &= ok;
// UPDATE WAYPOINTS, HOME LOCATION AND OTHER LOCATIONS // UPDATE WAYPOINTS, HOME LOCATION AND OTHER LOCATIONS
if (coordsOk) { if (coordsOk)
{
QString idText = documentElement("dragWaypointIndex").toString(); QString idText = documentElement("dragWaypointIndex").toString();
if (idText == "HOME") { if (idText == "HOME")
{
qDebug() << "HOME UPDATED!"; qDebug() << "HOME UPDATED!";
UASManager::instance()->setHomePosition(latitude, longitude, altitude); UASManager::instance()->setHomePosition(latitude, longitude, altitude);
ui->setHomeButton->setChecked(false); ui->setHomeButton->setChecked(false);
} else { }
else
{
// Update waypoint or symbol // Update waypoint or symbol
if (mav) { if (mav)
{
QVector<Waypoint*> wps = mav->getWaypointManager()->getGlobalFrameAndNavTypeWaypointList(); QVector<Waypoint*> wps = mav->getWaypointManager()->getGlobalFrameAndNavTypeWaypointList();
bool ok; bool ok;
int index = idText.toInt(&ok); int index = idText.toInt(&ok);
if (ok && index >= 0 && index < wps.count()) { if (ok && index >= 0 && index < wps.count())
{
Waypoint* wp = wps.at(index); Waypoint* wp = wps.at(index);
wp->setLatitude(latitude); wp->setLatitude(latitude);
wp->setLongitude(longitude); wp->setLongitude(longitude);
...@@ -700,7 +707,9 @@ void QGCGoogleEarthView::updateState() ...@@ -700,7 +707,9 @@ void QGCGoogleEarthView::updateState()
} }
} }
} }
} else { }
else
{
// If coords were not ok, move the view in google earth back // If coords were not ok, move the view in google earth back
// to last acceptable location // to last acceptable location
updateWaypointList(mav->getUASID()); updateWaypointList(mav->getUASID());
......
...@@ -71,7 +71,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : ...@@ -71,7 +71,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
hilAction(new QAction("Enable Hardware-in-the-Loop Simulation", this )), hilAction(new QAction("Enable Hardware-in-the-Loop Simulation", this )),
selectAirframeAction(new QAction("Choose Airframe", this)), selectAirframeAction(new QAction("Choose Airframe", this)),
setBatterySpecsAction(new QAction("Set Battery Options", this)), setBatterySpecsAction(new QAction("Set Battery Options", this)),
lowPowerModeEnabled(false), lowPowerModeEnabled(true),
m_ui(new Ui::UASView) m_ui(new Ui::UASView)
{ {
// FIXME XXX // FIXME XXX
...@@ -140,7 +140,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : ...@@ -140,7 +140,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
connect(refreshTimer, SIGNAL(timeout()), this, SLOT(refresh())); connect(refreshTimer, SIGNAL(timeout()), this, SLOT(refresh()));
if (lowPowerModeEnabled) if (lowPowerModeEnabled)
{ {
refreshTimer->start(updateInterval*10); refreshTimer->start(updateInterval*3);
} else { } else {
refreshTimer->start(updateInterval); refreshTimer->start(updateInterval);
} }
......
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