diff --git a/groundStation/gui/MicroCART/MicroCART.pro b/groundStation/gui/MicroCART/MicroCART.pro
index f8aa76c36c28c220e464900187f77d1a8b9225bf..094787c0fc0a27ed7504a4aaeed3448e5a849aaf 100644
--- a/groundStation/gui/MicroCART/MicroCART.pro
+++ b/groundStation/gui/MicroCART/MicroCART.pro
@@ -16,13 +16,11 @@ INCLUDEPATH += ../../src/frontend/
SOURCES += main.cpp\
mainwindow.cpp \
- wrappers.c \
trackerworker.cpp \
controlworker.cpp \
quaditem.cpp
HEADERS += mainwindow.h \
- wrappers.h \
trackerworker.h \
controlworker.h \
quaditem.h
diff --git a/groundStation/gui/MicroCART/controlworker.cpp b/groundStation/gui/MicroCART/controlworker.cpp
index 6cd8b01c08877ad2b17ef8c27307006f361cb265..0127485bcbd97be98b40c969001ee2bad26cbb51 100644
--- a/groundStation/gui/MicroCART/controlworker.cpp
+++ b/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>
@@ -40,7 +41,7 @@ void ControlWorker::getNodes()
QStringList const_block_nodes;
for (size_t i = 0; i < num_nodes; i++) {
nodes.append(QString(nd[i].name));
- if (nd[i].type == BLOCK_CONSTANT) {
+ if ((nd[i].type == BLOCK_CONSTANT) || (nd[i].type == BLOCK_ADD)) {
const_block_nodes.append(nd[i].name);
}
}
@@ -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) {
diff --git a/groundStation/gui/MicroCART/controlworker.h b/groundStation/gui/MicroCART/controlworker.h
index 9a2c6de5101100953c3cd65eb8fa5abc5998ba08..c32f46e0d8c2dfb6506d44feed7c89c1731ce87f 100644
--- a/groundStation/gui/MicroCART/controlworker.h
+++ b/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;
diff --git a/groundStation/gui/MicroCART/mainwindow.cpp b/groundStation/gui/MicroCART/mainwindow.cpp
index b3ee612dd1d3eedeaa233ccd380f1159662599f4..74b375419a9b593bc10717fa634de6e2edba080b 100644
--- a/groundStation/gui/MicroCART/mainwindow.cpp
+++ b/groundStation/gui/MicroCART/mainwindow.cpp
@@ -8,22 +8,21 @@
#include <QProcessEnvironment>
#include <QPixmap>
-#include "wrappers.h"
#include "trackerworker.h"
#include "controlworker.h"
#include "graph_blocks.h"
#include "quaditem.h"
+#include <iostream>
MainWindow::MainWindow(QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainWindow),
- backendPid(0),
- backendPipe(-1),
setpointList(new QStandardItemModel(this)),
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);
@@ -62,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)));
@@ -71,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()));
@@ -79,16 +80,17 @@ 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()), trackerWorker, SLOT(process()));
- connect(ui->pbRefresh, SIGNAL (clicked()), trackerWorker, SLOT (process()));
+ connect(trackerTimer, SIGNAL(timeout()), this, SLOT(updatePosAtt()));
+ connect(ui->pbRefresh, SIGNAL (clicked()), this, SLOT (updatePosAtt()));
+
+ connect(this, SIGNAL(getPosAttFromBackend()), trackerWorker, SLOT(process()));
/* Timer used for next setpoint */
nextSpTimer->setSingleShot(true);
connect(nextSpTimer, SIGNAL (timeout()), this, SLOT (on_pbNextSetpoint_clicked()));
/* Start the things */
- trackerTimer->start(300);
+ trackerTimer->start(100);
workerThread->start();
cwThread->start();
@@ -105,9 +107,17 @@ MainWindow::~MainWindow()
delete ui;
}
-void MainWindow::updateConsole()
+void MainWindow::updatePosAtt()
{
- if (backendPipe != -1) {
+ if (ui->posattSrcVrpn->isChecked()) {
+ emit(getPosAttFromBackend());
+ } else if (ui->posattSrcQuad->isChecked()) {
+ 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()));
}
}
@@ -133,10 +143,8 @@ void MainWindow::updateTracker(float x, float y, float z, float p, float r, floa
void MainWindow::on_pbStart_clicked()
{
QProcessEnvironment::systemEnvironment().insert("UCART_SOCKET", ui->socketPath->text());
- this->backendPid = startBackend(ui->backendPath_2->text().toStdString().c_str(), &backendPipe);
ui->pbStart->setEnabled(false);
ui->pbStop->setEnabled(true);
- backendState = 1;
}
void MainWindow::on_pbConnect_clicked()
@@ -146,21 +154,14 @@ void MainWindow::on_pbConnect_clicked()
ui->pbConnect->setEnabled(false);
ui->pbStop->setEnabled(true);
emit(connectWorkers());
- backendState = 1;
}
void MainWindow::on_pbStop_clicked()
{
emit(disconnectWorkers());
- if (backendPid) {
- stopBackend(backendPid, backendPipe);
- backendPipe = -1;
- backendPid = 0;
- }
ui->pbStart->setEnabled(true);
ui->pbConnect->setEnabled(true);
ui->pbStop->setEnabled(false);
- backendState = 0;
}
void MainWindow::on_chooseBackend_clicked()
@@ -185,30 +186,93 @@ void MainWindow::newNodes(QStringList blocks)
void MainWindow::newConstantBlocks(QStringList blocks)
{
- QComboBox * xSelect = ui->xSetpointSelect;
- xSelect->clear();
- xSelect->addItems(blocks);
+ ui->xSetpointSelect->clear();
+ ui->xSetpointSelect->addItems(blocks);
+
+ ui->ySetpointSelect->clear();
+ ui->ySetpointSelect->addItems(blocks);
+
+ ui->zSetpointSelect->clear();
+ ui->zSetpointSelect->addItems(blocks);
+
+ ui->yawSetpointSelect->clear();
+ ui->yawSetpointSelect->addItems(blocks);
+
+ ui->xPositionSelect->clear();
+ ui->xPositionSelect->addItems(blocks);
+
+ ui->yPositionSelect->clear();
+ ui->yPositionSelect->addItems(blocks);
- QComboBox * ySelect = ui->ySetpointSelect;
- ySelect->clear();
- ySelect->addItems(blocks);
+ ui->zPositionSelect->clear();
+ ui->zPositionSelect->addItems(blocks);
- QComboBox * zSelect = ui->zSetpointSelect;
- zSelect->clear();
- zSelect->addItems(blocks);
+ ui->pAttitudeSelect->clear();
+ ui->pAttitudeSelect->addItems(blocks);
+
+ ui->rAttitudeSelect->clear();
+ ui->rAttitudeSelect->addItems(blocks);
+
+ ui->yAttitudeSelect->clear();
+ ui->yAttitudeSelect->addItems(blocks);
for (ssize_t i = 0; i < blocks.size(); i++) {
if (blocks[i].contains("setpoint", Qt::CaseInsensitive) || blocks[i].contains("sp", Qt::CaseInsensitive)) {
if (blocks[i].contains("x ", Qt::CaseInsensitive)) {
- xSelect->setCurrentIndex(i);
+ ui->xSetpointSelect->setCurrentIndex(i);
}
if (blocks[i].contains("y ", Qt::CaseInsensitive)) {
- ySelect->setCurrentIndex(i);
+ ui->ySetpointSelect->setCurrentIndex(i);
}
if (blocks[i].contains("z ", Qt::CaseInsensitive) || blocks[i].contains("alt", Qt::CaseInsensitive)) {
- zSelect->setCurrentIndex(i);
+ ui->zSetpointSelect->setCurrentIndex(i);
+ }
+ if (blocks[i].contains("yaw", Qt::CaseInsensitive) || blocks[i].contains("alt", Qt::CaseInsensitive)) {
+ ui->yawSetpointSelect->setCurrentIndex(i);
}
}
+
+ if (blocks[i] == QString("Pitch")) {
+ ui->pAttitudeSelect->setCurrentIndex(i);
+ }
+
+ if (blocks[i] == QString("Roll")) {
+ ui->rAttitudeSelect->setCurrentIndex(i);
+ }
+
+ if (blocks[i] == QString("Yaw")) {
+ ui->yAttitudeSelect->setCurrentIndex(i);
+ }
+
+ if (blocks[i] == QString("Lidar")) {
+ ui->zPositionSelect->setCurrentIndex(i);
+ }
+
+ if (blocks[i] == QString("OF X Trim Add")) {
+ ui->xPositionSelect->setCurrentIndex(i);
+ }
+
+ if (blocks[i] == QString("OF Y Trim Add")) {
+ ui->yPositionSelect->setCurrentIndex(i);
+ }
+ }
+}
+
+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));
}
}
@@ -247,17 +311,29 @@ void MainWindow::on_paramValue_returnPressed()
void MainWindow::sendSetpoints()
{
- sp_x = ui->xSetpoint->text().toFloat();
- emit (setParamValue(ui->xSetpointSelect->currentText(),
- blockDefs[BLOCK_CONSTANT]->param_names[0], sp_x));
+ if (ui->sendX->isChecked()) {
+ sp_x = ui->xSetpoint->text().toFloat();
+ emit (setParamValue(ui->xSetpointSelect->currentText(),
+ blockDefs[BLOCK_CONSTANT]->param_names[0], sp_x));
+ }
- sp_y = ui->ySetpoint->text().toFloat();
- emit (setParamValue(ui->ySetpointSelect->currentText(),
- blockDefs[BLOCK_CONSTANT]->param_names[0], sp_y));
+ if (ui->sendY->isChecked()) {
+ sp_y = ui->ySetpoint->text().toFloat();
+ emit (setParamValue(ui->ySetpointSelect->currentText(),
+ blockDefs[BLOCK_CONSTANT]->param_names[0], sp_y));
+ }
- sp_z = ui->zSetpoint->text().toFloat();
- emit (setParamValue(ui->zSetpointSelect->currentText(),
- blockDefs[BLOCK_CONSTANT]->param_names[0], sp_z));
+ if (ui->sendZ->isChecked()) {
+ sp_z = ui->zSetpoint->text().toFloat();
+ emit (setParamValue(ui->zSetpointSelect->currentText(),
+ blockDefs[BLOCK_CONSTANT]->param_names[0], sp_z));
+ }
+
+ if (ui->sendYaw->isChecked()) {
+ emit (setParamValue(ui->yawSetpointSelect->currentText(),
+ blockDefs[BLOCK_CONSTANT]->param_names[0],
+ ui->yawSetpoint->text().toFloat()));
+ }
}
void MainWindow::on_pbAppendSetpoint_clicked()
@@ -284,13 +360,14 @@ void MainWindow::on_pbNextSetpoint_clicked()
void MainWindow::sendSelectedSetpoint()
{
if (ui->setpointList->currentIndex().isValid()) {
- QRegExp regex("\\[(.*), (.*), (.*)\\]");
+ QRegExp regex("\\[(.*), (.*), (.*), (.*)\\]");
int row = ui->setpointList->currentIndex().row();
regex.indexIn(setpointList->item(row)->text());
ui->xSetpoint->setText(regex.cap(1));
ui->ySetpoint->setText(regex.cap(2));
ui->zSetpoint->setText(regex.cap(3));
+ ui->yawSetpoint->setText(regex.cap(4));
sendSetpoints();
}
@@ -301,6 +378,7 @@ void MainWindow::on_pbActualToSetpoint_clicked()
ui->xSetpoint->setText(ui->xActual->text());
ui->ySetpoint->setText(ui->yActual->text());
ui->zSetpoint->setText(ui->zActual->text());
+ ui->yawSetpoint->setText(ui->yawActual->text());
}
void MainWindow::on_pbDeleteSetpoint_clicked()
@@ -318,8 +396,9 @@ void MainWindow::newControlGraph(QString graph)
void MainWindow::on_pbActualToWaypoint_clicked()
{
QString str("[" + ui->xActual->text() + ", "+
- ui->yActual->text() + ", " +
- ui->zActual->text() + "]");
+ ui->yActual->text() + ", " +
+ ui->zActual->text() + ", " +
+ ui->yawActual->text() + "]");
setpointList->appendRow(new QStandardItem(str));
}
@@ -354,16 +433,21 @@ void MainWindow::on_pbInsertSetpoint_clicked()
}
QString str("[" + ui->xSetpoint->text() + ", "+
- ui->ySetpoint->text() + ", " +
- ui->zSetpoint->text() + "]");
+ ui->ySetpoint->text() + ", " +
+ ui->zSetpoint->text() + ", " +
+ ui->yawSetpoint->text() + "]");
setpointList->insertRow(current, new QStandardItem(str));
}
void MainWindow::on_pbSaveWaypoints_clicked()
{
- QString savePath = QFileDialog::getSaveFileName(this);
+ QString filter = "Waypoints (*.wpt)";
+ QString savePath = QFileDialog::getSaveFileName(this, tr("Save waypoint"), QString(), filter, &filter, QFileDialog::DontUseNativeDialog);
if (!savePath.isEmpty()) {
+ QFileInfo finfo(savePath);
+ if (finfo.suffix().isNull())
+ savePath.append(".wpt");
QFile f(savePath);
f.open(QIODevice::WriteOnly | QIODevice::Text);
for (int i= 0; i < setpointList->rowCount(); i++) {
@@ -417,3 +501,20 @@ void MainWindow::on_zSetpoint_returnPressed()
emit (setParamValue(ui->zSetpointSelect->currentText(),
blockDefs[BLOCK_CONSTANT]->param_names[0], sp_z));
}
+
+void MainWindow::on_yawSetpoint_returnPressed()
+{
+ emit (setParamValue(ui->yawSetpointSelect->currentText(),
+ blockDefs[BLOCK_CONSTANT]->param_names[0],
+ ui->yawSetpoint->text().toFloat()));
+}
+
+void MainWindow::on_posattSrcVrpn_clicked()
+{
+ trackerTimer->setInterval(100);
+}
+
+void MainWindow::on_posattSrcQuad_clicked()
+{
+ trackerTimer->setInterval(500);
+}
diff --git a/groundStation/gui/MicroCART/mainwindow.h b/groundStation/gui/MicroCART/mainwindow.h
index 62b0443047c1fab4a612573f8d90f4ee1e150cda..a67d22907ec0000d4beeda473f132979949994a8 100644
--- a/groundStation/gui/MicroCART/mainwindow.h
+++ b/groundStation/gui/MicroCART/mainwindow.h
@@ -23,7 +23,9 @@ 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();
private slots:
void on_pbStart_clicked();
@@ -36,11 +38,12 @@ private slots:
void updateTracker(float x, float y, float z, float p, float r, float yaw);
- void updateConsole();
+ void updatePosAtt();
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);
@@ -73,16 +76,26 @@ private slots:
void on_socketPath_returnPressed();
+ void on_xSetpoint_returnPressed();
+
+ void on_ySetpoint_returnPressed();
+
+ void on_zSetpoint_returnPressed();
+
+ void on_posattSrcVrpn_clicked();
+
+ void on_posattSrcQuad_clicked();
+
+ void on_yawSetpoint_returnPressed();
+
private:
Ui::MainWindow *ui;
- pid_t backendPid;
- int backendPipe;
- int backendState;
QStandardItemModel * setpointList;
QTimer * nextSpTimer;
float sp_x;
float sp_y;
float sp_z;
+ QTimer * trackerTimer;
};
#endif // MAINWINDOW_H
diff --git a/groundStation/gui/MicroCART/mainwindow.ui b/groundStation/gui/MicroCART/mainwindow.ui
index a6795802eddc4c1dc1e89b03f408fb82bb30762b..097552bfbcfd9ae482f88f835a7cef234974178b 100644
--- a/groundStation/gui/MicroCART/mainwindow.ui
+++ b/groundStation/gui/MicroCART/mainwindow.ui
@@ -37,7 +37,7 @@
<item row="0" column="1">
<widget class="QLineEdit" name="socketPath">
<property name="enabled">
- <bool>true</bool>
+ <bool>false</bool>
</property>
<property name="text">
<string/>
@@ -154,8 +154,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>98</width>
- <height>76</height>
+ <width>968</width>
+ <height>577</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_8">
@@ -258,6 +258,13 @@
</item>
</layout>
</item>
+ <item>
+ <widget class="Line" name="line_5">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_4">
<item>
@@ -315,6 +322,126 @@
<item>
<widget class="QComboBox" name="zSetpointSelect"/>
</item>
+ <item>
+ <widget class="QLabel" name="label_18">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Yaw Setpoint:</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QComboBox" name="yawSetpointSelect"/>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout_10">
+ <item>
+ <widget class="QLabel" name="label_11">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>X Position</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QComboBox" name="xPositionSelect"/>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_13">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Y Position </string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QComboBox" name="yPositionSelect"/>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_14">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Z Position</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QComboBox" name="zPositionSelect"/>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout_11">
+ <item>
+ <widget class="QLabel" name="label_15">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>P Attitude</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QComboBox" name="pAttitudeSelect"/>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_16">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>R Attitude</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QComboBox" name="rAttitudeSelect"/>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_17">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Y Attitude</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QComboBox" name="yAttitudeSelect"/>
+ </item>
</layout>
</item>
</layout>
@@ -362,6 +489,52 @@
</property>
</widget>
</item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout_9">
+ <item>
+ <widget class="QLabel" name="label_12">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Position/Attitude Source: </string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QRadioButton" name="posattSrcVrpn">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Backend Tracker (&VRPN)</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QRadioButton" name="posattSrcQuad">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>From &Quad</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_3">
<item>
@@ -593,36 +766,6 @@
</property>
</widget>
</item>
- <item row="0" column="1">
- <widget class="QLineEdit" name="xSetpoint">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QLineEdit" name="ySetpoint">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QLineEdit" name="zSetpoint">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- </widget>
- </item>
<item row="2" column="0">
<widget class="QLabel" name="setpointLabel_3">
<property name="text">
@@ -637,6 +780,109 @@
</property>
</widget>
</item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="yawLabel">
+ <property name="text">
+ <string>Yaw</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <layout class="QHBoxLayout" name="horizontalLayout_12">
+ <item>
+ <widget class="QLineEdit" name="xSetpoint">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QCheckBox" name="sendX">
+ <property name="text">
+ <string/>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="1">
+ <layout class="QHBoxLayout" name="horizontalLayout_13">
+ <item>
+ <widget class="QLineEdit" name="ySetpoint">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QCheckBox" name="sendY">
+ <property name="text">
+ <string/>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="2" column="1">
+ <layout class="QHBoxLayout" name="horizontalLayout_14">
+ <item>
+ <widget class="QLineEdit" name="zSetpoint">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QCheckBox" name="sendZ">
+ <property name="text">
+ <string/>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="3" column="1">
+ <layout class="QHBoxLayout" name="horizontalLayout_15">
+ <item>
+ <widget class="QLineEdit" name="yawSetpoint">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QCheckBox" name="sendYaw">
+ <property name="text">
+ <string/>
+ </property>
+ <property name="checked">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
</layout>
</item>
<item>
@@ -882,7 +1128,7 @@
<x>0</x>
<y>0</y>
<width>1004</width>
- <height>27</height>
+ <height>30</height>
</rect>
</property>
</widget>
diff --git a/groundStation/gui/MicroCART/wrappers.c b/groundStation/gui/MicroCART/wrappers.c
deleted file mode 100644
index 45fdf0b28619f4d7d3cd6803239e6b0e2f999f62..0000000000000000000000000000000000000000
--- a/groundStation/gui/MicroCART/wrappers.c
+++ /dev/null
@@ -1,68 +0,0 @@
-#include "wrappers.h"
-#include <sys/types.h>
-#include <sys/wait.h>
-#include <sys/socket.h>
-#include <unistd.h>
-#include <signal.h>
-#include <err.h>
-#include <stdlib.h>
-
-
-size_t readBackend(int fd, char * buf, size_t maxlen)
-{
- int len = read(fd, buf, maxlen);
- if (len < 0) {
- len = 0;
- }
-
- return len;
-}
-
-int stopBackend(int pid, int pipefd)
-{
- close(pipefd);
- kill(pid, SIGTERM);
- int status;
- wait(&status);
- return status;
-}
-
-int startBackend(const char * backend, int * pipefd)
-{
- /* Create a pipe */
- int pipe_fds[2];
- if (pipe(pipe_fds)) {
- warn("Failed to open pipe, cannot start backend!");
- return -1;
- }
-
- int pid = fork();
- if (!pid) {
- /* Child closes read end of pipe */
- close(pipe_fds[0]);
-
- write(pipe_fds[1], "From child", 11);
- /* dup write end of pipe to stdout (FD 1), closing old stdout */
- //dup2(pipe_fds[1], 1);
- //dup2(pipe_fds[1], 2);
-
- write(2, "Child stderr\n", 13);
- printf("Child stdout\n");
-
- /* Don't need whatever FD is in pipe_fds[1] anymore, since it is stdout now */
- close(pipe_fds[1]);
-
- execl(backend, "BackEnd", NULL);
- exit(0);
- }
-
- /* Return the read end of the pipe in pipefd pointer */
- *pipefd = pipe_fds[0];
-
-
- /* Parent closes write end of the pipe */
- close(pipe_fds[1]);
-
-
- return pid;
-}
diff --git a/groundStation/gui/MicroCART/wrappers.h b/groundStation/gui/MicroCART/wrappers.h
deleted file mode 100644
index 6a29ecf90a9b59c71a3595242fa5fb31c68899e8..0000000000000000000000000000000000000000
--- a/groundStation/gui/MicroCART/wrappers.h
+++ /dev/null
@@ -1,19 +0,0 @@
-#ifndef WRAPPERS_H
-#define WRAPPERS_H
-
-#ifdef __cplusplus
-extern "C"
-{
-#endif
-
-#include <stdio.h>
-
-int startBackend(const char * backend, int * pipefd);
-int stopBackend(int pid, int pipefd);
-size_t readBackend(int fd, char * buf, size_t maxlen);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // WRAPPERS_H
diff --git a/groundStation/src/frontend/frontend_output.h b/groundStation/src/frontend/frontend_output.h
index b2a4e4333cc83344c0154c9bc20d764d9439799d..f0e2fbaea121bf7a5c726501c187640d9d1d2434 100644
--- a/groundStation/src/frontend/frontend_output.h
+++ b/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 */
diff --git a/quad/src/gen_diagram/network.dot b/quad/src/gen_diagram/network.dot
index 401acb652fdf093d6198c64d26ffa37061e16f49..8307517c4b6debd7884616eb7bf1140263a5d72d 100644
--- a/quad/src/gen_diagram/network.dot
+++ b/quad/src/gen_diagram/network.dot
@@ -12,9 +12,9 @@ label="<f0>Pitch PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4>
"Ts_IMU" -> "Pitch PID":f3 [label="Constant"]
"Yaw PID"[shape=record
label="<f0>Yaw PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=2.600] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
-"PSI Sum" -> "Yaw PID":f1 [label="Sum"]
+"Yaw" -> "Yaw PID":f1 [label="Constant"]
"Yaw Setpoint" -> "Yaw PID":f2 [label="Constant"]
-"Ts_IMU" -> "Yaw PID":f3 [label="Constant"]
+"Ts_VRPN" -> "Yaw PID":f3 [label="Constant"]
"Roll Rate PID"[shape=record
label="<f0>Roll Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.030] |<f5> [Ki=0.000] |<f6> [Kd=0.005] |<f7> [alpha=0.880]"]
"Gyro X" -> "Roll Rate PID":f1 [label="Constant"]
@@ -32,19 +32,19 @@ label="<f0>Yaw Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f
"Ts_IMU" -> "Yaw Rate PID":f3 [label="Constant"]
"X pos PID"[shape=record
label="<f0>X pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.550] |<f5> [Ki=0.007] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
-"OF X Trim Add" -> "X pos PID":f1 [label="Sum"]
+"VRPN X" -> "X pos PID":f1 [label="Constant"]
"X Setpoint" -> "X pos PID":f2 [label="Constant"]
-"Ts_IMU" -> "X pos PID":f3 [label="Constant"]
+"Ts_VRPN" -> "X pos PID":f3 [label="Constant"]
"Y pos PID"[shape=record
label="<f0>Y pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.550] |<f5> [Ki=0.007] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
-"OF Y Trim Add" -> "Y pos PID":f1 [label="Sum"]
+"VRPN Y" -> "Y pos PID":f1 [label="Constant"]
"Y Setpoint" -> "Y pos PID":f2 [label="Constant"]
-"Ts_IMU" -> "Y pos PID":f3 [label="Constant"]
+"Ts_VRPN" -> "Y pos PID":f3 [label="Constant"]
"Altitude PID"[shape=record
label="<f0>Altitude PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-0.098] |<f5> [Ki=-0.008] |<f6> [Kd=-0.074] |<f7> [alpha=0.880]"]
-"Lidar" -> "Altitude PID":f1 [label="Constant"]
+"VRPN Alt" -> "Altitude PID":f1 [label="Constant"]
"Alt Setpoint" -> "Altitude PID":f2 [label="Constant"]
-"Ts_IMU" -> "Altitude PID":f3 [label="Constant"]
+"Ts_VRPN" -> "Altitude PID":f3 [label="Constant"]
"X Setpoint"[shape=record
label="<f0>X Setpoint |<f1> [Constant=0.000]"]
"Y Setpoint"[shape=record
@@ -100,6 +100,9 @@ label="<f0>R PWM Clamp |<f1> --\>Bounds in |<f2> [Min=-20000.000] |<f3> [Max=20
"Y PWM Clamp"[shape=record
label="<f0>Y PWM Clamp |<f1> --\>Bounds in |<f2> [Min=-20000.000] |<f3> [Max=20000.000]"]
"Yaw Rate PID" -> "Y PWM Clamp":f1 [label="Correction"]
+"Yaw Rate Clamp"[shape=record
+label="<f0>Yaw Rate Clamp |<f1> --\>Bounds in |<f2> [Min=-1.000] |<f3> [Max=1.000]"]
+"Yaw PID" -> "Yaw Rate Clamp":f1 [label="Correction"]
"VRPN X"[shape=record
label="<f0>VRPN X |<f1> [Constant=0.000]"]
"VRPN Y"[shape=record
@@ -120,18 +123,24 @@ label="<f0>RC Yaw |<f1> [Constant=0.000]"]
label="<f0>RC Throttle |<f1> [Constant=0.000]"]
"X Vel PID"[shape=record
label="<f0>X Vel PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-0.100] |<f5> [Ki=0.000] |<f6> [Kd=-0.020] |<f7> [alpha=0.000]"]
-"Flow Vel X Filt" -> "X Vel PID":f1 [label="Constant"]
+"X Vel" -> "X Vel PID":f1 [label="Correction"]
"X Vel Clamp" -> "X Vel PID":f2 [label="Bounded"]
-"Ts_IMU" -> "X Vel PID":f3 [label="Constant"]
+"Ts_VRPN" -> "X Vel PID":f3 [label="Constant"]
"Y Vel PID"[shape=record
label="<f0>Y Vel PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.100] |<f5> [Ki=0.000] |<f6> [Kd=0.020] |<f7> [alpha=0.000]"]
-"Flow Vel Y Filt" -> "Y Vel PID":f1 [label="Constant"]
+"Y Vel" -> "Y Vel PID":f1 [label="Correction"]
"Y vel Clamp" -> "Y Vel PID":f2 [label="Bounded"]
-"Ts_IMU" -> "Y Vel PID":f3 [label="Constant"]
+"Ts_VRPN" -> "Y Vel PID":f3 [label="Constant"]
"X Vel"[shape=record
-label="<f0>X Vel |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.880]"]
+label="<f0>X Vel |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=-1.000] |<f7> [alpha=0.880]"]
+"VRPN X" -> "X Vel":f1 [label="Constant"]
+"zero" -> "X Vel":f2 [label="Constant"]
+"Ts_VRPN" -> "X Vel":f3 [label="Constant"]
"Y Vel"[shape=record
-label="<f0>Y Vel |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.880]"]
+label="<f0>Y Vel |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=-1.000] |<f7> [alpha=0.880]"]
+"VRPN Y" -> "Y Vel":f1 [label="Constant"]
+"zero" -> "Y Vel":f2 [label="Constant"]
+"Ts_VRPN" -> "Y Vel":f3 [label="Constant"]
"X Vel Clamp"[shape=record
label="<f0>X Vel Clamp |<f1> --\>Bounds in |<f2> [Min=-2.000] |<f3> [Max=2.000]"]
"X pos PID" -> "X Vel Clamp":f1 [label="Correction"]
@@ -140,12 +149,12 @@ label="<f0>Y vel Clamp |<f1> --\>Bounds in |<f2> [Min=-2.000] |<f3> [Max=2.000]
"Y pos PID" -> "Y vel Clamp":f1 [label="Correction"]
"Yaw Correction"[shape=record
label="<f0>Yaw Correction |<f1> --\>Current Yaw |<f2> --\>X Position |<f3> --\>Y Position"]
-"PSI Sum" -> "Yaw Correction":f1 [label="Sum"]
+"Yaw" -> "Yaw Correction":f1 [label="Constant"]
"X Vel PID" -> "Yaw Correction":f2 [label="Correction"]
"Y Vel PID" -> "Yaw Correction":f3 [label="Correction"]
"OF Offset Angle"[shape=record
label="<f0>OF Offset Angle |<f1> --\>Current Yaw |<f2> --\>X Position |<f3> --\>Y Position"]
-"PSI Sum" -> "OF Offset Angle":f1 [label="Sum"]
+"Yaw" -> "OF Offset Angle":f1 [label="Constant"]
"Flow Vel X" -> "OF Offset Angle":f2 [label="Constant"]
"Flow Vel Y" -> "OF Offset Angle":f3 [label="Constant"]
"OF Integrate X"[shape=record
diff --git a/quad/src/gen_diagram/network.png b/quad/src/gen_diagram/network.png
index b45f77b3e859babf1d2aa3e249d765ca6b43eb49..da87c6615e245cd3066f465689aa9d677e610e84 100644
Binary files a/quad/src/gen_diagram/network.png and b/quad/src/gen_diagram/network.png differ
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 15c500d31c9917de9a671c901e2592d42171d5d7..00a1fd39e993ed354292ec5bfc03b66860359107 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -13,10 +13,10 @@
#include "util.h"
#include "timer.h"
-#define USE_LIDAR
-#define USE_OF
-#define USE_GYRO_YAW
-#define NO_VRPN
+//#define USE_LIDAR
+//#define USE_OF
+//#define USE_GYRO_YAW
+//#define NO_VRPN
//10 degrees
#define ANGLE_CLAMP (0.1745)
@@ -33,7 +33,7 @@ void connect_autonomous(parameter_t* ps) {
//graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->y_pos_pid, PID_CORRECTION);
graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->yaw_correction, ROT_OUT_X);
graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->yaw_correction, ROT_OUT_Y);
- graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->yaw_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->yaw_clamp, BOUNDS_OUT);
#ifdef USE_OF
//graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, ADD_SUM);
graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
@@ -94,6 +94,9 @@ int control_algorithm_init(parameter_t * ps)
ps->clamp_d_pwmR = graph_add_defined_block(graph, BLOCK_BOUNDS, "R PWM Clamp");
ps->clamp_d_pwmY = graph_add_defined_block(graph, BLOCK_BOUNDS, "Y PWM Clamp");
+ // Yaw clamp
+ ps->yaw_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "Yaw Rate Clamp");
+
// Create blocks for VRPN data
ps->vrpn_x = graph_add_defined_block(graph, BLOCK_CONSTANT, "VRPN X");
ps->vrpn_y = graph_add_defined_block(graph, BLOCK_CONSTANT, "VRPN Y");
@@ -167,7 +170,7 @@ int control_algorithm_init(parameter_t * ps)
graph_set_source(graph, ps->roll_pid, PID_DT, ps->angle_time, CONST_VAL);
// Connect yaw PID chain
- graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->rc_yaw, PID_CORRECTION);
+ graph_set_source(graph, ps->yaw_clamp, BOUNDS_IN, ps->yaw_pid, PID_CORRECTION);
graph_set_source(graph, ps->yaw_r_pid, PID_CUR_POINT, ps->gyro_z, CONST_VAL);
graph_set_source(graph, ps->yaw_r_pid, PID_DT, ps->angle_time, CONST_VAL);
@@ -361,6 +364,10 @@ int control_algorithm_init(parameter_t * ps)
graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MIN, -VEL_CLAMP);
graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MAX, VEL_CLAMP);
+ // Set yaw clamping limits
+ graph_set_param_val(graph, ps->yaw_clamp, BOUNDS_MIN, -1.5);
+ graph_set_param_val(graph, ps->yaw_clamp, BOUNDS_MAX, 1.5);
+
// Set trims
graph_set_param_val(graph, ps->pitch_trim, CONST_SET, PITCH_TRIM);
diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c
index 4996c9117c605b9ee504d1e01d18a64b315a1e7d..ec33504f517e5e27fc0db34a36ab939fab9bd5a5 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/quad/src/quad_app/sensor_processing.c
@@ -206,12 +206,6 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
sensor_struct->roll_angle_filtered,
sensor_struct->pitch_angle_filtered,
sensor_struct->currentQuadPosition.yaw);
- // flow_to_vel(&sensor_struct->optical_flow, raw_sensor_struct->lidar_distance_m);
-// sensor_struct->optical_flow.xVel = biquad_execute(&sensor_struct->flow_x_filt, -sensor_struct->optical_flow.xVel);
-// sensor_struct->optical_flow.yVel = biquad_execute(&sensor_struct->flow_y_filt, -sensor_struct->optical_flow.yVel);
- //Note: This was wrong and dumb
- //sensor_struct->optical_flow.xVel *= -1;
- //sensor_struct->optical_flow.yVel *= -1;
//Filter OF velocities
sensor_struct->optical_flow.xVelFilt = biquad_execute(&sensor_struct->flow_x_filt, sensor_struct->optical_flow.xVel);
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index cef24608aac3b2ca8095f570ba33f2e7a8df6c5b..e1ffb3d4a0768bcaa05849efb2fb2ca5fb580cc0 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -395,6 +395,7 @@ typedef struct parameter_t {
int clamp_d_pwmP;
int clamp_d_pwmR;
int clamp_d_pwmY;
+ int yaw_clamp;
// Loop times
int angle_time;
int pos_time;
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c
index d9a947a4ad8cc10fcb040e5b8a3bb44e4dc6595c..d5e94484da69e69e0ac9cd899af3f624ad97ef55 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_optical_flow.c
@@ -17,7 +17,7 @@ int zybo_optical_flow_read(struct OpticalFlowDriver *self, px4flow_t *of) {
struct I2CDriver *i2c = self->i2c;
int error = 0;
- typedef struct i2c_integral_frame
+ struct i2c_integral_frame
{
uint16_t frame_count_since_last_readout;//number of flow measurements since last I2C readout [#frames]
int16_t pixel_flow_x_integral;//accumulated flow in radians*10000 around x axis since last I2C readout [rad*10000]
diff --git a/quad/xsdk_workspace/real_quad/src/main.c b/quad/xsdk_workspace/real_quad/src/main.c
index 198b48d057c468db94336fad51a42d6a5d7682ab..d11f783b9d9777390a7be79d1793acb7690ab525 100644
--- a/quad/xsdk_workspace/real_quad/src/main.c
+++ b/quad/xsdk_workspace/real_quad/src/main.c
@@ -48,7 +48,7 @@ int main()
#ifdef RUN_TESTS
//test_zybo_mio7_led_and_system();
//test_zybo_i2c();
- test_zybo_i2c_imu();
+ //test_zybo_i2c_imu();
//test_zybo_i2c_px4flow();
//test_zybo_i2c_lidar();
//test_zybo_i2c_all();
@@ -56,7 +56,7 @@ int main()
//test_zybo_motors();
//test_zybo_uart();
//test_zybo_axi_timer();
- test_zybo_uart_comm();
+ //test_zybo_uart_comm();
return 0;
#endif