diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 7db59f38b7e1a883ab18d70701e0f7d761d293b1..f1d37c7fa56e7346996dbb975ba8cefcdc2092fa 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -198,5 +198,6 @@ SOURCES += src/main.cc \ src/ui/watchdog/WatchdogProcessView.cc \ src/ui/watchdog/WatchdogView.cc \ src/uas/UASWaypointManager.cc \ - src/ui/HSIDisplay.cc + src/ui/HSIDisplay.cc \ + src/QGC.cc RESOURCES = mavground.qrc diff --git a/src/QGC.cc b/src/QGC.cc new file mode 100644 index 0000000000000000000000000000000000000000..fe78d323d918b984d5318382d85e09ae70d25b92 --- /dev/null +++ b/src/QGC.cc @@ -0,0 +1,14 @@ +#include "QGC.h" + +namespace QGC { + +quint64 groundTimeUsecs() +{ + QDateTime time = QDateTime::currentDateTime(); + time = time.toUTC(); + /* Return seconds and milliseconds, in milliseconds unit */ + quint64 microseconds = time.toTime_t() * static_cast(1000000); + return static_cast(microseconds + (time.time().msec()*1000)); +} + +} diff --git a/src/QGC.h b/src/QGC.h index 5dfdd881add82475179d3035e8bd3d4fbd1922d0..6478ae00b1355f2181828db57c020867acf3cd4b 100644 --- a/src/QGC.h +++ b/src/QGC.h @@ -8,14 +8,8 @@ namespace QGC { const QColor ColorCyan(55, 154, 195); - static quint64 groundTimeUsecs() - { - QDateTime time = QDateTime::currentDateTime(); - time = time.toUTC(); - /* Return seconds and milliseconds, in milliseconds unit */ - quint64 microseconds = time.toTime_t() * static_cast(1000000); - return static_cast(microseconds + (time.time().msec()*1000)); - } + /** @brief Get the current ground time in microseconds */ + quint64 groundTimeUsecs(); } #endif // QGC_H diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index 7892a5f4b2eb54d1c999dbdd737b11746e116356..1f867fe796f1b235a11f473670991dcac05b5a02 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -377,9 +377,20 @@ void MAVLinkSimulationLink::mainloop() rate10hzCounter = 1; // Move X Position - x += sin(QGC::groundTimeUsecs()); - y += sin(QGC::groundTimeUsecs()); - z += sin(QGC::groundTimeUsecs()); + x += sin(QGC::groundTimeUsecs()) * 0.1f; + y += sin(QGC::groundTimeUsecs()) * 0.1f; + z += sin(QGC::groundTimeUsecs()) * 0.1f; + + x = (x > 1.0f) ? 1.0f : x; + y = (y > 1.0f) ? 1.0f : y; + z = (z > 1.0f) ? 1.0f : z; + // Send back new setpoint + mavlink_message_t ret; + mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, spX, spY, spZ, spYaw); + bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); + //add data into datastream + memcpy(stream+streampointer,buffer, bufferlength); + streampointer += bufferlength; } // 1 HZ TASKS diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 19b1686163dda9e8b30b9b8e20de27b528f15d66..8b979c17e8d52fa4b2d59d811ed8562171041ea4 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -492,7 +492,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw) { mavlink_message_t msg; - + mavlink_msg_position_control_setpoint_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, 0, x, y, z, yaw); + sendMessage(msg); } quint64 UAS::getUnixTime(quint64 time) diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 8747d9a7522a5555bdf595505e1b303005bfd0ca..a21c4ee4d8934595b8bb4f7e19d7a74c67b201a4 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -205,6 +205,8 @@ public slots: virtual void enableRawControllerDataTransmission(bool enabled) = 0; virtual void enableRawSensorFusionTransmission(bool enabled) = 0; + virtual void setLocalPositionSetpoint(float x, float y, float z, float yaw) = 0; + protected: QColor color; diff --git a/src/ui/HDDisplay.cc b/src/ui/HDDisplay.cc index b866ca6f1ccb567e00ff96ce14683a826c7bfaa8..0b7574fb9ade780bce5ee8d1aa6d8cc979eaa8fa 100644 --- a/src/ui/HDDisplay.cc +++ b/src/ui/HDDisplay.cc @@ -210,6 +210,7 @@ void HDDisplay::setActiveUAS(UASInterface* uas) // Setup communication connect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64))); //} + this->uas = uas; } /** diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 9db24bd6c147b7698e12feb02015ce7ba5dc6ae9..38c2cc372558b1b4578e729f6416ec0a697826eb 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -392,7 +392,11 @@ void HSIDisplay::setBodySetpointCoordinateXY(double x, double y) uiXSetCoordinate = sp.x(); uiYSetCoordinate = sp.y(); - qDebug() << "Setting new setpoint at x: " << x << "metric y:" << y; + if (uas) + { + uas->setLocalPositionSetpoint(uiXSetCoordinate, uiYSetCoordinate, uiZSetCoordinate, uiYawSet); + qDebug() << "Setting new setpoint at x: " << x << "metric y:" << y; + } } void HSIDisplay::setBodySetpointCoordinateZ(double z) diff --git a/src/ui/QGCParamWidget.cc b/src/ui/QGCParamWidget.cc index 6d29661d0f037ac6399622da90026f29df3b2034..26e1ec44ce667baa836f3cd606f631cf1237880c 100644 --- a/src/ui/QGCParamWidget.cc +++ b/src/ui/QGCParamWidget.cc @@ -44,7 +44,8 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) : QWidget(parent), mav(uas), components(new QMap()), - changedValues()//QMap* >()) + paramGroups(), + changedValues() { // Create tree widget tree = new QTreeWidget(this); @@ -107,15 +108,22 @@ UASInterface* QGCParamWidget::getUAS() void QGCParamWidget::addComponent(int uas, int component, QString componentName) { Q_UNUSED(uas); - QStringList list; - list.append(componentName); - list.append(QString::number(component)); - QTreeWidgetItem* comp = new QTreeWidgetItem(list); - bool updated = false; - if (components->contains(component)) updated = true; - components->insert(component, comp); - if (!updated) + if (components->contains(component)) { + // Update existing + components->value(component)->setData(0, Qt::DisplayRole, componentName); + components->value(component)->setData(1, Qt::DisplayRole, QString::number(component)); + } + else + { + // Add new + QStringList list; + list.append(componentName); + list.append(QString::number(component)); + QTreeWidgetItem* comp = new QTreeWidgetItem(list); + components->insert(component, comp); + // Create grouping and update maps + paramGroups.insert(component, new QMap()); tree->addTopLevelItem(comp); tree->update(); } @@ -130,15 +138,6 @@ void QGCParamWidget::addParameter(int uas, int component, QString parameterName, { Q_UNUSED(uas); // Insert parameter into map - - QString splitToken = "_"; - // Check if auto-grouping can work - /* - if (parameterName.contains(splitToken)) - { - QString parent = parameterName.section(splitToken, 0, 0, QString::SectionSkipEmpty); - QString children = parameterName.section(splitToken, 1, -1, QString::SectionSkipEmpty); - }*/ QStringList plist; plist.append(parameterName); plist.append(QString::number(value)); @@ -150,27 +149,46 @@ void QGCParamWidget::addParameter(int uas, int component, QString parameterName, addComponent(uas, component, "Component #" + QString::number(component)); } - bool found = false; - QTreeWidgetItem* parent = components->value(component); - for (int i = 0; i < parent->childCount(); i++) + QString splitToken = "_"; + // Check if auto-grouping can work + if (parameterName.contains(splitToken)) { - QTreeWidgetItem* child = parent->child(i); - QString key = child->data(0, Qt::DisplayRole).toString(); - if (key == parameterName) + QString parent = parameterName.section(splitToken, 0, 0, QString::SectionSkipEmpty); + QMap* compParamGroups = paramGroups.value(component); + if (!compParamGroups->contains(parent)) { - qDebug() << "UPDATED CHILD"; - child->setData(1, Qt::DisplayRole, value); - found = true; + // Insert group item + QStringList glist; + glist.append(parent); + QTreeWidgetItem* item = new QTreeWidgetItem(glist); + compParamGroups->insert(parent, item); + components->value(component)->addChild(item); } } - - if (!found) + else { - components->value(component)->addChild(item); - item->setFlags(item->flags() | Qt::ItemIsEditable); + bool found = false; + QTreeWidgetItem* parent = components->value(component); + for (int i = 0; i < parent->childCount(); i++) + { + QTreeWidgetItem* child = parent->child(i); + QString key = child->data(0, Qt::DisplayRole).toString(); + if (key == parameterName) + { + //qDebug() << "UPDATED CHILD"; + child->setData(1, Qt::DisplayRole, value); + found = true; + } + } + + if (!found) + { + components->value(component)->addChild(item); + item->setFlags(item->flags() | Qt::ItemIsEditable); + } + //connect(item, SIGNAL()) + tree->expandAll(); } - //connect(item, SIGNAL()) - tree->expandAll(); tree->update(); } @@ -213,6 +231,7 @@ void QGCParamWidget::parameterItemChanged(QTreeWidgetItem* current, int column) { qDebug() << "PARAM CHANGED: COMP:" << key << "KEY:" << str << "VALUE:" << value; map->insert(str, value); + // FIXME CHANGE COLOR OF CHANGED PARAM } } } diff --git a/src/ui/QGCParamWidget.h b/src/ui/QGCParamWidget.h index 13c1bf434286d4757f38197abf3d3711eb65d048..55dfd441b9ac6f62fc89dea9c2e7a743c60a77da 100644 --- a/src/ui/QGCParamWidget.h +++ b/src/ui/QGCParamWidget.h @@ -73,6 +73,7 @@ protected: UASInterface* mav; ///< The MAV this widget is controlling QTreeWidget* tree; ///< The parameter tree QMap* components; ///< The list of components + QMap* > paramGroups; ///< Parameter groups QMap* > changedValues; ///< Changed values }; diff --git a/src/ui/UASControl.ui b/src/ui/UASControl.ui index 127e85e4459431ffebbbdf783faef822175ee4cc..c1a095749e0cb2cde8db34ff6ce149fb01b79d85 100644 --- a/src/ui/UASControl.ui +++ b/src/ui/UASControl.ui @@ -13,7 +13,7 @@ Form - + 6