Working on parameter groups

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

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<quint64>(1000000);
+    return static_cast<quint64>(microseconds + (time.time().msec()*1000));
+}
+
+}

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<quint64>(1000000);
-        return static_cast<quint64>(microseconds + (time.time().msec()*1000));
-    }
+    /** @brief Get the current ground time in microseconds */
+    quint64 groundTimeUsecs();
 }
 
 #endif // QGC_H

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

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)

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;
 

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;
 }
 
 /**

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)

src/ui/QGCParamWidget.cc

@@ -44,7 +44,8 @@ QGCParamWidget::QGCParamWidget(UASInterface* uas, QWidget *parent) :
         QWidget(parent),
         mav(uas),
         components(new QMap<int, QTreeWidgetItem*>()),
-        changedValues()//QMap<int, QMap<QString, float>* >())
+        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<QString, QTreeWidgetItem*>());
         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<QString, QTreeWidgetItem*>* 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
                 }
             }
         }

src/ui/QGCParamWidget.h

@@ -73,6 +73,7 @@ protected:
     UASInterface* mav;  ///< The MAV this widget is controlling
     QTreeWidget* tree;  ///< The parameter tree
     QMap<int, QTreeWidgetItem*>* components; ///< The list of components
+    QMap<int, QMap<QString, QTreeWidgetItem*>* > paramGroups; ///< Parameter groups
     QMap<int, QMap<QString, float>* > changedValues; ///< Changed values
 
 };

src/ui/UASControl.ui

@@ -13,7 +13,7 @@
   <property name="windowTitle">
    <string>Form</string>
   </property>
-  <layout class="QGridLayout" name="gridLayout">
+  <layout class="QGridLayout" name="gridLayout" rowminimumheight="0,0,0,0,0,0,100">
    <property name="leftMargin">
     <number>6</number>
    </property>