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 132e3de85641396dd4810289a15803411824e3db..8307517c4b6debd7884616eb7bf1140263a5d72d 100644
--- a/quad/src/gen_diagram/network.dot
+++ b/quad/src/gen_diagram/network.dot
@@ -3,18 +3,18 @@ rankdir="LR"
"Roll PID"[shape=record
label="<f0>Roll PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=35.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.880]"]
"Roll" -> "Roll PID":f1 [label="Constant"]
-"Yaw Correction" -> "Roll PID":f2 [label="Rotated Y"]
+"RC Roll" -> "Roll PID":f2 [label="Constant"]
"Ts_IMU" -> "Roll PID":f3 [label="Constant"]
"Pitch PID"[shape=record
label="<f0>Pitch PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=35.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.880]"]
"Pitch trim add" -> "Pitch PID":f1 [label="Sum"]
-"Yaw Correction" -> "Pitch PID":f2 [label="Rotated X"]
+"RC Pitch" -> "Pitch PID":f2 [label="Constant"]
"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]"]
-"Integrated gyro z" -> "Yaw PID":f1 [label="Integrated"]
+"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"]
@@ -28,23 +28,23 @@ label="<f0>Pitch Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |
"Yaw Rate PID"[shape=record
label="<f0>Yaw Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.297] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
"Gyro Z" -> "Yaw Rate PID":f1 [label="Constant"]
-"Yaw PID" -> "Yaw Rate PID":f2 [label="Correction"]
+"RC Yaw" -> "Yaw Rate PID":f2 [label="Constant"]
"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
@@ -71,6 +71,10 @@ label="<f0>Lidar |<f1> [Constant=0.000]"]
label="<f0>Flow Vel X |<f1> [Constant=0.000]"]
"Flow Vel Y"[shape=record
label="<f0>Flow Vel Y |<f1> [Constant=0.000]"]
+"Flow Vel X Filt"[shape=record
+label="<f0>Flow Vel X Filt |<f1> [Constant=0.000]"]
+"Flow Vel Y Filt"[shape=record
+label="<f0>Flow Vel Y Filt |<f1> [Constant=0.000]"]
"Flow Quality"[shape=record
label="<f0>Flow Quality |<f1> [Constant=0.000]"]
"Flow Distance"[shape=record
@@ -96,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
@@ -116,26 +123,38 @@ 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]"]
+"X Vel" -> "X Vel PID":f1 [label="Correction"]
+"X Vel Clamp" -> "X Vel PID":f2 [label="Bounded"]
+"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]"]
+"Y Vel" -> "Y Vel PID":f1 [label="Correction"]
+"Y vel Clamp" -> "Y Vel PID":f2 [label="Bounded"]
+"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=-0.174] |<f3> [Max=0.174]"]
+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"]
"Y vel Clamp"[shape=record
-label="<f0>Y vel Clamp |<f1> --\>Bounds in |<f2> [Min=-0.174] |<f3> [Max=0.174]"]
+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"]
-"Integrated gyro z" -> "Yaw Correction":f1 [label="Integrated"]
-"X Vel Clamp" -> "Yaw Correction":f2 [label="Bounded"]
-"Y vel Clamp" -> "Yaw Correction":f3 [label="Bounded"]
+"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"]
-"Integrated gyro z" -> "OF Offset Angle":f1 [label="Integrated"]
+"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
@@ -158,12 +177,29 @@ label="<f0>OF X Trim Add |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
label="<f0>OF Y Trim Add |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
"OF Integrate Y" -> "OF Y Trim Add":f1 [label="Integrated"]
"OF Trim Y" -> "OF Y Trim Add":f2 [label="Constant"]
-"Integrated gyro z"[shape=record
-label="<f0>Integrated gyro z |<f1> --\>Integrator In |<f2> --\>Integrator dt"]
-"Gyro Z" -> "Integrated gyro z":f1 [label="Constant"]
+"PSI Dot"[shape=record
+label="<f0>PSI Dot |<f1> [Constant=0.000]"]
+"PSI Dot Offset"[shape=record
+label="<f0>PSI Dot Offset |<f1> [Constant=0.000]"]
+"PSI Dot Sum"[shape=record
+label="<f0>PSI Dot Sum |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
+"PSI Dot" -> "PSI Dot Sum":f1 [label="Constant"]
+"PSI Dot Offset" -> "PSI Dot Sum":f2 [label="Constant"]
+"PSI Angle"[shape=record
+label="<f0>PSI Angle |<f1> --\>Integrator In |<f2> --\>Integrator dt"]
+"PSI Dot Sum" -> "PSI Angle":f1 [label="Sum"]
+"Ts_IMU" -> "PSI Angle":f2 [label="Constant"]
+"PSI Angle Offset"[shape=record
+label="<f0>PSI Angle Offset |<f1> [Constant=0.000]"]
+"PSI Sum"[shape=record
+label="<f0>PSI Sum |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
+"PSI Angle" -> "PSI Sum":f1 [label="Integrated"]
+"PSI Angle Offset" -> "PSI Sum":f2 [label="Constant"]
+"Mag Yaw"[shape=record
+label="<f0>Mag Yaw |<f1> [Constant=0.000]"]
"Signal Mixer"[shape=record
label="<f0>Signal Mixer |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4> --\>Yaw"]
-"T trim add" -> "Signal Mixer":f1 [label="Sum"]
+"RC Throttle" -> "Signal Mixer":f1 [label="Constant"]
"P PWM Clamp" -> "Signal Mixer":f2 [label="Bounded"]
"R PWM Clamp" -> "Signal Mixer":f3 [label="Bounded"]
"Y PWM Clamp" -> "Signal Mixer":f4 [label="Bounded"]
diff --git a/quad/src/gen_diagram/network.png b/quad/src/gen_diagram/network.png
index d0c2ae5e41d0e3108c5c5480eb0748e18ffd2de0..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 1d6fad4bf0a9d961a1f1b6f2215495a3d06dfa37..54e1e66d42248e4fa6f8f040f203a0d237cd02e5 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_MAG_YAW
-#define MAX_VALID_LIDAR (10.0) // Maximum valid distance to read from LiDAR to update
+//#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);
@@ -79,6 +79,8 @@ int control_algorithm_init(parameter_t * ps)
ps->lidar = graph_add_defined_block(graph, BLOCK_CONSTANT, "Lidar");
ps->flow_vel_x = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Vel X");
ps->flow_vel_y = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Vel Y");
+ ps->flow_vel_x_filt = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Vel X Filt");
+ ps->flow_vel_y_filt = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Vel Y Filt");
ps->flow_quality = graph_add_defined_block(graph, BLOCK_CONSTANT, "Flow Quality");
// Sensor trims
ps->pitch_trim = graph_add_defined_block(graph, BLOCK_CONSTANT, "Pitch trim");
@@ -92,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");
@@ -124,8 +129,13 @@ int control_algorithm_init(parameter_t * ps)
ps->of_trimmed_x = graph_add_defined_block(graph, BLOCK_ADD, "OF X Trim Add");
ps->of_trimmed_y = graph_add_defined_block(graph, BLOCK_ADD, "OF Y Trim Add");
- // gyroscope z integrator
- ps->gyro_yaw = graph_add_defined_block(graph, BLOCK_INTEGRATOR, "Integrated gyro z");
+ // psi dot integrator
+ ps->psi_dot = graph_add_defined_block(graph, BLOCK_CONSTANT, "PSI Dot");
+ ps->psi_dot_offset = graph_add_defined_block(graph, BLOCK_CONSTANT, "PSI Dot Offset");
+ ps->psi_dot_sum = graph_add_defined_block(graph, BLOCK_ADD, "PSI Dot Sum");
+ ps->psi = graph_add_defined_block(graph, BLOCK_INTEGRATOR, "PSI Angle");
+ ps->psi_offset = graph_add_defined_block(graph, BLOCK_CONSTANT, "PSI Angle Offset");
+ ps->psi_sum = graph_add_defined_block(graph, BLOCK_ADD, "PSI Sum");
//Complementary yaw
ps->mag_yaw = graph_add_defined_block(graph, BLOCK_CONSTANT, "Mag Yaw");
@@ -156,15 +166,21 @@ int control_algorithm_init(parameter_t * ps)
graph_set_source(graph, ps->roll_r_pid, PID_CUR_POINT, ps->gyro_x, CONST_VAL);
graph_set_source(graph, ps->roll_r_pid, PID_DT, ps->angle_time, CONST_VAL);
graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->cur_roll, CONST_VAL);
- //graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->vrpn_roll, CONST_VAL);USE_FAKE_YAW
+ //graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->vrpn_roll, CONST_VAL);
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);
- graph_set_source(graph, ps->gyro_yaw, INTEGRATE_IN, ps->gyro_z, CONST_VAL);
+ //PSI-dot integration chain
+ graph_set_source(graph, ps->psi_dot_sum, ADD_SUMMAND1, ps->psi_dot, CONST_VAL);
+ graph_set_source(graph, ps->psi_dot_sum, ADD_SUMMAND2, ps->psi_dot_offset, CONST_VAL);
+ graph_set_source(graph, ps->psi, INTEGRATE_IN, ps->psi_dot_sum, CONST_VAL);
+ graph_set_source(graph, ps->psi, INTEGRATE_DT, ps->angle_time, CONST_VAL);
+ graph_set_source(graph, ps->psi_sum, ADD_SUMMAND1, ps->psi, INTEGRATED);
+ graph_set_source(graph, ps->psi_sum, ADD_SUMMAND2, ps->psi_offset, CONST_VAL);
#ifndef USE_OF
// X velocity PID
@@ -176,19 +192,6 @@ int control_algorithm_init(parameter_t * ps)
// Connect velocity PID itself
graph_set_source(graph, ps->x_vel_pid, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->x_vel_pid, PID_CUR_POINT, ps->x_vel, PID_CORRECTION);
- //graph_set_source(graph, ps->x_vel_pid, PID_CUR_POINT, ps->of_angle_corr, ROT_OUT_X);
- graph_set_source(graph, ps->x_vel_pid, PID_SETPOINT, ps->x_vel_clamp, BOUNDS_OUT);
- graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
-
- // X/Y global to local conversion
-#ifdef USE_MAG_YAW
- graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->mag_yaw, CONST_VAL);
-#else
- graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->cur_yaw, CONST_VAL);
-#endif
- graph_set_source(graph, ps->yaw_correction, ROT_CUR_X, ps->x_vel_pid, PID_CORRECTION);
- graph_set_source(graph, ps->yaw_correction, ROT_CUR_Y, ps->y_vel_pid, PID_CORRECTION);
-
// Y velocity PID
// Use a PID block to compute the derivative
@@ -199,47 +202,50 @@ int control_algorithm_init(parameter_t * ps)
// Connect velocity PID itself
graph_set_source(graph, ps->y_vel_pid, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->y_vel_pid, PID_CUR_POINT, ps->y_vel, PID_CORRECTION);
- //graph_set_source(graph, ps->y_vel_pid, PID_CUR_POINT, ps->of_angle_corr, ROT_OUT_Y);
- graph_set_source(graph, ps->y_vel_pid, PID_SETPOINT, ps->y_vel_clamp, BOUNDS_OUT);
- graph_set_source(graph, ps->y_vel_clamp, BOUNDS_IN, ps->y_pos_pid, PID_CORRECTION);
// X position
graph_set_source(graph, ps->x_pos_pid, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->x_pos_pid, PID_CUR_POINT, ps->vrpn_x, CONST_VAL);
- //graph_set_source(graph, ps->x_pos_pid, PID_CUR_POINT, ps->of_trimmed_x, CONST_VAL);
- graph_set_source(graph, ps->x_pos_pid, PID_SETPOINT, ps->x_set, CONST_VAL);
+
// Y position
graph_set_source(graph, ps->y_pos_pid, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->vrpn_y, CONST_VAL);
- //graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->of_trimmed_y, CONST_VAL);
- graph_set_source(graph, ps->y_pos_pid, PID_SETPOINT, ps->y_set, CONST_VAL);
-#endif
-
-#ifdef USE_OF
+#else
// X position
graph_set_source(graph, ps->x_pos_pid, PID_DT, ps->angle_time, CONST_VAL);
graph_set_source(graph, ps->x_pos_pid, PID_CUR_POINT, ps->of_trimmed_x, ADD_SUM);
- //graph_set_source(graph, ps->x_pos_pid, PID_CUR_POINT, ps->of_trimmed_x, CONST_VAL);
- graph_set_source(graph, ps->x_pos_pid, PID_SETPOINT, ps->x_set, CONST_VAL);
+
// Y position
graph_set_source(graph, ps->y_pos_pid, PID_DT, ps->angle_time, CONST_VAL);
graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->of_trimmed_y, ADD_SUM);
- //graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->of_trimmed_y, CONST_VAL);
- graph_set_source(graph, ps->y_pos_pid, PID_SETPOINT, ps->y_set, CONST_VAL);
- graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
- graph_set_source(graph, ps->y_vel_clamp, BOUNDS_IN, ps->y_pos_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->x_vel_pid, PID_DT, ps->angle_time, CONST_VAL);
+ graph_set_source(graph, ps->x_vel_pid, PID_CUR_POINT, ps->flow_vel_x_filt, PID_CORRECTION);
+ graph_set_source(graph, ps->y_vel_pid, PID_DT, ps->angle_time, CONST_VAL);
+ graph_set_source(graph, ps->y_vel_pid, PID_CUR_POINT, ps->flow_vel_y_filt, PID_CORRECTION);
+
+#endif
+
+ graph_set_source(graph, ps->x_vel_pid, PID_SETPOINT, ps->x_vel_clamp, BOUNDS_OUT);
+ graph_set_source(graph, ps->x_vel_clamp, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
// X/Y global to local conversion
-#ifdef USE_MAG_YAW
- graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->mag_yaw, CONST_VAL);
+#ifdef USE_GYRO_YAW
+ graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->psi_sum, ADD_SUM);
#else
graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->cur_yaw, CONST_VAL);
#endif
- graph_set_source(graph, ps->yaw_correction, ROT_CUR_X, ps->x_vel_clamp, PID_CORRECTION);
- graph_set_source(graph, ps->yaw_correction, ROT_CUR_Y, ps->y_vel_clamp, PID_CORRECTION);
-#endif
+ graph_set_source(graph, ps->yaw_correction, ROT_CUR_X, ps->x_vel_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->yaw_correction, ROT_CUR_Y, ps->y_vel_pid, PID_CORRECTION);
+
+ graph_set_source(graph, ps->y_vel_pid, PID_SETPOINT, ps->y_vel_clamp, BOUNDS_OUT);
+ graph_set_source(graph, ps->y_vel_clamp, BOUNDS_IN, ps->y_pos_pid, PID_CORRECTION);
+
+ graph_set_source(graph, ps->x_pos_pid, PID_SETPOINT, ps->x_set, CONST_VAL);
+
+ //graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->of_trimmed_y, CONST_VAL);
+ graph_set_source(graph, ps->y_pos_pid, PID_SETPOINT, ps->y_set, CONST_VAL);
// Alt autonomous
#ifdef USE_LIDAR
@@ -253,9 +259,9 @@ int control_algorithm_init(parameter_t * ps)
graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND1, ps->alt_pid, PID_CORRECTION);
graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND2, ps->throttle_trim, CONST_VAL);
// Yaw angle
-#ifdef USE_MAG_YAW
+#ifdef USE_GYRO_YAW
graph_set_source(graph, ps->yaw_pid, PID_DT, ps->angle_time, CONST_VAL);
- graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->mag_yaw, CONST_VAL);
+ graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->psi_sum, ADD_SUM);
#else
graph_set_source(graph, ps->yaw_pid, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->cur_yaw, CONST_VAL);
@@ -274,8 +280,8 @@ int control_algorithm_init(parameter_t * ps)
graph_set_source(graph, ps->mixer, MIXER_YAW, ps->clamp_d_pwmY, BOUNDS_OUT);
// Connect optical flow
-#ifdef USE_MAG_YAW
- graph_set_source(graph, ps->of_angle_corr, ROT_YAW, ps->mag_yaw, ADD_SUM);
+#ifdef USE_GYRO_YAW
+ graph_set_source(graph, ps->of_angle_corr, ROT_YAW, ps->psi_sum, ADD_SUM);
#else
graph_set_source(graph, ps->of_angle_corr, ROT_YAW, ps->cur_yaw, ADD_SUM);
#endif
@@ -352,19 +358,15 @@ int control_algorithm_init(parameter_t * ps)
graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MIN, -PWM_DIFF_BOUNDS);
graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MAX, PWM_DIFF_BOUNDS);
-#ifdef USE_OF
- //Set angle clamping limits
- graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MIN, -ANGLE_CLAMP);
- graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MAX, ANGLE_CLAMP);
- graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MIN, -ANGLE_CLAMP);
- graph_set_param_val(graph, ps->y_vel_clamp, BOUNDS_MAX, ANGLE_CLAMP);
-#else
// Set velocity clamping limits
graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MIN, -VEL_CLAMP);
graph_set_param_val(graph, ps->x_vel_clamp, BOUNDS_MAX, VEL_CLAMP);
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);
-#endif
+
+ // 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);
@@ -398,7 +400,11 @@ int control_algorithm_init(parameter_t * ps)
// flap switch was just toggled to auto flight mode
if((last_fm_switch != cur_fm_switch) && (cur_fm_switch == AUTO_FLIGHT_MODE))
{
+#ifdef NO_VRPN
+ user_defined_struct->engaging_auto = 2;
+#else
user_defined_struct->engaging_auto = 1;
+#endif
graph_set_param_val(graph, ps->throttle_trim, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
}
@@ -470,17 +476,16 @@ int control_algorithm_init(parameter_t * ps)
graph_set_param_val(graph, ps->gyro_y, CONST_SET, sensor_struct->gyr_y);
graph_set_param_val(graph, ps->gyro_x, CONST_SET, sensor_struct->gyr_x);
graph_set_param_val(graph, ps->gyro_z, CONST_SET, sensor_struct->gyr_z);
- //if (fabs(sensor_struct->lidar_altitude) <= MAX_VALID_LIDAR) {
- graph_set_param_val(graph, ps->lidar, CONST_SET, sensor_struct->lidar_altitude);
- //}
+ graph_set_param_val(graph, ps->psi_dot, CONST_SET, sensor_struct->psi_dot);
+ graph_set_param_val(graph, ps->lidar, CONST_SET, sensor_struct->lidar_altitude);
graph_set_param_val(graph, ps->flow_quality, CONST_SET, sensor_struct->optical_flow.quality);
- //As per documentation, disregard frames with low quality, as they contain unreliable data
- //NOTE: As per the Wehrneyton(R) Method(TM), we will be exponentially decaying the
- //optical flow velocities when the quality is below the threshold
- //if (sensor_struct->optical_flow.quality >= PX4FLOW_QUAL_MIN) {
- graph_set_param_val(graph, ps->flow_vel_x, CONST_SET, sensor_struct->optical_flow.xVel);
- graph_set_param_val(graph, ps->flow_vel_y, CONST_SET, sensor_struct->optical_flow.yVel);
- //}
+
+ //Optical flow
+ graph_set_param_val(graph, ps->flow_vel_x, CONST_SET, sensor_struct->optical_flow.xVel);
+ graph_set_param_val(graph, ps->flow_vel_y, CONST_SET, sensor_struct->optical_flow.yVel);
+ graph_set_param_val(graph, ps->flow_distance, CONST_SET, sensor_struct->optical_flow.distance);
+ graph_set_param_val(graph, ps->flow_vel_x_filt, CONST_SET, sensor_struct->optical_flow.xVelFilt);
+ graph_set_param_val(graph, ps->flow_vel_y_filt, CONST_SET, sensor_struct->optical_flow.yVelFilt);
// RC values
graph_set_param_val(graph, ps->rc_pitch, CONST_SET, user_input_struct->pitch_angle_manual_setpoint);
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index 94566042e3ddc79352fe1d2ba89b49780ceacfa0..7079987a54d2288970d34af671f41d8b06e2cafd 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -118,7 +118,7 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
addOutputToLog(log_struct, ps->of_angle_corr, ROT_OUT_Y, m_s);
addOutputToLog(log_struct, ps->of_integ_x, INTEGRATED, m);
addOutputToLog(log_struct, ps->of_integ_y, INTEGRATED, m);
- addOutputToLog(log_struct, ps->gyro_yaw, INTEGRATED, rad);
+ addOutputToLog(log_struct, ps->psi_sum, ADD_SUM, rad);
addOutputToLog(log_struct, ps->mag_yaw, CONST_VAL, rad);
addParamToLog(log_struct, ps->cur_roll, CONST_VAL, rad);
@@ -146,7 +146,6 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
}
}
-
int log_data(log_t* log_struct, parameter_t* ps)
{
if(arrayIndex >= arraySize)
diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c
index 219a6bea3ef5af6526bfe5b14f4d8db0c5e8d2d8..ec33504f517e5e27fc0db34a36ab939fab9bd5a5 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/quad/src/quad_app/sensor_processing.c
@@ -23,6 +23,8 @@
#define GYRO_Z_OFFSET (0.0073)
+#define MAX_VALID_LIDAR (10.0) // Maximum valid distance to read from LiDAR to update
+
int sensor_processing_init(sensor_t* sensor_struct) {
float a0 = 0.0200833310260;
float a1 = 0.0401666620520;
@@ -32,11 +34,13 @@ int sensor_processing_init(sensor_t* sensor_struct) {
sensor_struct->accel_x_filt = filter_make_state(a0, a1, a2, b1, b2);
sensor_struct->accel_y_filt = filter_make_state(a0, a1, a2, b1, b2);
sensor_struct->accel_z_filt = filter_make_state(a0, a1, a2, b1, b2);
- float vel_a0 = 0.0098;
- float vel_a1 = 0.0195;
- float vel_a2 = 0.0098;
- float vel_b1 = -1.5816;
- float vel_b2 = 0.6853;
+
+ //1 Hert filter
+ float vel_a0 = 2.3921e-4;
+ float vel_a1 = 4.7841e-4;
+ float vel_a2 = 2.3921e-4;
+ float vel_b1 = -1.9381;
+ float vel_b2 = 0.9391;
sensor_struct->flow_x_filt = filter_make_state(vel_a0, vel_a1, vel_a2, vel_b1, vel_b2);
sensor_struct->flow_y_filt = filter_make_state(vel_a0, vel_a1, vel_a2, vel_b1, vel_b2);
@@ -202,12 +206,10 @@ 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);
+ sensor_struct->optical_flow.yVelFilt = biquad_execute(&sensor_struct->flow_y_filt, sensor_struct->optical_flow.yVel);
/*
* Altitude double complementary filter
@@ -218,6 +220,10 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
static float last_lidar = 0;
float this_lidar = -raw_sensor_struct->lidar_distance_m;
+ if(this_lidar < (-MAX_VALID_LIDAR)) {
+ this_lidar = filtered_alt;
+ }
+
// Acceleration in m/s without gravity
float quad_z_accel = 9.8 * (accel_z + 1);
filtered_vel = alt_alpha*(filtered_vel + quad_z_accel*get_last_loop_time()) +
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index 280da1a58e12c09f32b72e23f9b217588b3479f5..e1ffb3d4a0768bcaa05849efb2fb2ca5fb580cc0 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -132,6 +132,8 @@ typedef struct px4flow {
// Time since last readout in seconds
double dt;
+ double xVelFilt, yVelFilt;
+
int16_t quality;
SensorError_t error;
@@ -370,6 +372,8 @@ typedef struct parameter_t {
int lidar;
int flow_vel_x; // optical flow
int flow_vel_y;
+ int flow_vel_x_filt;
+ int flow_vel_y_filt;
int flow_quality; // Quality value returned by optical flow sensor
int flow_distance;
// VRPN blocks
@@ -391,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;
@@ -419,7 +424,13 @@ typedef struct parameter_t {
int of_trim_y;
int of_trimmed_x; // Trimmed optical flow integrated value (of_integ_x + of_trim_x)
int of_trimmed_y;
- int gyro_yaw; // Integrates the gyro z value
+ //psi dot integration chain
+ int psi_dot;
+ int psi_dot_offset;
+ int psi_dot_sum;
+ int psi;
+ int psi_offset;
+ int psi_sum;
int mag_yaw; //Complementary filtered magnetometer/gyro yaw
} parameter_t;
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
index c7f2bfff3b7751ebf40346d52431eca8e15352f3..31b68f49d66c93896cb02f9f17afa4fe37f4cb34 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_imu.c
@@ -46,7 +46,7 @@
#define GYRO_X_BIAS 0.005f
#define GYRO_Y_BIAS -0.014f
-#define GYRO_Z_BIAS 0.0614//0.0541f
+#define GYRO_Z_BIAS 0.0534//0.0541f
#define ACCEL_X_BIAS 0.023f
#define ACCEL_Y_BIAS 0.009f