Pushed parameter interface forward, now receiving of onboard params works, working on display and writing

qgroundcontrol.pri

@@ -86,6 +86,8 @@ macx {
 
 # GNU/Linux
 linux-g++ { 
+
+    CONFIG += debug
     
     debug {
         DESTDIR = $$BASEDIR

src/Core.cc

@@ -59,7 +59,7 @@ This file is part of the PIXHAWK project
 Core::Core(int &argc, char* argv[]) : QApplication(argc, argv)
 {
     this->setApplicationName("QGroundControl");
-    this->setApplicationVersion("v. 0.7.0");
+    this->setApplicationVersion("v. 0.7.0 (Beta)");
     this->setOrganizationName(QLatin1String("OpenMAV Association"));
     this->setOrganizationDomain("http://qgroundcontrol.org");
 

src/comm/MAVLinkProtocol.cc

@@ -138,13 +138,13 @@ QString MAVLinkProtocol::getName()
 }
 
 /** @return System id of this application */
-int getSystemId()
+int MAVLinkProtocol::getSystemId()
 {
     return MG::SYSTEM::ID;
 }
 
 /** @return Component id of this application */
-int getComponentId()
+int MAVLinkProtocol::getComponentId()
 {
     return MG::SYSTEM::COMPID;
 }

src/comm/MAVLinkSimulationLink.cc

@@ -143,9 +143,6 @@ void MAVLinkSimulationLink::mainloop()
     uint8_t stream[streamlength] = {};
 
     // Fake system values
-    uint8_t systemId = 220;
-    uint8_t componentId = 0;
-    uint16_t version = 1000;
 
     static float fullVoltage = 4.2 * 3;
     static float emptyVoltage = 3.35 * 3;
@@ -487,6 +484,12 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
     mavlink_message_t msg;
     mavlink_status_t comm;
 
+    uint8_t stream[2048];
+    int streampointer = 0;
+    mavlink_message_t ret;
+    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+    int bufferlength = 0;
+
     // Output all bytes as hex digits
     int i;
     for (i=0; i<size; i++)
@@ -546,6 +549,21 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
                     qDebug() << "\n" << "ROLL:" << control.roll << "PITCH:" << control.pitch;
                 }
                 break;
+            case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
+                {
+                    param_request_list_t read;
+                    message_param_request_list_decode(&msg, &read);
+                    // Output all params
+
+                    // Pack message and get size of encoded byte string
+                    message_param_value_pack(systemId, componentId, &msg, (int8_t*)"ROLL_K_P", 0.5f);
+                    // Allocate buffer with packet data
+                    bufferlength = message_to_send_buffer(buffer, &msg);
+
+                    //add data into datastream
+                    memcpy(stream+streampointer,buffer, bufferlength);
+                    streampointer+=bufferlength;
+                }
             }
 
 
@@ -555,6 +573,13 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
     }
     fprintf(stderr,"\n");
 
+    readyBufferMutex.lock();
+    for (int i = 0; i < streampointer; i++)
+    {
+        readyBuffer.enqueue(*(stream + i));
+    }
+    readyBufferMutex.unlock();
+
     // Update comm status
     status.packet_drop = comm.packet_rx_drop_count;
 

src/comm/MAVLinkSimulationLink.h

@@ -119,6 +119,10 @@ protected:
 
     void enqueue(uint8_t* stream, uint8_t* index, mavlink_message_t* msg);
 
+    static const uint8_t systemId = 220;
+    static const uint8_t componentId = 0;
+    static const uint16_t version = 1000;
+
 signals:
     void valueChanged(int uasId, QString curve, double value, quint64 usec);
 

src/uas/UAS.cc

@@ -44,6 +44,7 @@ This file is part of the PIXHAWK project
 #include <mavlink.h>
 
 UAS::UAS(MAVLinkProtocol* protocol, int id) :
+        uasId(id),
         startTime(MG::TIME::getGroundTimeNow()),
         commStatus(COMM_DISCONNECTED),
         name(""),
@@ -68,7 +69,6 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) :
         sendDropRate(0),
         unknownPackets()
 {
-    uasId = id;
     setBattery(LIPOLY, 3);
     mavlink = protocol;
 }
@@ -78,7 +78,7 @@ UAS::~UAS()
     delete links;
 }
 
-int UAS::getUASID()
+int UAS::getUASID() const
 {
     return uasId;
 }
@@ -326,6 +326,13 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                 emit localPositionChanged(this, pos.x, pos.y, pos.z, time);
             }
             break;
+            case MAVLINK_MSG_ID_PARAM_VALUE:
+                {
+                    param_value_t value;
+                    message_param_value_decode(&message, &value);
+                    emit parameterChanged(uasId, message.compid, QString((char*)value.param_id), value.param_value);
+                }
+                break;
         case MAVLINK_MSG_ID_DEBUG:
             emit valueChanged(uasId, QString("debug ") + QString::number(message_debug_get_ind(&message)), message_debug_get_value(&message), MG::TIME::getGroundTimeNow());
             break;
@@ -431,7 +438,7 @@ void UAS::sendMessage(LinkInterface* link, mavlink_message_t message)
 /**
  * @param value battery voltage
  */
-const float UAS::filterVoltage(float value)
+float UAS::filterVoltage(float value) const
 {
     return lpVoltage * 0.7f + value * 0.3f;
     /*
@@ -503,7 +510,7 @@ void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDesc
  * @return The uptime in milliseconds
  *
  **/
-quint64 UAS::getUptime()
+quint64 UAS::getUptime() const
 {
     if(startTime == 0) {
         return 0;
@@ -512,7 +519,7 @@ quint64 UAS::getUptime()
     }
 }
 
-int UAS::getCommunicationStatus()
+int UAS::getCommunicationStatus() const
 {
     return commStatus;
 }
@@ -528,7 +535,8 @@ void UAS::requestWaypoints()
 void UAS::requestParameters()
 {
     mavlink_message_t msg;
-    message_param_request_read_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, 0);
+    message_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg);
+    sendMessage(msg);
 }
 
 /**
@@ -727,7 +735,7 @@ void UAS::shutdown()
 /**
  * @return The name of this system as string in human-readable form
  */
-QString UAS::getUASName(void)
+QString UAS::getUASName(void) const
 {
     QString result;
     if (name == "")

src/uas/UAS.h

@@ -66,23 +66,23 @@ public:
     /* MANAGEMENT */
 
     /** @brief The name of the robot */
-    QString getUASName(void);
+    QString getUASName(void) const;
     /** @brief Get the unique system id */
-    int getUASID();
+    int getUASID() const;
     /** @brief The time interval the robot is switched on */
-    quint64 getUptime();
+    quint64 getUptime() const;
     /** @brief Get the status flag for the communication */
-    int getCommunicationStatus();
+    int getCommunicationStatus() const;
     /** @brief Add one measurement and get low-passed voltage */
-    const float filterVoltage(float value);
+    float filterVoltage(float value) const;
     /** @brief Get the links associated with this robot */
     QList<LinkInterface*>* getLinks();
 
 protected:
-    int type;
+    int uasId;                    ///< Unique system ID
+    int type;                     ///< UAS type (from type enum)
     quint64 startTime;            ///< The time the UAS was switched on
     CommStatus commStatus;        ///< Communication status
-    int uasId;                    ///< Unique system ID
     QString name;                 ///< Human-friendly name of the vehicle, e.g. bravo
     QList<LinkInterface*>* links; ///< List of links this UAS can be reached by
     MAVLinkProtocol* mavlink;     ///< Reference to the MAVLink instance

src/uas/UASInterface.h

@@ -60,13 +60,13 @@ public:
     /* MANAGEMENT */
 
     /** @brief The name of the robot **/
-    virtual QString getUASName() = 0;
+    virtual QString getUASName() const = 0;
     //virtual QColor getColor() = 0;
-    virtual int getUASID() = 0; ///< Get the ID of the connected UAS
+    virtual int getUASID() const = 0; ///< Get the ID of the connected UAS
     /** @brief The time interval the robot is switched on **/
-    virtual quint64 getUptime() = 0;
+    virtual quint64 getUptime() const = 0;
     /** @brief Get the status flag for the communication **/
-    virtual int getCommunicationStatus() = 0;
+    virtual int getCommunicationStatus() const = 0;
 
     /* COMMUNICATION FLAGS */
 
@@ -259,6 +259,7 @@ signals:
     void waypointSelected(int uasId, int id);
     void waypointReached(UASInterface* uas, int id);
     void autoModeChanged(bool autoMode);
+    void parameterChanged(int uas, int component, QString parameterName, float value);
     void detectionReceived(int uasId, QString patternPath, int x1, int y1, int x2, int y2, int x3, int y3, int x4, int y4, double confidence, bool detected);
     /**
      * @brief The battery status has been updated

src/ui/HDDisplay.cc

@@ -538,6 +538,7 @@ float HDDisplay::refLineWidthToPen(float line)
 
 void HDDisplay::updateValue(UASInterface* uas, QString name, double value, quint64 msec)
 {
+    Q_UNUSED(uas);
     //if (this->uas == uas)
     //{
         // Update mean

src/ui/HUD.cc

@@ -1390,6 +1390,7 @@ void HUD::setImageSize(int width, int height, int depth, int channels)
 
 void HUD::startImage(int imgid, int width, int height, int depth, int channels)
 {
+    Q_UNUSED(imgid);
     //qDebug() << "HUD: starting image (" << width << "x" << height << ", " << depth << "bits) with " << channels << "channels";
 
     // Copy previous image to screen if it hasn't been finished properly
@@ -1459,6 +1460,7 @@ void HUD::saveImage()
 
 void HUD::setPixels(int imgid, const unsigned char* imageData, int length, int startIndex)
 {
+    Q_UNUSED(imgid);
     //    qDebug() << "at" << __FILE__ << __LINE__ << ": Received startindex" << startIndex << "and length" << length << "(" << startIndex+length << "of" << rawExpectedBytes << "bytes)";
 
     if (imageStarted)

src/ui/ParameterInterface.cc

@@ -5,9 +5,11 @@
 #include "UASManager.h"
 #include "ui_ParameterInterface.h"
 
+#include <QDebug>
+
 ParameterInterface::ParameterInterface(QWidget *parent) :
-    QWidget(parent),
-    m_ui(new Ui::parameterWidget)
+        QWidget(parent),
+        m_ui(new Ui::parameterWidget)
 {
     m_ui->setupUi(this);
     connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*)));
@@ -16,14 +18,15 @@ ParameterInterface::ParameterInterface(QWidget *parent) :
 
     QString testData = "IMU\n ROLL_K_P\t0.527\n ROLL_K_I\t1.255\n PITCH_K_P\t0.621\n PITCH_K_I\t2.5545\n";
 
-    ParamTreeModel* model = new ParamTreeModel(testData);
+    tree = new ParamTreeModel();
 
-    QTreeView* tree = new QTreeView();
-    tree->setModel(model);
+    treeView = new QTreeView();
+    treeView->setModel(tree);
 
     QStackedWidget* stack = m_ui->stackedWidget;
-    stack->addWidget(tree);
-    stack->setCurrentWidget(tree);
+    stack->addWidget(treeView);
+    stack->setCurrentWidget(treeView);
+
 }
 
 ParameterInterface::~ParameterInterface()
@@ -38,10 +41,18 @@ ParameterInterface::~ParameterInterface()
 void ParameterInterface::addUAS(UASInterface* uas)
 {
     m_ui->vehicleComboBox->addItem(uas->getUASName());
+
+    mav = uas;
+
+    // Setup UI connections
+    connect(m_ui->readParamsButton, SIGNAL(clicked()), this, SLOT(requestParameterList()));
+
+    // Connect signals
+    connect(uas, SIGNAL(parameterChanged(int,int,QString,float)), this, SLOT(receiveParameter(int,int,QString,float)));
     //if (!paramViews.contains(uas))
     //{
-        //uasViews.insert(uas, new UASView(uas, this));
-        //listLayout->addWidget(uasViews.value(uas));
+    //uasViews.insert(uas, new UASView(uas, this));
+    //listLayout->addWidget(uasViews.value(uas));
 
     //}
 }
@@ -59,14 +70,17 @@ void ParameterInterface::requestParameterList()
  */
 void ParameterInterface::addComponent(UASInterface* uas, int component, QString componentName)
 {
-
+    Q_UNUSED(uas);
 }
 
-void ParameterInterface::receiveParameter(UASInterface* uas, int component, QString parameterName, float value)
+void ParameterInterface::receiveParameter(int uas, int component, QString parameterName, float value)
 {
-  // Insert parameter into map
-
-  // Refresh view
+    Q_UNUSED(uas);
+    qDebug() << "RECEIVED PARAMETER" << component << parameterName << value;
+    // Insert parameter into map
+    tree->appendParam(component, parameterName, value);
+    // Refresh view
+    treeView->setModel(tree);
 }
 
 /**
@@ -77,7 +91,7 @@ void ParameterInterface::receiveParameter(UASInterface* uas, int component, QStr
  */
 void ParameterInterface::setParameter(UASInterface* uas, int component, QString parameterName, float value)
 {
-
+    Q_UNUSED(uas);
 }
 
 /**

src/ui/ParameterInterface.h

@@ -2,8 +2,10 @@
 #define PARAMETERINTERFACE_H
 
 #include <QtGui/QWidget>
+#include <QtGui/QTreeView>
 #include "ui_ParameterInterface.h"
 #include "UASInterface.h"
+#include "ParamTreeModel.h"
 
 namespace Ui {
     class ParameterInterface;
@@ -18,7 +20,7 @@ public:
 public slots:
     void addUAS(UASInterface* uas);
     void addComponent(UASInterface* uas, int component, QString componentName);
-    void receiveParameter(UASInterface* uas, int component, QString parameterName, float value);
+    void receiveParameter(int uas, int component, QString parameterName, float value);
     void requestParameterList();
     void setParameter(UASInterface* uas, int component, QString parameterName, float value);
     void commitParameter(UASInterface* uas, int component, QString parameterName, float value);
@@ -27,6 +29,8 @@ protected:
     virtual void changeEvent(QEvent *e);
 
     UASInterface* mav;
+    ParamTreeModel* tree;
+    QTreeView* treeView;
 
 private:
     Ui::parameterWidget *m_ui;

src/ui/param/ParamTreeItem.cc

@@ -32,13 +32,11 @@ This file is part of the PIXHAWK project
 
 #include "ParamTreeItem.h"
 
-ParamTreeItem::ParamTreeItem(int id, QString name, float value, ParamTreeItem* parent)
+ParamTreeItem::ParamTreeItem(QString name, float value, ParamTreeItem* parent)
 {
     parentItem = parent;
-    itemData = QList<QVariant>();
-    itemData.append(id);
-    itemData.append(name);
-    itemData.append(value);
+    paramName = name;
+    paramValue = value;
 }
 
 ParamTreeItem::ParamTreeItem(const QList<QVariant> &data, ParamTreeItem *parent)
@@ -69,12 +67,35 @@ int ParamTreeItem::childCount() const
 
 int ParamTreeItem::columnCount() const
 {
-    return itemData.count();
+    return 2;// itemData.count();
+}
+
+QString ParamTreeItem::getParamName()
+{
+    return paramName;
+}
+
+float ParamTreeItem::getParamValue()
+{
+    return paramValue;
 }
 
 QVariant ParamTreeItem::data(int column) const
 {
-    return itemData.value(column);
+    QVariant ret;
+    switch (column)
+    {
+    case 0:
+        ret.setValue(paramName);
+        break;
+    case 1:
+        ret.setValue(paramValue);
+        break;
+    default:
+        ret.setValue(QString(""));
+        break;
+    }
+    //return itemData.value(column);
 }
 
 ParamTreeItem *ParamTreeItem::parent()

src/ui/param/ParamTreeItem.h

@@ -40,11 +40,13 @@ This file is part of the PIXHAWK project
 class ParamTreeItem
 {
 public:
-    ParamTreeItem(int id, QString name, float value, ParamTreeItem* parent = 0);
+    ParamTreeItem(QString name, float value, ParamTreeItem* parent = 0);
     ParamTreeItem(const QList<QVariant> &data, ParamTreeItem *parent = 0);
     ~ParamTreeItem();
 
     void appendChild(ParamTreeItem *child);
+    QString getParamName();
+    float getParamValue();
 
     ParamTreeItem *child(int row);
     int childCount() const;
@@ -53,6 +55,10 @@ public:
     int row() const;
     ParamTreeItem *parent();
 
+    protected:
+    QString paramName;
+    float paramValue;
+
 private:
     QList<ParamTreeItem*> childItems;
     QList<QVariant> itemData;

src/ui/param/ParamTreeModel.cc

@@ -38,7 +38,7 @@ ParamTreeModel::ParamTreeModel(QObject *parent)
     components()
 {
     QList<QVariant> rootData;
-    rootData << tr("ID") << tr("Parameter") << tr("Value");
+    rootData << tr("Parameter") << tr("Value");
     rootItem = new ParamTreeItem(rootData);
     //setupModelData(data.split(QString("\n")), rootItem);
 }
@@ -153,12 +153,12 @@ void ParamTreeModel::appendComponent(int componentId, QString name)
 {
     if (!components.contains(componentId))
     {
-        ParamTreeItem* item = new ParamTreeItem(componentId, name, 0, rootItem);
+        ParamTreeItem* item = new ParamTreeItem(name + QString("(") + QString::number(componentId) + QString(")"), 0, rootItem);
         components.insert(componentId, item);
     }
 }
 
-void ParamTreeModel::appendParam(int componentId, int id, QString name, float value)
+void ParamTreeModel::appendParam(int componentId, QString name, float value)
 {
     ParamTreeItem* comp = components.value(componentId);
     // If component does not exist yet
@@ -168,7 +168,8 @@ void ParamTreeModel::appendParam(int componentId, int id, QString name, float va
         comp = components.value(componentId);
     }
     // FIXME Children may be double here
-    comp->appendChild(new ParamTreeItem(id, name, value, comp));
+    comp->appendChild(new ParamTreeItem(name, value, comp));
+    qDebug() << __FILE__ << __LINE__ << "Added param" << name << value << "for component" << comp->getParamName();
 }
 
 void ParamTreeModel::setupModelData(const QStringList &lines, ParamTreeItem *parent)

src/ui/param/ParamTreeModel.h

@@ -58,7 +58,7 @@ public:
 
 public slots:
     /** @brief Add a new parameter */
-    void appendParam(int componentId, int id, QString name, float value);
+    void appendParam(int componentId, QString name, float value);
     /** @brief Add a new component for this system */
     void appendComponent(int componentId, QString name);
 

src/ui/uas/UASInfoWidget.cc

@@ -146,6 +146,7 @@ void UASInfoWidget::updateCPULoad(UASInterface* uas, double load)
 
 void UASInfoWidget::updateDropRate(int sysId, float receiveDrop, float sendDrop)
 {
+    Q_UNUSED(sysId);
     ui.receiveLossBar->setValue(receiveDrop * 100.0f);
     ui.sendLossBar->setValue(sendDrop * 100.0f);
 }
@@ -187,6 +188,7 @@ void UASInfoWidget::updateDropRate(int sysId, float receiveDrop, float sendDrop)
 
 void UASInfoWidget::setVoltage(UASInterface* uas, double voltage)
 {
+    Q_UNUSED(uas);
     this->voltage = voltage;
 }
 

src/ui/uas/UASView.cc

@@ -137,11 +137,13 @@ void UASView::setUASasActive(bool active)
 
 void UASView::updateMode(int sysId, QString status, QString description)
 {
+    Q_UNUSED(description);
     if (sysId == this->uas->getUASID()) m_ui->modeLabel->setText(status);
 }
 
 void UASView::mouseDoubleClickEvent (QMouseEvent * event)
 {
+    Q_UNUSED(event);
     UASManager::instance()->setActiveUAS(uas);
     qDebug() << __FILE__ << __LINE__ << "DOUBLECLICKED";
 }
@@ -214,6 +216,7 @@ void UASView::setSystemType(UASInterface* uas, unsigned int systemType)
 
 void UASView::updateLocalPosition(UASInterface* uas, double x, double y, double z, quint64 usec)
 {
+    Q_UNUSED(usec);
     if (uas == this->uas)
     {
         QString position;