Can get tracking info from quad

groundStation/gui/MicroCART/controlworker.cpp

@@ -3,6 +3,7 @@
 #include "frontend_param.h"
 #include "frontend_source.h"
 #include "graph_blocks.h"
+#include "frontend_output.h"
 #include <QProcess>
 #include <err.h>
 
@@ -159,6 +160,28 @@ void ControlWorker::getParamValue(QString node, QString param)
     }
 }
 
+void ControlWorker::getNodeOutput(QString node)
+{
+    if (conn) {
+        frontend_node_data *nd = NULL;
+        size_t num_nodes = 0;
+
+        frontend_getnodes(conn, &nd, &num_nodes);        for (size_t i = 0; i < num_nodes; i++) {
+            if (QString(nd[i].name) == node) {
+                frontend_output_data od;
+                od.block = nd[i].block;
+
+                od.output = 0; // TODO: Get rid of this assumption
+
+                frontend_getoutput(conn, &od);
+
+                emit(gotNodeOutput(node, od.value));
+            }
+        }
+        frontend_free_node_data(nd, num_nodes);
+    }
+}
+
 void ControlWorker::setParamValue(QString node, QString param, float value)
 {
     if (conn) {

groundStation/gui/MicroCART/controlworker.h

@@ -16,6 +16,7 @@ signals:
     void gotNodes(QStringList nodes);
     void gotParams(QStringList params);
     void gotParamValue(QString node, QString param, float value);
+    void gotNodeOutput(QString node, float output);
     void gotConstantBlocks(QStringList blocks);
     void paramSet(QString node, QString param);
     void graphRendered(QString graph);
@@ -27,6 +28,7 @@ public slots:
     void getParams(QString node);
     void getParamValue(QString node, QString param);
     void setParamValue(QString node, QString name, float value);
+    void getNodeOutput(QString node);
 
 private:
     struct backend_conn * conn;

groundStation/gui/MicroCART/mainwindow.cpp

@@ -21,7 +21,8 @@ MainWindow::MainWindow(QWidget *parent) :
     nextSpTimer(new QTimer(this)),
     sp_x(0.0f),
     sp_y(0.0f),
-    sp_z(0.0f)
+    sp_z(0.0f),
+    trackerTimer(new QTimer(this))
 {
     ui->setupUi(this);
 
@@ -60,6 +61,7 @@ MainWindow::MainWindow(QWidget *parent) :
     connect(controlWorker, SIGNAL (gotNodes(QStringList)), this, SLOT (newNodes(QStringList)));
     connect(controlWorker, SIGNAL (gotParams(QStringList)), this, SLOT (newParams(QStringList)));
     connect(controlWorker, SIGNAL (gotParamValue(QString, QString, float)), this, SLOT (newParamValue(QString, QString, float)));
+    connect(controlWorker, SIGNAL (gotNodeOutput(QString, float)), this, SLOT (newNodeOutput(QString, float)));
     connect(controlWorker, SIGNAL (gotConstantBlocks(QStringList)), this, SLOT (newConstantBlocks(QStringList)));
     connect(controlWorker, SIGNAL (graphRendered(QString)), this, SLOT (newControlGraph(QString)));
     connect(controlWorker, SIGNAL (paramSet(QString, QString)), controlWorker, SLOT (getParamValue(QString, QString)));
@@ -69,6 +71,7 @@ MainWindow::MainWindow(QWidget *parent) :
     connect(ui->nodeSelect, SIGNAL (currentIndexChanged(QString)), controlWorker, SLOT (getParams(QString)));
     connect(this, SIGNAL (getParamValue(QString, QString)), controlWorker, SLOT (getParamValue(QString, QString)));
     connect(this, SIGNAL (setParamValue(QString, QString, float)), controlWorker, SLOT (setParamValue(QString, QString, float)));
+    connect(this, SIGNAL (getNodeOutput(QString)), controlWorker, SLOT (getNodeOutput(QString)));
 
     /* Connect and disconnect from backend when signals emitted */
     connect(this, SIGNAL (connectWorkers()), trackerWorker, SLOT (connectBackend()));
@@ -77,7 +80,6 @@ MainWindow::MainWindow(QWidget *parent) :
     connect(this, SIGNAL (disconnectWorkers()), controlWorker, SLOT (disconnectBackend()));
 
     /* Connect refresh button and refresh timer to tracker worker */
-    QTimer * trackerTimer = new QTimer(this);
     connect(trackerTimer, SIGNAL(timeout()), this, SLOT(updatePosAtt()));
     connect(ui->pbRefresh, SIGNAL (clicked()), this, SLOT (updatePosAtt()));
 
@@ -88,7 +90,7 @@ MainWindow::MainWindow(QWidget *parent) :
     connect(nextSpTimer, SIGNAL (timeout()), this, SLOT (on_pbNextSetpoint_clicked()));
 
     /* Start the things */
-    trackerTimer->start(300);
+    trackerTimer->start(100);
     workerThread->start();
     cwThread->start();
 
@@ -110,12 +112,12 @@ void MainWindow::updatePosAtt()
     if (ui->posattSrcVrpn->isChecked()) {
         emit(getPosAttFromBackend());
     } else if (ui->posattSrcQuad->isChecked()) {
-        emit(getParamValue(ui->xPositionSelect->currentText(), blockDefs[BLOCK_CONSTANT]->param_names[0]));
-        emit(getParamValue(ui->yPositionSelect->currentText(), blockDefs[BLOCK_CONSTANT]->param_names[0]));
-        emit(getParamValue(ui->zPositionSelect->currentText(), blockDefs[BLOCK_CONSTANT]->param_names[0]));
-        emit(getParamValue(ui->pAttitudeSelect->currentText(), blockDefs[BLOCK_CONSTANT]->param_names[0]));
-        emit(getParamValue(ui->rAttitudeSelect->currentText(), blockDefs[BLOCK_CONSTANT]->param_names[0]));
-        emit(getParamValue(ui->yAttitudeSelect->currentText(), blockDefs[BLOCK_CONSTANT]->param_names[0]));
+        emit(getNodeOutput(ui->xPositionSelect->currentText()));
+        emit(getNodeOutput(ui->yPositionSelect->currentText()));
+        emit(getNodeOutput(ui->zPositionSelect->currentText()));
+        emit(getNodeOutput(ui->pAttitudeSelect->currentText()));
+        emit(getNodeOutput(ui->rAttitudeSelect->currentText()));
+        emit(getNodeOutput(ui->yAttitudeSelect->currentText()));
     }
 }
 
@@ -250,6 +252,24 @@ void MainWindow::newConstantBlocks(QStringList blocks)
     }
 }
 
+void MainWindow::newNodeOutput(QString node, float val)
+{
+    /* Update the nav page if quad is set as the source for pos/att */
+    if (node == ui->xPositionSelect->currentText()) {
+        ui->xActual->setText(QString::number(val));
+    } else if (node == ui->yPositionSelect->currentText()) {
+        ui->yActual->setText(QString::number(val));
+    } else if (node == ui->zPositionSelect->currentText()) {
+        ui->zActual->setText(QString::number(val));
+    } else if (node == ui->pAttitudeSelect->currentText()) {
+        ui->pitchActual->setText(QString::number(val));
+    } else if (node == ui->rAttitudeSelect->currentText()) {
+        ui->rollActual->setText(QString::number(val));
+    } else if (node == ui->yAttitudeSelect->currentText()) {
+        ui->yawActual->setText(QString::number(val));
+    }
+}
+
 void MainWindow::newParams(QStringList params)
 {
     QComboBox * select = ui->paramSelect;
@@ -269,23 +289,6 @@ void MainWindow::newParamValue(QString node, QString param, float val)
     } else if (node == ui->zSetpointSelect->currentText()) {
         ui->zSetpoint->setText(QString::number(val));
     }
-
-    std::cout << node.toStdString() << " " << param.toStdString() << " " << val << std::endl;
-
-    /* Update the nav page if quad is set as the source for pos/att */
-    if (node == ui->xPositionSelect->currentText()) {
-        ui->xActual->setText(QString::number(val));
-    } else if (node == ui->yPositionSelect->currentText()) {
-        ui->yActual->setText(QString::number(val));
-    } else if (node == ui->zPositionSelect->currentText()) {
-        ui->zActual->setText(QString::number(val));
-    } else if (node == ui->pAttitudeSelect->currentText()) {
-        ui->pitchActual->setText(QString::number(val));
-    } else if (node == ui->rAttitudeSelect->currentText()) {
-        ui->rollActual->setText(QString::number(val));
-    } else if (node == ui->yAttitudeSelect->currentText()) {
-        ui->yawActual->setText(QString::number(val));
-    }
 }
 
 void MainWindow::on_paramSelect_currentIndexChanged(const QString &arg1)
@@ -476,3 +479,13 @@ void MainWindow::on_zSetpoint_returnPressed()
     emit (setParamValue(ui->zSetpointSelect->currentText(),
                         blockDefs[BLOCK_CONSTANT]->param_names[0], sp_z));
 }
+
+void MainWindow::on_posattSrcVrpn_clicked()
+{
+    trackerTimer->setInterval(100);
+}
+
+void MainWindow::on_posattSrcQuad_clicked()
+{
+    trackerTimer->setInterval(500);
+}

groundStation/gui/MicroCART/mainwindow.h

@@ -23,6 +23,7 @@ signals:
     void connectWorkers();
     void disconnectWorkers();
     void getParamValue(QString node, QString param);
+    void getNodeOutput(QString node);
     void setParamValue(QString node, QString param, float value);
     void getPosAttFromBackend();
 
@@ -42,6 +43,7 @@ private slots:
     void newNodes(QStringList blocks);
     void newParams(QStringList params);
     void newParamValue(QString node, QString param, float val);
+    void newNodeOutput(QString node, float output);
     void newConstantBlocks(QStringList blocks);
     void newControlGraph(QString graph);
 
@@ -80,6 +82,10 @@ private slots:
 
     void on_zSetpoint_returnPressed();
 
+    void on_posattSrcVrpn_clicked();
+
+    void on_posattSrcQuad_clicked();
+
 private:
     Ui::MainWindow *ui;
     QStandardItemModel * setpointList;
@@ -87,6 +93,7 @@ private:
     float sp_x;
     float sp_y;
     float sp_z;
+    QTimer * trackerTimer;
 };
 
 #endif // MAINWINDOW_H

groundStation/src/frontend/frontend_output.h

@@ -8,9 +8,15 @@
  *
  * Returns 0 on success, 1 on error
  */
+#ifdef __cplusplus
+extern "C" {
+#endif
 int frontend_getoutput(
 		struct backend_conn * conn,
 		struct frontend_output_data * output_data);
+#ifdef __cplusplus
+}
+#endif
 
 
-#endif /* __FRONTEND_OUTPUT_H */
\ No newline at end of file
+#endif /* __FRONTEND_OUTPUT_H */