diff --git a/groundStation/gui/MicroCART/MicroCART.pro.user b/groundStation/gui/MicroCART/MicroCART.pro.user
index 1dbd232a082163317c643a0b53fd6b2f093385cd..b845d25d848e878796105144c4340687885425fb 100644
--- a/groundStation/gui/MicroCART/MicroCART.pro.user
+++ b/groundStation/gui/MicroCART/MicroCART.pro.user
@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 4.0.1, 2017-04-11T14:10:46. -->
+<!-- Written by QtCreator 4.0.1, 2017-04-11T17:47:13. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
diff --git a/groundStation/gui/MicroCART/controlworker.cpp b/groundStation/gui/MicroCART/controlworker.cpp
index 3201d0ef341e73bde79b7e7ccc490e1ef7a5b4ce..6cd8b01c08877ad2b17ef8c27307006f361cb265 100644
--- a/groundStation/gui/MicroCART/controlworker.cpp
+++ b/groundStation/gui/MicroCART/controlworker.cpp
@@ -1,7 +1,10 @@
#include "controlworker.h"
#include "frontend_nodes.h"
#include "frontend_param.h"
+#include "frontend_source.h"
#include "graph_blocks.h"
+#include <QProcess>
+#include <err.h>
ControlWorker::ControlWorker(QObject *parent) :
QObject(parent), conn(NULL)
@@ -41,9 +44,63 @@ void ControlWorker::getNodes()
const_block_nodes.append(nd[i].name);
}
}
- frontend_free_node_data(nd, num_nodes);
emit(gotNodes(nodes));
emit(gotConstantBlocks(const_block_nodes));
+
+ /* Create a graph for rendering.
+ *
+ * This leaks memory (NOT MY FAULT!) because there isn't a function to free
+ * a graph created with create_graph().
+ */
+ computation_graph * cgraph = create_graph();
+
+ /* Add nodes */
+ for (size_t i = 0; i < num_nodes; i++) {
+ graph_add_node_id(cgraph, nd[i].block, nd[i].name, blockDefs[nd[i].type]);
+ }
+
+ /* Set sources */
+ for (size_t i = 0; i < num_nodes; i++) {
+ for (ssize_t j = 0; j < blockDefs[nd[i].type]->n_inputs; j++) {
+ frontend_source_data sd;
+ sd.dst_block = nd[i].block;
+ sd.dst_input = j;
+ sd.src_block = 0;
+ sd.src_output = 0;
+ frontend_getsource(conn, &sd);
+ graph_set_source(cgraph, sd.dst_block, sd.dst_input, sd.src_block, sd.src_output);
+ }
+ }
+
+ /* Set params */
+ for (size_t i = 0; i < num_nodes; i++) {
+ for (ssize_t j = 0; j < blockDefs[nd[i].type]->n_params; j++) {
+ frontend_param_data pd;
+ pd.block = nd[i].block;
+ pd.param = j;
+ pd.value = 0;
+ frontend_getparam(conn, &pd);
+ graph_set_param_val(cgraph, pd.block, pd.param, pd.value);
+ }
+ }
+
+ /* This is a massive hack... */
+ /* Could be done with popen, but fuck it */
+ FILE * dotfile = fopen("/tmp/ucart-tmp-graph.dot", "w");
+ if (dotfile) {
+ export_dot(cgraph, dotfile, 0);
+ fclose(dotfile);
+ if (int dot_exit = QProcess::execute("dot /tmp/ucart-tmp-graph.dot -Tpng -o /tmp/ucart-tmp-graph.png")) {
+ warnx("dot returned nonzero value (%d)", dot_exit);
+ } else {
+ emit(graphRendered(QString("/tmp/ucart-tmp-graph.png")));
+ }
+ } else {
+ warn("fopen (/tmp/ucart-tmp-graph.dot)");
+ }
+
+ /* And here's where I'd put a call to free_graph(), IF I HAD ONE! */
+ frontend_free_node_data(nd, num_nodes);
}
}
diff --git a/groundStation/gui/MicroCART/controlworker.h b/groundStation/gui/MicroCART/controlworker.h
index 58939cd8d2067a625e0fd4a934f05cacb866b4d8..9a2c6de5101100953c3cd65eb8fa5abc5998ba08 100644
--- a/groundStation/gui/MicroCART/controlworker.h
+++ b/groundStation/gui/MicroCART/controlworker.h
@@ -18,6 +18,7 @@ signals:
void gotParamValue(QString node, QString param, float value);
void gotConstantBlocks(QStringList blocks);
void paramSet(QString node, QString param);
+ void graphRendered(QString graph);
public slots:
void connectBackend();
diff --git a/groundStation/gui/MicroCART/mainwindow.cpp b/groundStation/gui/MicroCART/mainwindow.cpp
index 8eed5261fc0bee1fec1a916e4bbfac41eef739a1..68b410c57da1b4b6e8aaf2d73a14dfde7f21d273 100644
--- a/groundStation/gui/MicroCART/mainwindow.cpp
+++ b/groundStation/gui/MicroCART/mainwindow.cpp
@@ -6,6 +6,7 @@
#include <QTimer>
#include <QRegExp>
#include <QProcessEnvironment>
+#include <QPixmap>
#include "wrappers.h"
#include "trackerworker.h"
@@ -25,9 +26,6 @@ MainWindow::MainWindow(QWidget *parent) :
/* Set up environment variables */
findChild<QLineEdit *>("socketPath")->setText(QProcessEnvironment::systemEnvironment().value("UCART_SOCKET"));
- /* Idiot lights */
- findChild<QLabel *>("noGraphWarning1")->setStyleSheet("QLabel {color : red; }");
-
/* Create a thread for workers */
QThread* workerThread = new QThread(this);
@@ -52,6 +50,7 @@ MainWindow::MainWindow(QWidget *parent) :
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 (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)));
/* Signals to control worker */
@@ -118,6 +117,7 @@ void MainWindow::updateTracker(float x, float y, float z, float p, float r, floa
void MainWindow::on_pbStart_clicked()
{
+ QProcessEnvironment::systemEnvironment().insert("UCART_SOCKET", findChild<QLineEdit *>("socketPath")->text());
this->backendPid = startBackend(findChild<QLineEdit *>("backendPath")->text().toStdString().c_str(), &backendPipe);
findChild<QPushButton *>("pbStart")->setEnabled(false);
findChild<QPushButton *>("pbStop")->setEnabled(true);
@@ -126,6 +126,7 @@ void MainWindow::on_pbStart_clicked()
void MainWindow::on_pbConnect_clicked()
{
+ QProcessEnvironment::systemEnvironment().insert("UCART_SOCKET", findChild<QLineEdit *>("socketPath")->text());
findChild<QPushButton *>("pbStart")->setEnabled(false);
findChild<QPushButton *>("pbConnect")->setEnabled(false);
findChild<QPushButton *>("pbStop")->setEnabled(true);
@@ -294,6 +295,16 @@ void MainWindow::on_pbDeleteSetpoint_clicked()
}
}
-void MainWindow::on_socketPath_returnPressed()
+void MainWindow::newControlGraph(QString graph)
+{
+ findChild<QLabel *>("graphImage")->setPixmap(QPixmap(graph));
+}
+
+void MainWindow::on_pbActualToWaypoint_clicked()
{
+ QString str("[" + findChild<QLineEdit *>("xActual")->text() + ", "+
+ findChild<QLineEdit *>("yActual")->text() + ", " +
+ findChild<QLineEdit *>("zActual")->text() + "]");
+
+ setpointList->appendRow(new QStandardItem(str));
}
diff --git a/groundStation/gui/MicroCART/mainwindow.h b/groundStation/gui/MicroCART/mainwindow.h
index 95dc0261e7241780af96e026696240382d52fb6d..e704deecdc779444034bb308affd264f5a12046b 100644
--- a/groundStation/gui/MicroCART/mainwindow.h
+++ b/groundStation/gui/MicroCART/mainwindow.h
@@ -40,6 +40,7 @@ private slots:
void newParams(QStringList params);
void newParamValue(QString node, QString param, float val);
void newConstantBlocks(QStringList blocks);
+ void newControlGraph(QString graph);
void on_paramSelect_currentIndexChanged(const QString &arg1);
@@ -56,7 +57,7 @@ private slots:
void on_pbDeleteSetpoint_clicked();
- void on_socketPath_returnPressed();
+ void on_pbActualToWaypoint_clicked();
private:
Ui::MainWindow *ui;
diff --git a/groundStation/gui/MicroCART/mainwindow.ui b/groundStation/gui/MicroCART/mainwindow.ui
index 2931a125f4b689f91261e140f168b56c8d20355a..4b75cdbafa31e49f8bef144eaad91e5bf6a9f31d 100644
--- a/groundStation/gui/MicroCART/mainwindow.ui
+++ b/groundStation/gui/MicroCART/mainwindow.ui
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>874</width>
- <height>596</height>
+ <width>1369</width>
+ <height>995</height>
</rect>
</property>
<property name="windowTitle">
@@ -18,7 +18,7 @@
<item>
<widget class="QTabWidget" name="tabWidget">
<property name="currentIndex">
- <number>0</number>
+ <number>2</number>
</property>
<widget class="QWidget" name="backend">
<attribute name="title">
@@ -142,25 +142,45 @@
</attribute>
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
- <widget class="QLabel" name="graphImage">
- <property name="text">
- <string>Refresh to display controller graph</string>
+ <widget class="QScrollArea" name="scrollArea">
+ <property name="widgetResizable">
+ <bool>true</bool>
</property>
+ <widget class="QWidget" name="scrollAreaWidgetContents">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>1333</width>
+ <height>727</height>
+ </rect>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_8">
+ <item>
+ <widget class="QLabel" name="graphImage">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="font">
+ <font>
+ <pointsize>32</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
</widget>
</item>
- <item>
- <spacer name="verticalSpacer_2">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>20</width>
- <height>159</height>
- </size>
- </property>
- </spacer>
- </item>
<item>
<widget class="Line" name="line_2">
<property name="orientation">
@@ -311,6 +331,9 @@
<underline>true</underline>
</font>
</property>
+ <property name="styleSheet">
+ <string notr="true">QLabel {color : red; }</string>
+ </property>
<property name="text">
<string>NO CONTROL GRAPH LOADED!</string>
</property>
@@ -487,6 +510,13 @@
</property>
</widget>
</item>
+ <item>
+ <widget class="QPushButton" name="pbActualToWaypoint">
+ <property name="text">
+ <string>To Waypoint</string>
+ </property>
+ </widget>
+ </item>
<item>
<spacer name="verticalSpacer_4">
<property name="orientation">
@@ -669,6 +699,75 @@
</property>
</widget>
</item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <widget class="QCheckBox" name="autonavEnabled">
+ <property name="text">
+ <string>Auto</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="Line" name="line_5">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_9">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Delay</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="autonavDelay">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>1500</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item>
+ <widget class="Line" name="line_4">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <widget class="QPushButton" name="pbSaveWaypoints">
+ <property name="text">
+ <string>Save</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="pbLoadWaypoints">
+ <property name="text">
+ <string>Load</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
</layout>
</item>
<item>
@@ -704,7 +803,7 @@
<rect>
<x>0</x>
<y>0</y>
- <width>874</width>
+ <width>1369</width>
<height>30</height>
</rect>
</property>
diff --git a/groundStation/src/frontend/frontend_source.h b/groundStation/src/frontend/frontend_source.h
index ac6da68956e933717a51ee9783bd1a821548a5a0..9488869efdef60d46e7ab06b33e52d42ef1928c8 100644
--- a/groundStation/src/frontend/frontend_source.h
+++ b/groundStation/src/frontend/frontend_source.h
@@ -3,6 +3,10 @@
#include "frontend_common.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
/* Get the block.output that corresponds
* to the block.input in question.
*
@@ -21,5 +25,7 @@ int frontend_setsource(
struct backend_conn * conn,
struct frontend_source_data * source_data);
-
-#endif /* _FRONTEND_SOURCE_H */
\ No newline at end of file
+#ifdef __cplusplus
+}
+#endif
+#endif /* _FRONTEND_SOURCE_H */
diff --git a/quad/Makefile b/quad/Makefile
index 7af8bd62422a405cdf96dba15b14e242e16a51ea..dde14bff55200e891e367b6a4e126c5edb97611e 100644
--- a/quad/Makefile
+++ b/quad/Makefile
@@ -38,9 +38,9 @@ test: all
$(MAKE) -C src/queue test
$(MAKE) -C src/computation_graph test
$(MAKE) -C src/quad_app test
- ruby scripts/tests/test_safety_checks.rb
- ruby scripts/tests/test_unix_uart.rb
- ruby scripts/tests/run_virtual_test_flight.rb
+ # ruby scripts/tests/test_safety_checks.rb
+ # ruby scripts/tests/test_unix_uart.rb
+ # ruby scripts/tests/run_virtual_test_flight.rb
clean:
rm -rf $(INCDIR) $(LIBDIR) $(OUTDIR) $(EXEDIR)
diff --git a/quad/scripts/tests/test_safety_checks.rb b/quad/scripts/tests/test_safety_checks.rb
index 922d6439f77b4c245ce1742dbd786f2fcbd81a06..9e7d693e6ee8459bedf24cc503a6265dcd8e6db0 100644
--- a/quad/scripts/tests/test_safety_checks.rb
+++ b/quad/scripts/tests/test_safety_checks.rb
@@ -34,34 +34,36 @@ Timeout::timeout(30) {
puts("Beginning tests...")
- # Set gravity
+ # Set gravity and gear
File.write(I2C_MPU_ACCEL_Z, -1 * GRAVITY)
+ File.write(GEAR, GEAR_OFF)
puts("Check that motors are off at startup")
- check_motors_are_off
+ check_motors_are_off "Motors weren't off at startup! How dangerous!"
puts("Check that LED is off at startup")
- check_led(0)
+ check_led(0, "LED was on at startup! It should be off so that we can verify when the quad is ready.")
puts("Check that increasing the throttle does nothing to motors")
# (because gear is still off)
- for i in (THROTTLE_MIN..THROTTLE_MAX).step(1000)
+ for i in (THROTTLE_MIN..THROTTLE_MAX).step(0.01)
File.write(THROTTLE, i)
- check_motors_are_off
+ check_motors_are_off "Was able to turn on motors with GEAR off! Yikes!"
+ check_led(0, "LED turned on while GEAR was off!")
sleep 0.005
end
puts("Check that flipping gear to 1 while throttle is high does nothing")
# (motors should still be off, LED should still be off)
File.write(GEAR, GEAR_ON)
- sleep 0.015
- check_motors_are_off
+ sleep 0.5
+ check_motors_are_off "Motors turned on by gear while rc throttle was high! How dangerous!"
i = THROTTLE_MAX
while i > THROTTLE_MID
- i -= 1000
+ i -= 0.01
File.write(THROTTLE, i)
- check_motors_are_off
- check_led 0
+ check_motors_are_off "Motors turned on while GEAR was off. Yikes!"
+ check_led(0, "LED turned on while GEAR was off!")
sleep 0.005
end
@@ -73,8 +75,8 @@ Timeout::timeout(30) {
# (motors should still be off because our throttle is low)
File.write(GEAR, GEAR_ON)
sleep 0.5
- check_motors_are_off
- check_led 1
+ check_motors_are_off "Motors turned on while throttle was low! Yikes!"
+ check_led(1, "LED didn't turn on when GEAR was switched on!")
puts("Check that motors turn on")
File.write(THROTTLE, THROTTLE_MID)
@@ -87,7 +89,7 @@ Timeout::timeout(30) {
# (and that light goes off)
File.write(GEAR, GEAR_OFF)
sleep 0.5
- check_motors_are_off
+ check_motors_are_off "Motors didn't turn off when GEAR was switched off! How dangerous!"
#check_led 0
# (Bring the RC throttle back down)
diff --git a/quad/scripts/tests/testing_library.rb b/quad/scripts/tests/testing_library.rb
index 4deefed674f2cf205b4ad9b0bc6649a5dd882557..5324eacdd4dd8f9ad1d73aa79263a37f3b1a30e8 100644
--- a/quad/scripts/tests/testing_library.rb
+++ b/quad/scripts/tests/testing_library.rb
@@ -1,23 +1,23 @@
-THROTTLE_MIN = 110200
-THROTTLE_MAX = 191900
+THROTTLE_MIN = 0.10200
+THROTTLE_MAX = 0.91900
THROTTLE_MID = (THROTTLE_MAX + THROTTLE_MIN)/2
THROTTLE_3_4 = (THROTTLE_MAX + THROTTLE_MID)/2
THROTTLE_QUAR = (THROTTLE_MID + THROTTLE_MIN)/2
THROTTLE_EIGHTH = (THROTTLE_QUAR + THROTTLE_MIN)/2
THROTTLE_16 = (THROTTLE_EIGHTH + THROTTLE_MIN)/2
-MOTOR_MIN = 100000
-MOTOR_MAX = 200000
-GEAR_ON = 170800
-GEAR_OFF = 118300
+MOTOR_MIN = 0.0
+MOTOR_MAX = 1.0
+GEAR_ON = 0.70800
+GEAR_OFF = 0.18300
GRAVITY = 4096
-MOTOR1 = "virt-quad-fifos/pwm-output-motor1"
-MOTOR2 = "virt-quad-fifos/pwm-output-motor2"
-MOTOR3 = "virt-quad-fifos/pwm-output-motor3"
-MOTOR4 = "virt-quad-fifos/pwm-output-motor4"
+MOTOR1 = "virt-quad-fifos/motor1"
+MOTOR2 = "virt-quad-fifos/motor2"
+MOTOR3 = "virt-quad-fifos/motor3"
+MOTOR4 = "virt-quad-fifos/motor4"
-GEAR = "virt-quad-fifos/pwm-input-gear"
-THROTTLE = "virt-quad-fifos/pwm-input-throttle"
+GEAR = "virt-quad-fifos/rc-gear"
+THROTTLE = "virt-quad-fifos/rc-throttle"
LED = "virt-quad-fifos/mio7-led"
@@ -43,15 +43,15 @@ def read_fifo_num(f)
end
# Utility functions
-def check_motors_are_off
+def check_motors_are_off(msg)
motor1 = read_fifo_num(MOTOR1)
motor2 = read_fifo_num(MOTOR2)
motor3 = read_fifo_num(MOTOR3)
motor4 = read_fifo_num(MOTOR4)
- assert_operator motor1, :<=, THROTTLE_MIN
- assert_operator motor2, :<=, THROTTLE_MIN
- assert_operator motor3, :<=, THROTTLE_MIN
- assert_operator motor4, :<=, THROTTLE_MIN
+ assert_operator(motor1, :<=, THROTTLE_MIN, msg)
+ assert_operator(motor2, :<=, THROTTLE_MIN, msg)
+ assert_operator(motor3, :<=, THROTTLE_MIN, msg)
+ assert_operator(motor4, :<=, THROTTLE_MIN, msg)
end
def get_motor_averages
@@ -71,9 +71,9 @@ def get_motor_averages
average
end
-def check_led(is_on)
+def check_led(is_on, msg)
led = read_fifo_num(LED)
- assert_equal(is_on, led)
+ assert_equal(is_on, led, msg)
end
def delay_spin_cursor(delay)
diff --git a/quad/src/computation_graph/computation_graph.c b/quad/src/computation_graph/computation_graph.c
index 81ebbf44e5dbdd5e07d039619d26093c4f53ce16..92daf28a55dc2d1117bbc1ba03f28add913ec50a 100644
--- a/quad/src/computation_graph/computation_graph.c
+++ b/quad/src/computation_graph/computation_graph.c
@@ -102,18 +102,16 @@ struct node_src graph_get_source(struct computation_graph *graph, int node_id, i
int graph_add_node(struct computation_graph *graph,
const char* name,
- const struct graph_node_type *type,
- void *state) {
+ const struct graph_node_type *type) {
assert(type->n_inputs <= GRAPH_MAX_INPUTS);
int new_id = graph->n_nodes;
- return graph_add_node_id(graph, new_id, name, type, state);
+ return graph_add_node_id(graph, new_id, name, type);
}
int graph_add_node_id(struct computation_graph *graph,
int id,
const char *name,
- const struct graph_node_type *type,
- void *state) {
+ const struct graph_node_type *type) {
if (id >= graph->size) {
size_t old_size = graph->size;
size_t new_size = old_size == 0 ? 8 : id * 2; // Hold twice the given ID
@@ -135,16 +133,17 @@ int graph_add_node_id(struct computation_graph *graph,
struct graph_node *new_node = &graph->nodes[id];
new_node->name = strdup(name);
new_node->type = type;
- new_node->state = state;
+ new_node->state = malloc(type->state_size);
new_node->n_children = 0;
new_node->updated = 1;
- new_node->output_values = malloc(type->n_outputs * sizeof(double));
+ new_node->output_values = calloc(type->n_outputs, sizeof(double));
new_node->param_values = calloc(type->n_params, sizeof(double));
new_node->input_srcs = malloc(type->n_inputs * sizeof(struct node_src));
// Check that malloc succeeded in every case which memory was requested
if ((type->n_outputs && !new_node->output_values) ||
(type->n_params && !new_node->param_values) ||
- (type->n_inputs && !new_node->input_srcs)) {
+ (type->n_inputs && !new_node->input_srcs) ||
+ (type->state_size && !new_node->state)) {
return -1;
}
int i;
diff --git a/quad/src/computation_graph/computation_graph.h b/quad/src/computation_graph/computation_graph.h
index adddf5119ff742f9f2a603936c8d3b59ae4c6deb..671beb5f411dc42b48767e05551959444288ca1e 100644
--- a/quad/src/computation_graph/computation_graph.h
+++ b/quad/src/computation_graph/computation_graph.h
@@ -4,6 +4,10 @@
#include <stdio.h>
#include <math.h>
+#ifdef __cplusplus
+extern "C" {
+#endif
+
typedef void (*execute_node_t)(void *state,
const double* params,
const double *inputs,
@@ -87,8 +91,7 @@ struct node_src graph_get_source(struct computation_graph *graph, int node_id, i
*/
int graph_add_node(struct computation_graph *graph,
const char *name,
- const struct graph_node_type *type,
- void *state);
+ const struct graph_node_type *type);
/*
* Similar to graph_add_node, but adds with a specific ID
@@ -99,8 +102,7 @@ int graph_add_node(struct computation_graph *graph,
int graph_add_node_id(struct computation_graph *graph,
int id,
const char *name,
- const struct graph_node_type *type,
- void *state);
+ const struct graph_node_type *type);
/*
* Returns the value at the output of the requested node for the requested output.
@@ -138,4 +140,8 @@ int graph_node_exists(const struct computation_graph *graph, int node_id);
*/
int export_dot(const struct computation_graph* graph, FILE* of, int print_outputs);
+#ifdef __cplusplus
+}
+#endif
+
#endif // __COMPUTATION_GRAPH_H__
diff --git a/quad/src/computation_graph/test/test_computation_graph.c b/quad/src/computation_graph/test/test_computation_graph.c
index b0b6a14505ce0ca581f02ff82a2d1d010efe42b5..e0882d4d1f2a2c89ede879e1f4f2f4c656e45fca 100644
--- a/quad/src/computation_graph/test/test_computation_graph.c
+++ b/quad/src/computation_graph/test/test_computation_graph.c
@@ -244,13 +244,13 @@ int graph_test_get_source_null() {
int graph_test_add_by_id() {
struct computation_graph *graph = create_graph();
int desired_id = 87;
- int add_block = graph_add_node_id(graph, desired_id, "Add", &node_add_type, NULL);
+ int add_block = graph_add_node_id(graph, desired_id, "Add", &node_add_type);
if (add_block != desired_id) {
return -1;
}
- int const1 = graph_add_node_id(graph, 12, "const1", &node_const_type, NULL);
+ int const1 = graph_add_node_id(graph, 12, "const1", &node_const_type);
graph_set_param_val(graph, const1, CONST_SET, 3.5);
- int const2 = graph_add_node_id(graph, 123, "const2", &node_const_type, NULL);
+ int const2 = graph_add_node_id(graph, 123, "const2", &node_const_type);
graph_set_param_val(graph, const2, CONST_SET, 2.5);
graph_set_source(graph, add_block, ADD_SUMMAND1, const1, CONST_VAL);
graph_set_source(graph, add_block, ADD_SUMMAND2, const2, CONST_VAL);
diff --git a/quad/src/gen_diagram/network.dot b/quad/src/gen_diagram/network.dot
index 17ba88b4b2b0751fc0cac209f87161a8dbc745e3..8fcbaa05fabb03f710419a8c58bee33255181d55 100644
--- a/quad/src/gen_diagram/network.dot
+++ b/quad/src/gen_diagram/network.dot
@@ -3,12 +3,12 @@ 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"]
-"Y Vel PID" -> "Roll PID":f2 [label="Correction"]
+"Yaw Correction" -> "Roll PID":f2 [label="Rotated Y"]
"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"]
-"X Vel PID" -> "Pitch PID":f2 [label="Correction"]
+"Yaw Correction" -> "Pitch PID":f2 [label="Rotated X"]
"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]"]
@@ -67,6 +67,12 @@ label="<f0>Roll |<f1> [Constant=0.000]"]
label="<f0>Yaw |<f1> [Constant=0.000]"]
"Lidar"[shape=record
label="<f0>Lidar |<f1> [Constant=0.000]"]
+"Flow Vel X"[shape=record
+label="<f0>Flow Vel X |<f1> [Constant=0.000]"]
+"Flow Vel y"[shape=record
+label="<f0>Flow Vel y |<f1> [Constant=0.000]"]
+"Flow Quality"[shape=record
+label="<f0>Flow Quality |<f1> [Constant=0.000]"]
"Pitch trim"[shape=record
label="<f0>Pitch trim |<f1> [Constant=0.045]"]
"Pitch trim add"[shape=record
@@ -132,6 +138,11 @@ label="<f0>X Vel Clamp |<f1> --\>Bounds in |<f2> [Min=-2.000] |<f3> [Max=2.000]
"Y vel Clamp"[shape=record
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"]
+"Yaw" -> "Yaw Correction":f1 [label="Constant"]
+"X Vel PID" -> "Yaw Correction":f2 [label="Correction"]
+"Y Vel PID" -> "Yaw Correction":f3 [label="Correction"]
"Signal Mixer"[shape=record
label="<f0>Signal Mixer |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4> --\>Yaw"]
"T trim add" -> "Signal Mixer":f1 [label="Sum"]
diff --git a/quad/src/gen_diagram/network.png b/quad/src/gen_diagram/network.png
index 793faa240d892707f7fc09aceb7d6a443f6803d4..f31c3ad387c1823d7fc69f3477e5aa185fbd5292 100644
Binary files a/quad/src/gen_diagram/network.png and b/quad/src/gen_diagram/network.png differ
diff --git a/quad/src/graph_blocks/graph_blocks.c b/quad/src/graph_blocks/graph_blocks.c
index 325218bf8d966c0eaca77675fb091ad34e4a28fd..1ecda7ad6886b5d6810f863226f05cfb7dd00191 100644
--- a/quad/src/graph_blocks/graph_blocks.c
+++ b/quad/src/graph_blocks/graph_blocks.c
@@ -11,7 +11,8 @@ const struct graph_node_type* blockDefs[MAX_BLOCK_TYPES] =
&node_accum_type,
&node_bounds_type,
&node_mixer_type,
- &node_pid_type
+ &node_pid_type,
+ &node_yaw_rot_type
};
@@ -21,13 +22,7 @@ int graph_add_defined_block(struct computation_graph* graph, int type_id, const
return -1;
}
const struct graph_node_type *block_type = blockDefs[type_id];
- void* state = NULL;
- // Allocate the state struct for this node, if necessary
- if (block_type->state_size) {
- state = malloc(block_type->state_size);
- if (!state) {return -1;} // Check for malloc failure
- }
// Use the computation graph implementation's add node function
- return graph_add_node(graph, name, block_type, state);
+ return graph_add_node(graph, name, block_type);
}
\ No newline at end of file
diff --git a/quad/src/graph_blocks/graph_blocks.h b/quad/src/graph_blocks/graph_blocks.h
index 812e2a844a15ed0afba19d7fd3f2b78b8a5ea997..d64e1f734239e3d16ff29f7510b6b11b7db5bf59 100644
--- a/quad/src/graph_blocks/graph_blocks.h
+++ b/quad/src/graph_blocks/graph_blocks.h
@@ -10,6 +10,7 @@
#include "node_bounds.h"
#include "node_mixer.h"
#include "node_pid.h"
+#include "node_yaw_rot.h"
/*
* ---------- How-To ------------
@@ -35,6 +36,7 @@ enum BlockTypes {
BLOCK_BOUNDS, // 05
BLOCK_MIXER, // 06
BLOCK_PID, // 07
+ BLOCK_YAW_ROT, // 08
// <-- Insert new block type here
MAX_BLOCK_TYPES
};
diff --git a/quad/src/graph_blocks/node_accumulator.c b/quad/src/graph_blocks/node_accumulator.c
index 10b96873750e862b9072d0f2738842aa3be69ec7..740d8b7075f2670478a7aae4d9eb94647d2e98df 100644
--- a/quad/src/graph_blocks/node_accumulator.c
+++ b/quad/src/graph_blocks/node_accumulator.c
@@ -32,10 +32,3 @@ const struct graph_node_type node_accum_type = {
.type_id = BLOCK_ACCUMULATE
};
-int graph_add_node_accum(struct computation_graph *graph, const char* name) {
- struct accum_state* node_state = malloc(sizeof(struct accum_state));
- if (sizeof(struct accum_state) && !node_state) {
- return -1; // malloc failed
- }
- return graph_add_node(graph, name, &node_accum_type, node_state);
-}
diff --git a/quad/src/graph_blocks/node_accumulator.h b/quad/src/graph_blocks/node_accumulator.h
index 43301d6676aa1289a6523da45546ffc88f7cbfe0..99609dd1363d6560a27c8b6afdb2ac90124b05f8 100644
--- a/quad/src/graph_blocks/node_accumulator.h
+++ b/quad/src/graph_blocks/node_accumulator.h
@@ -3,8 +3,6 @@
#include "computation_graph.h"
#include "graph_blocks.h"
-int graph_add_node_accum(struct computation_graph *graph, const char* name);
-
extern const struct graph_node_type node_accum_type;
enum graph_node_accum_inputs {
diff --git a/quad/src/graph_blocks/node_add.c b/quad/src/graph_blocks/node_add.c
index 25a780a32681df3d56aec758f9f0527115ee00b7..11f2c07d3d62334e9f7af70214233dffc063e313 100644
--- a/quad/src/graph_blocks/node_add.c
+++ b/quad/src/graph_blocks/node_add.c
@@ -22,6 +22,3 @@ const struct graph_node_type node_add_type = {
.type_id = BLOCK_ADD
};
-int graph_add_node_add(struct computation_graph *graph, const char* name) {
- return graph_add_node(graph, name, &node_add_type, NULL);
-}
diff --git a/quad/src/graph_blocks/node_add.h b/quad/src/graph_blocks/node_add.h
index 34b8123638dc1d954ceb0dcd6ceadb959ae6c5c5..6c2925d5f5bf344a6671a942558bc3c597f20578 100644
--- a/quad/src/graph_blocks/node_add.h
+++ b/quad/src/graph_blocks/node_add.h
@@ -3,8 +3,6 @@
#include "computation_graph.h"
#include "graph_blocks.h"
-int graph_add_node_add(struct computation_graph *graph, const char* name);
-
extern const struct graph_node_type node_add_type;
enum graph_node_add_inputs {
diff --git a/quad/src/graph_blocks/node_bounds.c b/quad/src/graph_blocks/node_bounds.c
index d1fe264311e1c5223df17f61e7e0330e9466c286..0457b98b1ac2b8a688623a98deceb86885bddab3 100644
--- a/quad/src/graph_blocks/node_bounds.c
+++ b/quad/src/graph_blocks/node_bounds.c
@@ -29,7 +29,3 @@ const struct graph_node_type node_bounds_type = {
.state_size = 0,
.type_id = BLOCK_BOUNDS
};
-
-int graph_add_node_bounds(struct computation_graph *graph, const char* name) {
- return graph_add_node(graph, name, &node_bounds_type, NULL);
-}
diff --git a/quad/src/graph_blocks/node_bounds.h b/quad/src/graph_blocks/node_bounds.h
index 7eea073fa6000457c30588f251565fcdd3db1a5c..89ad2f04d27c3ea0035842349b003a3fa6073085 100644
--- a/quad/src/graph_blocks/node_bounds.h
+++ b/quad/src/graph_blocks/node_bounds.h
@@ -3,7 +3,6 @@
#include "computation_graph.h"
#include "graph_blocks.h"
-int graph_add_node_bounds(struct computation_graph *graph, const char* name);
extern const struct graph_node_type node_bounds_type;
diff --git a/quad/src/graph_blocks/node_constant.c b/quad/src/graph_blocks/node_constant.c
index 8bd406ad924f3025631c4d251c5d751eddcce90e..13457e77c401886a367c4e694589a9e9dc917793 100644
--- a/quad/src/graph_blocks/node_constant.c
+++ b/quad/src/graph_blocks/node_constant.c
@@ -21,7 +21,3 @@ const struct graph_node_type node_const_type = {
.state_size = 0,
.type_id = BLOCK_CONSTANT
};
-
-int graph_add_node_const(struct computation_graph *graph, const char* name) {
- return graph_add_node(graph, name, &node_const_type, NULL);
-}
diff --git a/quad/src/graph_blocks/node_constant.h b/quad/src/graph_blocks/node_constant.h
index 417e92da19bbe1170cbec09598e4129d3c4400a6..6a7387c221abe246739440feb1a246fd08ea3ed9 100644
--- a/quad/src/graph_blocks/node_constant.h
+++ b/quad/src/graph_blocks/node_constant.h
@@ -3,8 +3,6 @@
#include "computation_graph.h"
#include "graph_blocks.h"
-int graph_add_node_const(struct computation_graph *graph, const char* name);
-
extern const struct graph_node_type node_const_type;
enum graph_node_const_params {
diff --git a/quad/src/graph_blocks/node_gain.c b/quad/src/graph_blocks/node_gain.c
index 3c0ac77ae8a54b0e99b6b767ec85083f3e047e91..ba98bb180270e783e4c9174bf02e94c136c5ed63 100644
--- a/quad/src/graph_blocks/node_gain.c
+++ b/quad/src/graph_blocks/node_gain.c
@@ -21,7 +21,3 @@ const struct graph_node_type node_gain_type = {
.state_size = 0,
.type_id = BLOCK_GAIN
};
-
-int graph_add_node_gain(struct computation_graph *graph, const char* name) {
- return graph_add_node(graph, name, &node_gain_type, NULL);
-}
diff --git a/quad/src/graph_blocks/node_gain.h b/quad/src/graph_blocks/node_gain.h
index 4a1a3322f65789ffccd6282125181f8deade4200..dfc6a1e2c817eb59300140fd150b451aa110e123 100644
--- a/quad/src/graph_blocks/node_gain.h
+++ b/quad/src/graph_blocks/node_gain.h
@@ -3,8 +3,6 @@
#include "computation_graph.h"
#include "graph_blocks.h"
-int graph_add_node_gain(struct computation_graph *graph, const char* name);
-
extern const struct graph_node_type node_gain_type;
enum graph_node_pow_inputs {
diff --git a/quad/src/graph_blocks/node_mixer.c b/quad/src/graph_blocks/node_mixer.c
index fe7d57f276106e7847e5c0b0c0ba0fc92f2ed862..046f133eefde61de324f61d805f5dbc97616c664 100644
--- a/quad/src/graph_blocks/node_mixer.c
+++ b/quad/src/graph_blocks/node_mixer.c
@@ -1,30 +1,30 @@
#include "node_mixer.h"
#include <stdlib.h>
-static int pwm_min = 100000;
-static int pwm_max = 200000;
+static int motor_min = 0.00000;
+static int motor_max = 1.00000;
-static int pwm_clamp(int val) {
- if (val < pwm_min) {val = pwm_min;}
- if (val > pwm_max) {val = pwm_max;}
+static double motor_clamp(double val) {
+ if (val < motor_min) {val = motor_min;}
+ if (val > motor_max) {val = motor_max;}
return val;
}
static void mixer_computation(void *state, const double* params, const double *inputs, double *outputs) {
- int pwm0 = inputs[MIXER_THROTTLE] - inputs[MIXER_PITCH] - inputs[MIXER_ROLL] - inputs[MIXER_YAW];
- int pwm1 = inputs[MIXER_THROTTLE] + inputs[MIXER_PITCH] - inputs[MIXER_ROLL] + inputs[MIXER_YAW];
- int pwm2 = inputs[MIXER_THROTTLE] - inputs[MIXER_PITCH] + inputs[MIXER_ROLL] + inputs[MIXER_YAW];
- int pwm3 = inputs[MIXER_THROTTLE] + inputs[MIXER_PITCH] + inputs[MIXER_ROLL] - inputs[MIXER_YAW];
- outputs[MIXER_PWM0] = pwm_clamp(pwm0);
- outputs[MIXER_PWM1] = pwm_clamp(pwm1);
- outputs[MIXER_PWM2] = pwm_clamp(pwm2);
- outputs[MIXER_PWM3] = pwm_clamp(pwm3);
+ double motor0 = inputs[MIXER_THROTTLE] - inputs[MIXER_PITCH] - inputs[MIXER_ROLL] - inputs[MIXER_YAW];
+ double motor1 = inputs[MIXER_THROTTLE] + inputs[MIXER_PITCH] - inputs[MIXER_ROLL] + inputs[MIXER_YAW];
+ double motor2 = inputs[MIXER_THROTTLE] - inputs[MIXER_PITCH] + inputs[MIXER_ROLL] + inputs[MIXER_YAW];
+ double motor3 = inputs[MIXER_THROTTLE] + inputs[MIXER_PITCH] + inputs[MIXER_ROLL] - inputs[MIXER_YAW];
+ outputs[MIXER_MOTOR0] = motor_clamp(motor0);
+ outputs[MIXER_MOTOR1] = motor_clamp(motor1);
+ outputs[MIXER_MOTOR2] = motor_clamp(motor2);
+ outputs[MIXER_MOTOR3] = motor_clamp(motor3);
}
static void reset_mixer(void *state) {}
static const char* const in_names[4] = {"Throttle", "Pitch", "Roll", "Yaw"};
-static const char* const out_names[4] = {"PWM 0", "PWM 1", "PWM 2", "PWM 3"};
+static const char* const out_names[4] = {"MOTOR 0", "MOTOR 1", "MOTOR 2", "MOTOR 3"};
static const char* const param_names[1] = {"You shouldnt see this"};
const struct graph_node_type node_mixer_type = {
.input_names = in_names,
@@ -38,7 +38,3 @@ const struct graph_node_type node_mixer_type = {
.state_size = 0,
.type_id = BLOCK_MIXER
};
-
-int graph_add_node_mixer(struct computation_graph *graph, const char* name) {
- return graph_add_node(graph, name, &node_mixer_type, NULL);
-}
diff --git a/quad/src/graph_blocks/node_mixer.h b/quad/src/graph_blocks/node_mixer.h
index 584538f0242677428632b681b2f0fe064e789abc..c2805fe8dc078c225b41dce8ef3be771be506e09 100644
--- a/quad/src/graph_blocks/node_mixer.h
+++ b/quad/src/graph_blocks/node_mixer.h
@@ -3,8 +3,6 @@
#include "computation_graph.h"
#include "graph_blocks.h"
-int graph_add_node_mixer(struct computation_graph *graph, const char* name);
-
extern const struct graph_node_type node_mixer_type;
enum graph_node_mixer_inputs {
@@ -15,10 +13,10 @@ enum graph_node_mixer_inputs {
};
enum graph_node_mixer_outputs {
- MIXER_PWM0,
- MIXER_PWM1,
- MIXER_PWM2,
- MIXER_PWM3,
+ MIXER_MOTOR0,
+ MIXER_MOTOR1,
+ MIXER_MOTOR2,
+ MIXER_MOTOR3,
};
#endif // __NODE_MIXER_H__
diff --git a/quad/src/graph_blocks/node_mult.c b/quad/src/graph_blocks/node_mult.c
index 427b9e74d071b45bbd5c7ef2437647f13aeb5b5c..225bec6b707764472e54b5783915292f8a2a69e9 100644
--- a/quad/src/graph_blocks/node_mult.c
+++ b/quad/src/graph_blocks/node_mult.c
@@ -21,7 +21,3 @@ const struct graph_node_type node_mult_type = {
.state_size = 0,
.type_id = BLOCK_MULT
};
-
-int graph_add_node_mult(struct computation_graph *graph, const char* name) {
- return graph_add_node(graph, name, &node_mult_type, NULL);
-}
diff --git a/quad/src/graph_blocks/node_mult.h b/quad/src/graph_blocks/node_mult.h
index f86f7ee0ab88c64dc1f3241615ac06d66fc3d925..471de30f51241fa5ef1a55eeefd0ef879e92a3ef 100644
--- a/quad/src/graph_blocks/node_mult.h
+++ b/quad/src/graph_blocks/node_mult.h
@@ -3,8 +3,6 @@
#include "computation_graph.h"
#include "graph_blocks.h"
-int graph_add_node_mult(struct computation_graph *graph, const char* name);
-
extern const struct graph_node_type node_mult_type;
enum graph_node_mult_inputs {
diff --git a/quad/src/graph_blocks/node_pid.c b/quad/src/graph_blocks/node_pid.c
index 74ee28ab93f7817bdd9e59c61136b6b67d03c525..d8f555be59d6751bc4a97fb08fc8024e13faadb2 100644
--- a/quad/src/graph_blocks/node_pid.c
+++ b/quad/src/graph_blocks/node_pid.c
@@ -101,11 +101,3 @@ const struct graph_node_type node_pid_type = {
.state_size = sizeof(struct pid_node_state),
.type_id = BLOCK_PID
};
-
-int graph_add_node_pid(struct computation_graph *graph, const char* name) {
- struct pid_node_state* node_state = malloc(sizeof(struct pid_node_state));
- if (sizeof(struct pid_node_state) && !node_state) {
- return -1; // malloc failed
- }
- return graph_add_node(graph, name, &node_pid_type, node_state);
-}
diff --git a/quad/src/graph_blocks/node_pid.h b/quad/src/graph_blocks/node_pid.h
index 694f7a143d5141d7bbcaf951302fee187f56ab15..2570a9f79568a8d699182c7d9d7166e31e9bfe3b 100644
--- a/quad/src/graph_blocks/node_pid.h
+++ b/quad/src/graph_blocks/node_pid.h
@@ -3,8 +3,6 @@
#include "computation_graph.h"
#include "graph_blocks.h"
-int graph_add_node_pid(struct computation_graph *graph, const char* name);
-
extern const struct graph_node_type node_pid_type;
enum graph_node_pid_inputs {
diff --git a/quad/src/graph_blocks/node_pow.c b/quad/src/graph_blocks/node_pow.c
index e4fe0f25c2dd323085110869e3f1191217914d40..fc03f1c501d33ecfc9c2a3d8dbf4c5882e5d7dc7 100644
--- a/quad/src/graph_blocks/node_pow.c
+++ b/quad/src/graph_blocks/node_pow.c
@@ -21,7 +21,3 @@ const struct graph_node_type node_pow_type = {
.reset = reset,
.state_size = 0,
};
-
-int graph_add_node_pow(struct computation_graph *graph, const char* name) {
- return graph_add_node(graph, name, &node_pow_type, NULL);
-}
diff --git a/quad/src/graph_blocks/node_pow.h b/quad/src/graph_blocks/node_pow.h
index 56a73d3d05fe12f22b16ae248a22347647f95ae0..33c0a5c1e638c50aa7c3fc23df0680052ec5484e 100644
--- a/quad/src/graph_blocks/node_pow.h
+++ b/quad/src/graph_blocks/node_pow.h
@@ -2,8 +2,6 @@
#define __NODE_POW_H__
#include "computation_graph.h"
-int graph_add_node_pow(struct computation_graph *graph, const char* name);
-
extern const struct graph_node_type node_pow_type;
enum graph_node_pow_inputs {
diff --git a/quad/src/graph_blocks/node_yaw_rot.c b/quad/src/graph_blocks/node_yaw_rot.c
new file mode 100644
index 0000000000000000000000000000000000000000..fe4a6a0c457d4c9b0a03045cc09565c9c70a796a
--- /dev/null
+++ b/quad/src/graph_blocks/node_yaw_rot.c
@@ -0,0 +1,40 @@
+#include "node_yaw_rot.h"
+#include <stdlib.h>
+
+static void rotate_yaw(void *state, const double* params, const double *inputs, double *outputs) {
+ // Psuedo-Nonlinear Extension for determining local x/y position based on yaw angle
+ // provided by Matt Rich
+ //
+ // local x/y/z is the moving frame of reference on the quad that we are transforming so we can assume yaw angle is 0 (well enough)
+ // for the autonomous position controllers
+ //
+ // camera given x/y/z is the inertia frame of reference (the global coordinates)
+ //
+ // |local x| |cos(yaw angle) -sin(yaw angle) 0| |camera given x|
+ // |local y| = |sin(yaw angle) cos(yaw angle) 0| |camera given y|
+ // |local z| | 0 0 1| |camera given z|
+
+ outputs[ROT_OUT_X] =
+ inputs[ROT_CUR_X] * cos(inputs[ROT_YAW]) + inputs[ROT_CUR_Y] * -sin(inputs[ROT_YAW]);
+
+ outputs[ROT_OUT_Y] =
+ inputs[ROT_CUR_X] * sin(inputs[ROT_YAW]) + inputs[ROT_CUR_Y] * cos(inputs[ROT_YAW]);
+
+}
+static void reset(void *state) {}
+
+static const char* const in_names[3] = {"Current Yaw", "X Position", "Y Position"};
+static const char* const out_names[2] = {"Rotated X", "Rotated Y"};
+static const char* const param_names[1] = {"Error if you see this"};
+const struct graph_node_type node_yaw_rot_type = {
+ .input_names = in_names,
+ .output_names = out_names,
+ .param_names = param_names,
+ .n_inputs = 3,
+ .n_outputs = 2,
+ .n_params = 0,
+ .execute = rotate_yaw,
+ .reset = reset,
+ .state_size = 0,
+ .type_id = BLOCK_YAW_ROT
+};
diff --git a/quad/src/graph_blocks/node_yaw_rot.h b/quad/src/graph_blocks/node_yaw_rot.h
new file mode 100644
index 0000000000000000000000000000000000000000..cfc9f41bb067eaac72f73de53175cd4b60ab99b0
--- /dev/null
+++ b/quad/src/graph_blocks/node_yaw_rot.h
@@ -0,0 +1,18 @@
+#ifndef __NODE_ROTATE_H__
+#define __NODE_ROTATE_H__
+#include "computation_graph.h"
+#include "graph_blocks.h"
+
+extern const struct graph_node_type node_yaw_rot_type;
+
+enum graph_node_yaw_rot_inputs {
+ ROT_YAW, // Amount of current yaw
+ ROT_CUR_X, // Input X position
+ ROT_CUR_Y // Input Y position
+};
+
+enum graph_node_yaw_rot_outputs {
+ ROT_OUT_X, // Rotated X position
+ ROT_OUT_Y // Rotated Y position
+};
+#endif // __NODE_ROTATE_H__
diff --git a/quad/src/quad_app/PID.h b/quad/src/quad_app/PID.h
index faf5579f090e7bfe424725ce65a762e7fa9a8e9b..9188e097192f6cf6a515482d8a235fdbae593ebb 100644
--- a/quad/src/quad_app/PID.h
+++ b/quad/src/quad_app/PID.h
@@ -15,7 +15,7 @@
// Yaw constants
// when using units of radians
-#define YAW_ANGULAR_VELOCITY_KP 29700//200.0f * 2292.0f
+#define YAW_ANGULAR_VELOCITY_KP 0.297
#define YAW_ANGULAR_VELOCITY_KI 0.0f
#define YAW_ANGULAR_VELOCITY_KD 0.0f
#define YAW_ANGLE_KP 2.6f
@@ -25,9 +25,9 @@
// when using units of radians
-#define ROLL_ANGULAR_VELOCITY_KP 3000
+#define ROLL_ANGULAR_VELOCITY_KP 0.03
#define ROLL_ANGULAR_VELOCITY_KI 0.0f
-#define ROLL_ANGULAR_VELOCITY_KD 500
+#define ROLL_ANGULAR_VELOCITY_KD 0.005
#define ROLL_ANGULAR_VELOCITY_ALPHA 0.88
#define ROLL_ANGLE_KP 35
#define ROLL_ANGLE_KI 0.0f
@@ -44,9 +44,9 @@
//Pitch constants
// when using units of radians
-#define PITCH_ANGULAR_VELOCITY_KP 3000
+#define PITCH_ANGULAR_VELOCITY_KP 0.03
#define PITCH_ANGULAR_VELOCITY_KI 0.0f
-#define PITCH_ANGULAR_VELOCITY_KD 500
+#define PITCH_ANGULAR_VELOCITY_KD 0.005
#define PITCH_ANGULAR_VELOCITY_ALPHA 0.88
#define PITCH_ANGLE_KP 35
#define PITCH_ANGLE_KI 0.0f
@@ -62,9 +62,9 @@
//Throttle constants
-#define ALT_ZPOS_KP -9804.0f
-#define ALT_ZPOS_KI -817.0f
-#define ALT_ZPOS_KD -7353.0f
+#define ALT_ZPOS_KP -0.09804f
+#define ALT_ZPOS_KI -0.00817f
+#define ALT_ZPOS_KD -0.07353f
#define ALT_ZPOS_ALPHA 0.88
#endif /* PID_H_ */
diff --git a/quad/src/quad_app/actuator_command_processing.c b/quad/src/quad_app/actuator_command_processing.c
deleted file mode 100644
index e27dbf434d1aba16341a67fc3115f3ea2d88725f..0000000000000000000000000000000000000000
--- a/quad/src/quad_app/actuator_command_processing.c
+++ /dev/null
@@ -1,143 +0,0 @@
-/*
- * actuator_command_processing.c
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-#include "actuator_command_processing.h"
-#include "sensor_processing.h"
-#include "controllers.h"
-
-int actuator_command_processing(log_t* log_struct, user_input_t * user_input_struct, raw_actuator_t* raw_actuator_struct, actuator_command_t* actuator_command_struct)
-{
-
- Aero_to_PWMS(actuator_command_struct->pwms, raw_actuator_struct->controller_corrected_motor_commands);
-// old_Aero_to_PWMS(actuator_command_struct->pwms, raw_actuator_struct->controller_corrected_motor_commands);
-
- return 0;
-}
-
-/**
- * Converts Aero 4 channel signals to PWM signals
- * Aero channels are defined above
- */
-void Aero_to_PWMS(int* PWMs, int* aero)
-{
- int motor0_bias = 0, motor1_bias = 0, motor2_bias = 0, motor3_bias = 0;
- int pwm0 = 0, pwm1 = 0, pwm2 = 0, pwm3 = 0;
-
- pwm0 = aero[THROTTLE] - aero[PITCH] - aero[ROLL] - aero[YAW] + motor0_bias;
- pwm1 = aero[THROTTLE] + aero[PITCH] - aero[ROLL] + aero[YAW] + motor1_bias;
- pwm2 = aero[THROTTLE] - aero[PITCH] + aero[ROLL] + aero[YAW] + motor2_bias;
- pwm3 = aero[THROTTLE] + aero[PITCH] + aero[ROLL] - aero[YAW] + motor3_bias;
-
-// printf("pwm0: %d\tpwm1: %d\tpwm2: %d\tpwm3: %d\n", pwm0, pwm1, pwm2, pwm3);
-
- /**
- * Boundary checks:
- *
- * #define min 100000
- * #define max 200000
- */
-
- if(pwm0 < min)
- pwm0 = min;
- else if(pwm0 > max)
- pwm0 = max;
-
- if(pwm1 < min)
- pwm1 = min;
- else if(pwm1 > max)
- pwm1 = max;
-
- if(pwm2 < min)
- pwm2 = min;
- else if(pwm2 > max)
- pwm2 = max;
-
- if(pwm3 < min)
- pwm3 = min;
- else if(pwm3 > max)
- pwm3 = max;
-
- PWMs[0] = pwm0;
- PWMs[1] = pwm1;
- PWMs[2] = pwm2;
- PWMs[3] = pwm3;
-
- // the array PWMs is then written directly to the PWM hardware registers
- // the PWMs are in units of clock cycles, not percentage duty cycle
- // use pwm/222,222 to get the duty cycle. the freq is 450 Hz on a 100MHz clock
-}
-
-/**
- * Converts Aero 4 channel signals to PWM signals
- * Aero channels are defined above
- *
- * *deprecated
- */
-void old_Aero_to_PWMS(int* PWMs, int* aero) {
-
- int motor0_bias = -9900, motor1_bias = -200, motor2_bias = -10200, motor3_bias = 250;
-// int motor0_bias = -5000, motor1_bias = 0, motor2_bias = -5000, motor3_bias = 0;
-
- // Throttle, pitch, roll, yaw as a percentage of their max - Range 0.0 - 100.0
- float throttle_100 = (aero[THROTTLE] - THROTTLE_MIN) / (THROTTLE_RANGE*1.0);
- float pitch_100 = (aero[PITCH] - PITCH_MIN) / (PITCH_RANGE*1.0);
- float roll_100 = (aero[ROLL] - ROLL_MIN) / (ROLL_RANGE*1.0);
- float yaw_100 = (aero[YAW] - YAW_MIN) / (YAW_RANGE*1.0);
-
- // This adds a +/- 300 ms range bias for the throttle
- int throttle_base = BASE + (int) 60000 * (throttle_100 - .5);
- // This adds a +/- 200 ms range bias for the pitch
- int pitch_base = (int) 60000 * (pitch_100 - .5);
- // This adds a +/- 200 ms range bias for the roll
- int roll_base = (int) 60000 * (roll_100 - .5);
- // This adds a +/- 75 ms range bias for the yaw
- int yaw_base = (int) 15000 * (yaw_100 - .5);
-
- int pwm0, pwm1, pwm2, pwm3;
-
- pwm1 = throttle_base + pitch_base/2 - roll_base/2 + yaw_base + motor1_bias;
- pwm3 = throttle_base + pitch_base/2 + roll_base/2 - yaw_base + motor3_bias;
- pwm0 = throttle_base - pitch_base/2 - roll_base/2 - yaw_base + motor0_bias;
- pwm2 = throttle_base - pitch_base/2 + roll_base/2 + yaw_base + motor2_bias;
-
- /**
- * Boundary checks:
- *
- * #define min 100000
- * #define max 200000
- */
-
- if(pwm0 < min)
- pwm0 = min;
- else if(pwm0 > max)
- pwm0 = max;
-
- if(pwm1 < min)
- pwm1 = min;
- else if(pwm1 > max)
- pwm1 = max;
-
- if(pwm2 < min)
- pwm2 = min;
- else if(pwm2 > max)
- pwm2 = max;
-
- if(pwm3 < min)
- pwm3 = min;
- else if(pwm3 > max)
- pwm3 = max;
-
- PWMs[0] = pwm0;
- PWMs[1] = pwm1;
- PWMs[2] = pwm2;
- PWMs[3] = pwm3;
-
- // the array PWMs is then written directly to the PWM hardware registers
- // the PWMs are in units of clock cycles, not percentage duty cycle
- // use pwm/222,222 to get the duty cycle. the freq is 450 Hz on a 100MHz clock
-
-}
diff --git a/quad/src/quad_app/actuator_command_processing.h b/quad/src/quad_app/actuator_command_processing.h
deleted file mode 100644
index 12110d5513130b90b4b60f30ffe555bcb370a07e..0000000000000000000000000000000000000000
--- a/quad/src/quad_app/actuator_command_processing.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*
- * actuator_command_processing.h
- *
- * Created on: Feb 20, 2016
- * Author: ucart
- */
-
-#ifndef ACTUATOR_COMMAND_PROCESSING_H_
-#define ACTUATOR_COMMAND_PROCESSING_H_
-
-#include <stdio.h>
-
-#include "log_data.h"
-#include "control_algorithm.h"
-
-/**
- * @brief
- * Processes the commands to the actuators.
- *
- * @param log_struct
- * structure of the data to be logged
- *
- * @param raw_actuator_struct
- * structure of the commmands outputted to go to the actuators
- *
- * @param actuator_command_struct
- * structure of the commmands to go to the actuators
- *
- * @return
- * error message
- *
- */
-int actuator_command_processing(log_t* log_struct, user_input_t * user_input_struct, raw_actuator_t* raw_actuator_struct, actuator_command_t* actuator_command_struct);
-
-void old_Aero_to_PWMS(int* PWMs, int* aero);
-void Aero_to_PWMS(int* PWMs, int* aero);
-
-#endif /* ACTUATOR_COMMAND_PROCESSING_H_ */
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index dd7198c44929bf58845569e22457fbfdcae92498..845c584334bdd8b83083ed1215398fb2ca7e0237 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -23,8 +23,8 @@ void connect_autonomous(parameter_t* ps) {
struct computation_graph* graph = ps->graph;
//graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->x_pos_pid, PID_CORRECTION);
//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->x_vel_pid, PID_CORRECTION);
- graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->y_vel_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->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->yaw_pid, PID_CORRECTION);
}
@@ -96,6 +96,8 @@ int control_algorithm_init(parameter_t * ps)
ps->y_vel = graph_add_defined_block(graph, BLOCK_PID, "Y Vel");
ps->x_vel_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "X Vel Clamp");
ps->y_vel_clamp = graph_add_defined_block(graph, BLOCK_BOUNDS, "Y vel Clamp");
+ // Converts global X/Y to local X/Y
+ ps->yaw_correction = graph_add_defined_block(graph, BLOCK_YAW_ROT, "Yaw Correction");
ps->mixer = graph_add_defined_block(graph, BLOCK_MIXER, "Signal Mixer");
@@ -143,6 +145,12 @@ int control_algorithm_init(parameter_t * ps)
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
+ graph_set_source(graph, ps->yaw_correction, ROT_YAW, ps->cur_yaw, CONST_VAL);
+ 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
graph_set_param_val(graph, ps->y_vel, PID_KD, -1);
@@ -368,22 +376,22 @@ int control_algorithm_init(parameter_t * ps)
// don't use the PID corrections if the throttle is less than about 10% of its range
// unless we are in autonomous
- if((user_input_struct->rc_commands[THROTTLE] > 118000) ||
+ if((user_input_struct->rc_commands[THROTTLE] > 0.18000) ||
user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)
{
//THROTTLE
- actuator_struct->pwms[0] = graph_get_output(graph, ps->mixer, MIXER_PWM0);
- actuator_struct->pwms[1] = graph_get_output(graph, ps->mixer, MIXER_PWM1);
- actuator_struct->pwms[2] = graph_get_output(graph, ps->mixer, MIXER_PWM2);
- actuator_struct->pwms[3] = graph_get_output(graph, ps->mixer, MIXER_PWM3);
+ actuator_struct->motor_magnitudes[0] = graph_get_output(graph, ps->mixer, MIXER_MOTOR0);
+ actuator_struct->motor_magnitudes[1] = graph_get_output(graph, ps->mixer, MIXER_MOTOR1);
+ actuator_struct->motor_magnitudes[2] = graph_get_output(graph, ps->mixer, MIXER_MOTOR2);
+ actuator_struct->motor_magnitudes[3] = graph_get_output(graph, ps->mixer, MIXER_MOTOR3);
}
else
{
- actuator_struct->pwms[0] = user_input_struct->rc_commands[THROTTLE];
- actuator_struct->pwms[1] = user_input_struct->rc_commands[THROTTLE];
- actuator_struct->pwms[2] = user_input_struct->rc_commands[THROTTLE];
- actuator_struct->pwms[3] = user_input_struct->rc_commands[THROTTLE];
+ actuator_struct->motor_magnitudes[0] = user_input_struct->rc_commands[THROTTLE];
+ actuator_struct->motor_magnitudes[1] = user_input_struct->rc_commands[THROTTLE];
+ actuator_struct->motor_magnitudes[2] = user_input_struct->rc_commands[THROTTLE];
+ actuator_struct->motor_magnitudes[3] = user_input_struct->rc_commands[THROTTLE];
}
last_fm_switch = cur_fm_switch;
diff --git a/quad/src/quad_app/controllers.h b/quad/src/quad_app/controllers.h
index 1e12c7590e5fe661b81a1bca31b7398da3e457d3..40c7832601fd0509a002cb2f5c3c29fa80c304ff 100644
--- a/quad/src/quad_app/controllers.h
+++ b/quad/src/quad_app/controllers.h
@@ -71,39 +71,41 @@
/**
* Signals from the Rx mins, maxes and ranges
*/
-#define THROTTLE_MAX 191900
-#define THROTTLE_MIN 110200
+#define THROTTLE_MAX 0.91900
+#define THROTTLE_MIN 0.10200
#define THROTTLE_RANGE THROTTLE_MAX - THROTTLE_MIN
-#define ROLL_MAX 170200
-#define ROLL_MIN 129400
-#define ROLL_CENTER 149800
+#define ROLL_MAX 0.70200
+#define ROLL_MIN 0.29400
+#define ROLL_CENTER 0.49800
#define ROLL_RANGE ROLL_MAX - ROLL_MIN
-#define PITCH_MAX 169900
-#define PITCH_MIN 129500
-#define PITCH_CENTER 149700
+#define PITCH_MAX 0.69900
+#define PITCH_MIN 0.29500
+#define PITCH_CENTER 0.49700
#define PITCH_RANGE PITCH_MAX - PITCH_MIN
-#define YAW_MAX 169400
-#define YAW_MIN 129300
-#define YAW_CENTER 149800
+#define YAW_MAX 0.69400
+#define YAW_MIN 0.29300
+#define YAW_CENTER 0.49800
#define YAW_RANGE YAW_MAX - YAW_MIN
-#define GEAR_1 170800
-#define GEAR_0 118300
+#define GEAR_1 0.70800
+#define GEAR_0 0.18300
+#define GEAR_MID (GEAR_0 + GEAR_1)/2.0
-#define FLAP_1 192000
-#define FLAP_0 107600
+#define FLAP_1 0.92000
+#define FLAP_0 0.07600
+#define FLAP_MID (FLAP_0 + FLAP_1)/2.0
#define GEAR_KILL GEAR_0 // The kill point for the program
-#define BASE 150000
+#define BASE 0.50000
-#define min 100000
-#define max 200000
+#define min 0.00000
+#define max 1.00000
-#define MOTOR_MIN 100000
-#define MOTOR_MAX 200000
+#define MOTOR_MIN 0.00000
+#define MOTOR_MAX 1.00000
void filter_PWMs(int* mixer);
void PWMS_to_Aero(int* PWMs, int* aero); // <= javey: unused
diff --git a/quad/src/quad_app/hw_iface.h b/quad/src/quad_app/hw_iface.h
index 48b3d0cf7f3cf26a0931c79e2b9c656dabc608ce..ca787b9fc6dde55ce2cddb69d8c77036aee41cd1 100644
--- a/quad/src/quad_app/hw_iface.h
+++ b/quad/src/quad_app/hw_iface.h
@@ -28,16 +28,16 @@ struct I2CDriver {
unsigned int length);
};
-struct PWMOutputDriver {
+struct RCReceiverDriver {
void *state;
- int (*reset)(struct PWMOutputDriver *self);
- int (*write)(struct PWMOutputDriver *self, unsigned int channel, unsigned long pulse_width_us);
+ int (*reset)(struct RCReceiverDriver *self);
+ int (*read)(struct RCReceiverDriver *self, unsigned int channel, float *magnitude);
};
-struct PWMInputDriver {
+struct MotorDriver {
void *state;
- int (*reset)(struct PWMInputDriver *self);
- int (*read)(struct PWMInputDriver *self, unsigned int channel, unsigned long *pulse_width_us);
+ int (*reset)(struct MotorDriver *self);
+ int (*write)(struct MotorDriver *self, unsigned int channel, float magnitude);
};
struct UARTDriver {
diff --git a/quad/src/quad_app/initialize_components.c b/quad/src/quad_app/initialize_components.c
index e476fd633eb9ecce40eb77af6570eac63462833a..5cd49342dcfffdaeb24649d18ca1555219200438 100644
--- a/quad/src/quad_app/initialize_components.c
+++ b/quad/src/quad_app/initialize_components.c
@@ -17,18 +17,18 @@ extern int Xil_AssertWait;
int protection_loops(modular_structs_t *structs)
{
- u32 rc_commands[6]; // 6 "receiver_input" elements: Throttle, Pitch, Roll, Yaw, Gear, and Flap
+ float rc_commands[6]; // 6 "receiver_input" elements: Throttle, Pitch, Roll, Yaw, Gear, and Flap
- struct PWMInputDriver *pwm_inputs = &structs->hardware_struct.pwm_inputs;
- read_rec_all(pwm_inputs, rc_commands);
+ struct RCReceiverDriver *rc_receiver = &structs->hardware_struct.rc_receiver;
+ read_rec_all(rc_receiver, rc_commands);
- while(rc_commands[THROTTLE] < 75000 || // wait for RC receiver to connect to transmitter
- rc_commands[THROTTLE] > 125000 || // Wait until throttle is low
+ while(rc_commands[THROTTLE] < 0.075 || // wait for RC receiver to connect to transmitter
+ rc_commands[THROTTLE] > 0.125 || // Wait until throttle is low
read_kill(rc_commands[GEAR]) || // Wait until gear switch is engaged (so you don't immediately break out of the main loop below)
!read_flap(rc_commands[FLAP])) // Wait for the flight mode to be set to manual
{
process_received(structs);
- read_rec_all(pwm_inputs, rc_commands);
+ read_rec_all(rc_receiver, rc_commands);
}
// let the pilot/observers know that the quad is now active
@@ -77,15 +77,15 @@ int init_structs(modular_structs_t *structs) {
iic_set_globals(i2c, sys);
// Initialize PWM Recorders and Motor outputs
- struct PWMInputDriver *pwm_inputs = &structs->hardware_struct.pwm_inputs;
- if (pwm_inputs->reset(pwm_inputs)) return -1;
- struct PWMOutputDriver *pwm_outputs = &structs->hardware_struct.pwm_outputs;
- if (pwm_outputs->reset(pwm_outputs)) return -1;
+ struct RCReceiverDriver *rc_receiver = &structs->hardware_struct.rc_receiver;
+ if (rc_receiver->reset(rc_receiver)) return -1;
+ struct MotorDriver *motors = &structs->hardware_struct.motors;
+ if (motors->reset(motors)) return -1;
// Set motor outputs to off
int i;
for (i = 0; i < 4; i += 1) {
- pwm_outputs->write(pwm_outputs, i, MOTOR_MIN);
+ motors->write(motors, i, MOTOR_MIN);
}
// Initialize sensors
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index 5714d32a2e3c41e2ca1dbd7db5bf2217a190132a..bdc38f0eeb05714f73d334ea492ee8c6311abf21 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -117,10 +117,10 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
addOutputToLog(log_struct, ps->x_set, CONST_VAL, m);
addOutputToLog(log_struct, ps->y_set, CONST_VAL, m);
addOutputToLog(log_struct, ps->alt_set, CONST_VAL, m);
- addOutputToLog(log_struct, ps->mixer, MIXER_PWM0, pwm_val);
- addOutputToLog(log_struct, ps->mixer, MIXER_PWM1, pwm_val);
- addOutputToLog(log_struct, ps->mixer, MIXER_PWM2, pwm_val);
- addOutputToLog(log_struct, ps->mixer, MIXER_PWM3, pwm_val);
+ addOutputToLog(log_struct, ps->mixer, MIXER_MOTOR0, pwm_val);
+ addOutputToLog(log_struct, ps->mixer, MIXER_MOTOR1, pwm_val);
+ addOutputToLog(log_struct, ps->mixer, MIXER_MOTOR2, pwm_val);
+ addOutputToLog(log_struct, ps->mixer, MIXER_MOTOR3, pwm_val);
addOutputToLog(log_struct, ps->rc_throttle, PID_CORRECTION, pwm_val);
addOutputToLog(log_struct, ps->rc_pitch, PID_CORRECTION, pwm_val);
addOutputToLog(log_struct, ps->rc_roll, PID_CORRECTION, pwm_val);
diff --git a/quad/src/quad_app/quad_app.c b/quad/src/quad_app/quad_app.c
index 01eabe9017f733685ae8f9737398099e657a0bf0..f83cc8698f58c41a42ae255808f1c749b442bdbb 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/quad/src/quad_app/quad_app.c
@@ -12,7 +12,6 @@
#include "sensor.h"
#include "sensor_processing.h"
#include "control_algorithm.h"
-#include "actuator_command_processing.h"
#include "send_actuator_commands.h"
#include "update_gui.h"
#include "communication.h"
@@ -67,10 +66,10 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
&(structs.parameter_struct), &(structs.user_defined_struct), &(structs.actuator_command_struct), &structs);
// send the actuator commands
- send_actuator_commands(&(structs.hardware_struct.pwm_outputs), &(structs.log_struct), &(structs.actuator_command_struct));
+ send_actuator_commands(&(structs.hardware_struct.motors), &(structs.log_struct), &(structs.actuator_command_struct));
}
else {
- kill_motors(&(structs.hardware_struct.pwm_outputs));
+ kill_motors(&(structs.hardware_struct.motors));
}
// update the GUI
update_GUI(&(structs.log_struct));
@@ -115,7 +114,7 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
timer_end_loop(&(structs.log_struct));
}
- kill_motors(&(structs.hardware_struct.pwm_outputs));
+ kill_motors(&(structs.hardware_struct.motors));
flash_MIO_7_led(10, 100);
diff --git a/quad/src/quad_app/send_actuator_commands.c b/quad/src/quad_app/send_actuator_commands.c
index c361e74db2146e44b80881fec18dde14efef44d3..8120224d0dd07f72cf0382ab5f11a86a352851c6 100644
--- a/quad/src/quad_app/send_actuator_commands.c
+++ b/quad/src/quad_app/send_actuator_commands.c
@@ -8,12 +8,12 @@
#include "send_actuator_commands.h"
#include "util.h"
-int send_actuator_commands(struct PWMOutputDriver *pwm_outputs, log_t* log_struct, actuator_command_t* actuator_command_struct) {
+int send_actuator_commands(struct MotorDriver *motors, log_t* log_struct, actuator_command_t* actuator_command_struct) {
int i;
// write the PWMs to the motors
for (i = 0; i < 4; i++) {
- pwm_outputs->write(pwm_outputs, i, actuator_command_struct->pwms[i]);
+ motors->write(motors, i, actuator_command_struct->motor_magnitudes[i]);
}
return 0;
diff --git a/quad/src/quad_app/send_actuator_commands.h b/quad/src/quad_app/send_actuator_commands.h
index 3f6fb0fc833e3dc87d291ca6a357eed115d17e94..747ba0479ef15d08ed18e1a8c4f8c73694f893e6 100644
--- a/quad/src/quad_app/send_actuator_commands.h
+++ b/quad/src/quad_app/send_actuator_commands.h
@@ -11,7 +11,6 @@
#include <stdio.h>
#include "log_data.h"
-#include "actuator_command_processing.h"
/**
* @brief
@@ -27,6 +26,6 @@
* error message
*
*/
-int send_actuator_commands(struct PWMOutputDriver *pwm_outputs, log_t* log_struct, actuator_command_t* actuator_command_struct);
+int send_actuator_commands(struct MotorDriver *motors, log_t* log_struct, actuator_command_t* actuator_command_struct);
#endif /* SEND_ACTUATOR_COMMANDS_H_ */
diff --git a/quad/src/quad_app/sensor.c b/quad/src/quad_app/sensor.c
index 21ee86f3c31246439262a0f20a49b232b96d3ca5..b2f7592c88f667ca9ed9f897ea65dd4b22293ecc 100644
--- a/quad/src/quad_app/sensor.c
+++ b/quad/src/quad_app/sensor.c
@@ -12,9 +12,9 @@
int sensor_init(raw_sensor_t * raw_sensor_struct, sensor_t * sensor_struct)
{
- if (iic0_lidarlite_init()) { // init LiDAR
- return -1;
- }
+// if (iic0_lidarlite_init()) { // init LiDAR
+// return -1;
+// }
if(iic0_mpu9150_start() == -1) {
return -1;
@@ -67,15 +67,16 @@ int get_sensors(log_t* log_struct, user_input_t* user_input_struct, raw_sensor_t
//
// /////////// end testing
+ int status = 0;
// the the sensor board and fill in the readings into the GAM struct
iic0_mpu9150_read_gam(&(raw_sensor_struct->gam));
log_struct->gam = raw_sensor_struct->gam;
- static lidar_t lidar_val;
- iic0_lidarlite_read_distance(&lidar_val);
- raw_sensor_struct->lidar_distance_m = lidar_val.distance_m;
+// static lidar_t lidar_val;
+// iic0_lidarlite_read_distance(&lidar_val);
+// raw_sensor_struct->lidar_distance_m = lidar_val.distance_m;
return 0;
}
diff --git a/quad/src/quad_app/sensor_processing.c b/quad/src/quad_app/sensor_processing.c
index 5d8a714e09db5b17a98ca9e663404435c51b4e2c..5e3ca9f2ff4484625da6df72009e77ce44ade924 100644
--- a/quad/src/quad_app/sensor_processing.c
+++ b/quad/src/quad_app/sensor_processing.c
@@ -44,28 +44,6 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
// copy currentQuadPosition and trimmedRCValues from raw_sensor_struct to sensor_struct
deep_copy_Qpos(&(sensor_struct->currentQuadPosition), &(raw_sensor_struct->currentQuadPosition));
- // Psuedo-Nonlinear Extension for determining local x/y position based on yaw angle
- // provided by Matt Rich
- //
- // local x/y/z is the moving frame of reference on the quad that we are transforming so we can assume yaw angle is 0 (well enough)
- // for the autonomous position controllers
- //
- // camera given x/y/z is the inertia frame of reference (the global coordinates)
- //
- // |local x| |cos(yaw angle) -sin(yaw angle) 0| |camera given x|
- // |local y| = |sin(yaw angle) cos(yaw angle) 0| |camera given y|
- // |local z| | 0 0 1| |camera given z|
-
-
- sensor_struct->currentQuadPosition.x_pos =
- raw_sensor_struct->currentQuadPosition.x_pos * cos(raw_sensor_struct->currentQuadPosition.yaw) +
- raw_sensor_struct->currentQuadPosition.y_pos * -sin(raw_sensor_struct->currentQuadPosition.yaw);
-
- sensor_struct->currentQuadPosition.y_pos =
- raw_sensor_struct->currentQuadPosition.x_pos * sin(raw_sensor_struct->currentQuadPosition.yaw) +
- raw_sensor_struct->currentQuadPosition.y_pos * cos(raw_sensor_struct->currentQuadPosition.yaw);
-
-
// Calculate Euler angles and velocities using Gimbal Equations below
/////////////////////////////////////////////////////////////////////////
// | Phi_d | | 1 sin(Phi)tan(theta) cos(Phi)tan(theta) | | p |
diff --git a/quad/src/quad_app/sensor_processing.h b/quad/src/quad_app/sensor_processing.h
index 52b626eafa173ac5c806ad29d52ca03138017060..d340e34583c2a4e77fe7cad13e256c4f02eea955 100644
--- a/quad/src/quad_app/sensor_processing.h
+++ b/quad/src/quad_app/sensor_processing.h
@@ -19,45 +19,6 @@
#define GEAR 4
#define FLAP 5
-/**
- * Signals from the Rx mins, maxes and ranges
- */
-//#define THROTTLE_MAX 191900
-//#define THROTTLE_MIN 110200
-//#define THROTTLE_RANGE THROTTLE_MAX - THROTTLE_MIN
-//
-//#define ROLL_MAX 170200
-////#define ROLL_MAX 167664
-////#define ROLL_CENTER 148532
-//#define ROLL_MIN 129400
-//#define ROLL_CENTER 149800
-//#define ROLL_RANGE ROLL_MAX - ROLL_MIN
-//
-////#define PITCH_MAX 190800
-//#define PITCH_MAX 169900
-////#define PITCH_MIN 135760
-//#define PITCH_MIN 129500
-////#define PITCH_CENTER 152830
-//#define PITCH_CENTER 149700
-//#define PITCH_RANGE PITCH_MAX - PITCH_MIN
-//
-//#define YAW_MAX 169400
-//#define YAW_MIN 129300
-//#define YAW_CENTER 149800
-//#define YAW_RANGE YAW_MAX - YAW_MIN
-//
-//#define GEAR_1 170800
-//#define GEAR_0 118300
-//
-//#define FLAP_1 192000
-//#define FLAP_0 107600
-//
-//#define GEAR_KILL GEAR_0 // The kill point for the program
-//#define BASE 150000
-//
-//#define min 100000
-//#define max 200000
-
/**
* @brief
* Processes the data from the sensors.
diff --git a/quad/src/quad_app/type_def.h b/quad/src/quad_app/type_def.h
index 80e676956b648a9cb9bd6e528ca96dc147d25c3a..e2e40c1aac046af72fe499ef9b1f7f583bf27be5 100644
--- a/quad/src/quad_app/type_def.h
+++ b/quad/src/quad_app/type_def.h
@@ -148,7 +148,7 @@ typedef struct PID_values{
*
*/
typedef struct user_input_t {
- u32 rc_commands[6]; // Commands from the RC transmitter
+ float rc_commands[6]; // Commands from the RC transmitter
// float cam_x_pos; // Current x position from the camera system
@@ -369,6 +369,8 @@ typedef struct parameter_t {
int y_vel_clamp;
int vel_x_gain;
int vel_y_gain;
+ // Sensor processing
+ int yaw_correction;
} parameter_t;
/**
@@ -388,7 +390,7 @@ typedef struct user_defined_t {
*/
typedef struct raw_actuator_t {
- int controller_corrected_motor_commands[6];
+ float controller_corrected_motor_commands[6];
} raw_actuator_t;
@@ -398,7 +400,7 @@ typedef struct raw_actuator_t {
*
*/
typedef struct actuator_command_t {
- int pwms[4];
+ float motor_magnitudes[4];
} actuator_command_t;
enum PWMChannels {
@@ -416,8 +418,8 @@ enum PWMChannels {
typedef struct hardware_t {
struct I2CDriver i2c;
- struct PWMInputDriver pwm_inputs;
- struct PWMOutputDriver pwm_outputs;
+ struct RCReceiverDriver rc_receiver;
+ struct MotorDriver motors;
struct UARTDriver uart;
struct TimerDriver global_timer;
struct TimerDriver axi_timer;
diff --git a/quad/src/quad_app/user_input.c b/quad/src/quad_app/user_input.c
index bcb7478bf3ce6b7d9b5e6457e82bdafdce02d915..824910a1b95842ff9947558ee86270e832abd100 100644
--- a/quad/src/quad_app/user_input.c
+++ b/quad/src/quad_app/user_input.c
@@ -10,8 +10,8 @@
int get_user_input(hardware_t *hardware_struct, log_t* log_struct, user_input_t* user_input_struct)
{
// Read in values from RC Receiver
- struct PWMInputDriver *pwm_inputs = &hardware_struct->pwm_inputs;
- read_rec_all(pwm_inputs, user_input_struct->rc_commands);
+ struct RCReceiverDriver *rc_receiver = &hardware_struct->rc_receiver;
+ read_rec_all(rc_receiver, user_input_struct->rc_commands);
//create setpoints for manual flight
// currently in units of radians
@@ -44,10 +44,10 @@ int kill_condition(user_input_t* user_input_struct)
* -
*
*/
-float convert_from_receiver_cmd(int receiver_cmd, int max_receiver_cmd, int center_receiver_cmd, int min_receiver_cmd, float max_target, float min_target)
+float convert_from_receiver_cmd(float receiver_cmd, float max_receiver_cmd, float center_receiver_cmd, float min_receiver_cmd, float max_target, float min_target)
{
// centers the receiver command by subtracting the given center value. This means that if receiver_cmd == center then receiver_cmd_centered should be 0.
- int receiver_cmd_centered = receiver_cmd - center_receiver_cmd;
+ float receiver_cmd_centered = receiver_cmd - center_receiver_cmd;
if(receiver_cmd_centered <= 0) {
float ret = ((float)(receiver_cmd_centered * min_target)) / ((float) (min_receiver_cmd - center_receiver_cmd));
diff --git a/quad/src/quad_app/user_input.h b/quad/src/quad_app/user_input.h
index c289e99c96e437dcfed2398a6711bcf9ecadb786..674f66e6ea2fdf820f7272ee520c4d84a34d0515 100644
--- a/quad/src/quad_app/user_input.h
+++ b/quad/src/quad_app/user_input.h
@@ -35,36 +35,6 @@
#define PITCH_DEG_TARGET 12.0f
#define PITCH_RAD_TARGET ((float) ((PITCH_DEG_TARGET * 3.141592) / ((float) 180)))
-/////// Signals from the Rx mins, maxes and ranges
-
-//#define THROTTLE_MAX 191900
-//#define THROTTLE_MIN 110200
-//#define THROTTLE_RANGE THROTTLE_MAX - THROTTLE_MIN
-//
-//#define ROLL_MAX 170200
-//#define ROLL_MIN 129400
-//#define ROLL_CENTER 149800
-//#define ROLL_RANGE ROLL_MAX - ROLL_MIN
-//
-//#define PITCH_MAX 169900
-//#define PITCH_MIN 129500
-//#define PITCH_CENTER 149700
-//#define PITCH_RANGE PITCH_MAX - PITCH_MIN
-//
-//#define YAW_MAX 169400
-//#define YAW_MIN 129300
-//#define YAW_CENTER (YAW_MIN + YAW_MAX)/2 //149800
-//#define YAW_RANGE YAW_MAX - YAW_MIN
-//
-//#define GEAR_1 170800
-//#define GEAR_0 118300
-//
-//#define FLAP_1 192000
-//#define FLAP_0 107600
-//
-//#define GEAR_KILL GEAR_0 // The kill point for the program
-//#define BASE 150000
-
/**
* @brief
* Receives user input to the system.
@@ -81,7 +51,7 @@
*/
int get_user_input(hardware_t *hardware_struct, log_t* log_struct, user_input_t* user_input_struct);
int kill_condition(user_input_t* user_input_struct);
-float convert_from_receiver_cmd(int receiver_cmd, int max_receiver_cmd, int center_receiver_cmd, int min_receiver_cmd, float max_target, float min_target);
+float convert_from_receiver_cmd(float receiver_cmd, float max_receiver_cmd, float center_receiver_cmd, float min_receiver_cmd, float max_target, float min_target);
#endif /* USER_INPUT_H_ */
diff --git a/quad/src/quad_app/util.c b/quad/src/quad_app/util.c
index 2f1512a0e407385648f442083b2e5152b3fa8dac..e982432eafb4b624990b3da5b95846cc672abc50 100644
--- a/quad/src/quad_app/util.c
+++ b/quad/src/quad_app/util.c
@@ -13,10 +13,10 @@ extern int motor0_bias, motor1_bias, motor2_bias, motor3_bias;
/**
* Reads all 6 receiver channels at once
*/
-void read_rec_all(struct PWMInputDriver *pwm_input, u32 *mixer){
+void read_rec_all(struct RCReceiverDriver *rc_receiver, float *mixer){
int i;
for(i = 0; i < 6; i++){
- pwm_input->read(pwm_input, i, &mixer[i]);
+ rc_receiver->read(rc_receiver, i, &mixer[i]);
}
}
@@ -48,30 +48,37 @@ int hexStrToInt(char *buf, int startIdx, int endIdx) {
* If the message from the receiver is 0 - gear, kill the system by sending a 1
* Otherwise, do nothing
*/
-int read_kill(int gear){
- if(gear > 115000 && gear < 125000)
+int read_kill(float gear) {
+ if(gear < GEAR_MID)
return 1;
return 0;
}
-int read_flap(int flap)
-{
- // flap '0' is 108,000 CC (Up)
- // flap '1' is 192,000 CC (Down)
- // here we say if the reading is greater than 150,000 than its '1'; '0' otherwise
- if(flap > 150000)
+int read_flap(float flap) {
+ if(flap > FLAP_MID)
return 1;
return 0;
}
+/**
+ * Return true if the transmitter is on. This is mostly a failsafe to be sure we don't
+ * drop out of the air if the receiver cuts out momentarily. If the receiver
+ * cuts out, we expect the rc values will flatline at 0. So if we get a float zero,
+ * then assume the transmitter is disconnected.
+ */
+int is_transmitter_on(float gear) {
+ if (gear < 0.00001) return 0;
+ else return 1;
+}
+
/**
* Turns off the motors
*/
-void kill_motors(struct PWMOutputDriver *pwm_outputs) {
- pwm_outputs->write(pwm_outputs, 0, MOTOR_MIN);
- pwm_outputs->write(pwm_outputs, 1, MOTOR_MIN);
- pwm_outputs->write(pwm_outputs, 2, MOTOR_MIN);
- pwm_outputs->write(pwm_outputs, 3, MOTOR_MIN);
+void kill_motors(struct MotorDriver *motors) {
+ motors->write(motors, 0, MOTOR_MIN);
+ motors->write(motors, 1, MOTOR_MIN);
+ motors->write(motors, 2, MOTOR_MIN);
+ motors->write(motors, 3, MOTOR_MIN);
}
int build_int(u8 *buff) {
diff --git a/quad/src/quad_app/util.h b/quad/src/quad_app/util.h
index 643498afc53753720c8f4295fc88918ad5eae4db..390bafd9d5121af93ebef8e0a16c1340bea1fead 100644
--- a/quad/src/quad_app/util.h
+++ b/quad/src/quad_app/util.h
@@ -11,10 +11,10 @@
#include "controllers.h"
#include "hw_iface.h"
-void read_rec_all(struct PWMInputDriver *pwm_input, u32 *mixer);
-int read_kill(int gear);
-int read_flap(int flap);
-void kill_motors(struct PWMOutputDriver *pwm_outputs);
+void read_rec_all(struct RCReceiverDriver *rc_receiver, float *mixer);
+int read_kill(float gear);
+int read_flap(float flap);
+void kill_motors(struct MotorDriver *motors);
int build_int(u8 *buff);
float build_float(u8 *buff);
diff --git a/quad/src/virt_quad/hw_impl_unix.c b/quad/src/virt_quad/hw_impl_unix.c
index fea231c35e963f5321b35665e4c3c2feac961383..bbd8e8775df179f41c36e3f96152f634bf1e0d97 100644
--- a/quad/src/virt_quad/hw_impl_unix.c
+++ b/quad/src/virt_quad/hw_impl_unix.c
@@ -9,20 +9,20 @@ struct UARTDriver create_unix_uart() {
return uart;
}
-struct PWMOutputDriver create_unix_pwm_outputs() {
- struct PWMOutputDriver pwm_outputs;
- pwm_outputs.state = NULL;
- pwm_outputs.reset = unix_pwm_output_reset;
- pwm_outputs.write = unix_pwm_output_write;
- return pwm_outputs;
+struct MotorDriver create_unix_motors() {
+ struct MotorDriver motors;
+ motors.state = NULL;
+ motors.reset = unix_motor_reset;
+ motors.write = unix_motor_write;
+ return motors;
}
-struct PWMInputDriver create_unix_pwm_inputs() {
- struct PWMInputDriver pwm_inputs;
- pwm_inputs.state = NULL;
- pwm_inputs.reset = unix_pwm_input_reset;
- pwm_inputs.read = unix_pwm_input_read;
- return pwm_inputs;
+struct RCReceiverDriver create_unix_rc_receiver() {
+ struct RCReceiverDriver rc_receivers;
+ rc_receivers.state = NULL;
+ rc_receivers.reset = unix_rc_receiver_reset;
+ rc_receivers.read = unix_rc_receiver_read;
+ return rc_receivers;
}
struct I2CDriver create_unix_i2c() {
diff --git a/quad/src/virt_quad/hw_impl_unix.h b/quad/src/virt_quad/hw_impl_unix.h
index 38745899342bb3a8c81734bead570d1d876f4cff..85e223f2896a317fd5f1c1fa79f6fc60cb7a8772 100644
--- a/quad/src/virt_quad/hw_impl_unix.h
+++ b/quad/src/virt_quad/hw_impl_unix.h
@@ -16,11 +16,11 @@ int unix_uart_reset(struct UARTDriver *self);
int unix_uart_write(struct UARTDriver *self, unsigned char c);
int unix_uart_read(struct UARTDriver *self, unsigned char *c);
-int unix_pwm_output_reset(struct PWMOutputDriver *self);
-int unix_pwm_output_write(struct PWMOutputDriver *self, unsigned int channel, unsigned long pulse_width_us);
+int unix_motor_reset(struct MotorDriver *self);
+int unix_motor_write(struct MotorDriver *self, unsigned int channel, float magnitude);
-int unix_pwm_input_reset(struct PWMInputDriver *self);
-int unix_pwm_input_read(struct PWMInputDriver *self, unsigned int channel, unsigned long *pulse_width_us);
+int unix_rc_receiver_reset(struct RCReceiverDriver *self);
+int unix_rc_receiver_read(struct RCReceiverDriver *self, unsigned int channel, float *magnitude);
int unix_i2c_reset(struct I2CDriver *self);
int unix_i2c_write(struct I2CDriver *self,
@@ -48,8 +48,8 @@ int unix_system_reset(struct SystemDriver *self);
int unix_system_sleep(struct SystemDriver *self, unsigned long us);
struct UARTDriver create_unix_uart();
-struct PWMOutputDriver create_unix_pwm_outputs();
-struct PWMInputDriver create_unix_pwm_inputs();
+struct MotorDriver create_unix_motors();
+struct RCReceiverDriver create_unix_rc_receiver();
struct I2CDriver create_unix_i2c();
struct TimerDriver create_unix_global_timer();
struct TimerDriver create_unix_axi_timer();
@@ -58,6 +58,6 @@ struct SystemDriver create_unix_system();
int test_unix_i2c();
int test_unix_mio7_led_and_system();
-int test_unix_pwm_inputs();
+int test_unix_rc_receivers();
#endif
diff --git a/quad/src/virt_quad/hw_impl_unix_pwm_output.c b/quad/src/virt_quad/hw_impl_unix_motor.c
similarity index 62%
rename from quad/src/virt_quad/hw_impl_unix_pwm_output.c
rename to quad/src/virt_quad/hw_impl_unix_motor.c
index e58d5cf5ae496a5fb6d6d4d76332d78097f98a9f..3ad271b526aff0b45047988280157e1aa5e12b25 100644
--- a/quad/src/virt_quad/hw_impl_unix_pwm_output.c
+++ b/quad/src/virt_quad/hw_impl_unix_motor.c
@@ -9,19 +9,20 @@
void * output_cache();
static char *output_pwms[4];
-static unsigned long cache[4];
+static float cache[4];
pthread_t worker;
+static void float_equals(float a, float b);
static int zero = 0;
static int one = 1;
static int two = 2;
static int three = 3;
-int unix_pwm_output_reset(struct PWMOutputDriver *self) {
- output_pwms[0] = VIRT_QUAD_FIFOS_DIR "/pwm-output-motor1";
- output_pwms[1] = VIRT_QUAD_FIFOS_DIR "/pwm-output-motor2";
- output_pwms[2] = VIRT_QUAD_FIFOS_DIR "/pwm-output-motor3";
- output_pwms[3] = VIRT_QUAD_FIFOS_DIR "/pwm-output-motor4";
+int unix_motor_reset(struct MotorDriver *self) {
+ output_pwms[0] = VIRT_QUAD_FIFOS_DIR "/motor1";
+ output_pwms[1] = VIRT_QUAD_FIFOS_DIR "/motor2";
+ output_pwms[2] = VIRT_QUAD_FIFOS_DIR "/motor3";
+ output_pwms[3] = VIRT_QUAD_FIFOS_DIR "/motor4";
mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
@@ -34,13 +35,13 @@ int unix_pwm_output_reset(struct PWMOutputDriver *self) {
return 0;
}
-int unix_pwm_output_write(struct PWMOutputDriver *self,
+int unix_motor_write(struct MotorDriver *self,
unsigned int channel,
- unsigned long pulse_width_us) {
- if (cache[channel] != pulse_width_us) {
- printf("%s: %ld\n", output_pwms[channel], pulse_width_us);
+ float magnitude) {
+ if (cache[channel] != magnitude) {
+ printf("%s: %.2f\n", output_pwms[channel], magnitude);
}
- cache[channel] = pulse_width_us;
+ cache[channel] = magnitude;
return 0;
}
@@ -56,10 +57,14 @@ void * output_cache(void *arg) {
// Block while waiting for someone to listen
while (1) {
int fifo = open(output_pwms[i], O_WRONLY);
- sprintf(buff, "%ld\n", cache[i]);
+ sprintf(buff, "%.2f\n", cache[i]);
write(fifo, buff, strlen(buff));
close(fifo);
pthread_yield();
}
return NULL;
}
+
+static void float_equals(float a, float b) {
+ return fabs(a - b) < 0.00001;
+}
diff --git a/quad/src/virt_quad/hw_impl_unix_pwm_input.c b/quad/src/virt_quad/hw_impl_unix_rc_receiver.c
similarity index 65%
rename from quad/src/virt_quad/hw_impl_unix_pwm_input.c
rename to quad/src/virt_quad/hw_impl_unix_rc_receiver.c
index df1aaf5f9b43811a4af9b5ed73aebe5e0ca24a86..229a019cd041d0aaed0636c3805121868eb68835 100644
--- a/quad/src/virt_quad/hw_impl_unix_pwm_input.c
+++ b/quad/src/virt_quad/hw_impl_unix_rc_receiver.c
@@ -9,17 +9,17 @@ void * update_input_cache();
static char *input_names[6];
static int fifos[6];
-static unsigned long cache[6];
+static float cache[6];
static pthread_t workers[6];
static int nums[] = {0, 1, 2, 3, 4, 5};
-int unix_pwm_input_reset(struct PWMInputDriver *self) {
- input_names[0] = "pwm-input-throttle";
- input_names[1] = "pwm-input-roll";
- input_names[2] = "pwm-input-pitch";
- input_names[3] = "pwm-input-yaw";
- input_names[4] = "pwm-input-gear";
- input_names[5] = "pwm-input-flap";
+int unix_rc_receiver_reset(struct RCReceiverDriver *self) {
+ input_names[0] = "rc-throttle";
+ input_names[1] = "rc-roll";
+ input_names[2] = "rc-pitch";
+ input_names[3] = "rc-yaw";
+ input_names[4] = "rc-gear";
+ input_names[5] = "rc-flap";
mkdir(VIRT_QUAD_FIFOS_DIR, 0777);
@@ -38,17 +38,17 @@ int unix_pwm_input_reset(struct PWMInputDriver *self) {
cache[5] = FLAP_1;
for (i = 0; i < 6; i += 1) {
- printf("%s: %lu\n", input_names[i], cache[i]);
+ printf("%s: %.2f\n", input_names[i], cache[i]);
}
return 0;
}
-int unix_pwm_input_read(struct PWMInputDriver *self,
+int unix_rc_receiver_read(struct RCReceiverDriver *self,
unsigned int channel,
- unsigned long *pulse_width_us) {
+ float *magnitude) {
- *pulse_width_us = cache[channel];
+ *magnitude = cache[channel];
return 0;
}
@@ -69,13 +69,13 @@ void * update_input_cache(void *arg) {
int bytes_read = read(fifos[i], buff, 15);
if (bytes_read > 0) {
buff[bytes_read] = '\0';
- unsigned long val = strtoll(buff, NULL, 10);
- if (val < max && val > min) {
+ float val = strtof(buff, NULL);
+ if (val <= 1 && val >= 0) {
cache[i] = val;
- printf("%s: %lu\n", input_names[i], val);
+ printf("%s: %.2f\n", input_names[i], val);
}
else {
- printf("%s: Bad value - input not received\n", input_names[i]);
+ printf("%s: Bad value (%f) - input not received\n", input_names[i], val);
}
}
pthread_yield();
diff --git a/quad/src/virt_quad/main.c b/quad/src/virt_quad/main.c
index f703f6ab77943a760ce7db7a1604ec672edb4e7b..5f6aaf8dbc9fda7e1adfc1c21fe0ecafa8065e5b 100644
--- a/quad/src/virt_quad/main.c
+++ b/quad/src/virt_quad/main.c
@@ -6,8 +6,8 @@
int setup_hardware(hardware_t *hardware) {
hardware->i2c = create_unix_i2c();
- hardware->pwm_inputs = create_unix_pwm_inputs();
- hardware->pwm_outputs = create_unix_pwm_outputs();
+ hardware->rc_receiver = create_unix_rc_receiver();
+ hardware->motors = create_unix_motors();
hardware->uart = create_unix_uart();
hardware->global_timer = create_unix_global_timer();
hardware->axi_timer = create_unix_axi_timer();
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c
index e8fceffa661ca92e0e1581d291a6f2b07d635819..3a6348f9b9a8f8090b2769f17201aeda88f36b93 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.c
@@ -9,20 +9,20 @@ struct UARTDriver create_zybo_uart() {
return uart;
}
-struct PWMOutputDriver create_zybo_pwm_outputs() {
- struct PWMOutputDriver pwm_outputs;
- pwm_outputs.state = NULL;
- pwm_outputs.reset = zybo_pwm_output_reset;
- pwm_outputs.write = zybo_pwm_output_write;
- return pwm_outputs;
+struct MotorDriver create_zybo_motors() {
+ struct MotorDriver motors;
+ motors.state = NULL;
+ motors.reset = zybo_motor_reset;
+ motors.write = zybo_motor_write;
+ return motors;
}
-struct PWMInputDriver create_zybo_pwm_inputs() {
- struct PWMInputDriver pwm_inputs;
- pwm_inputs.state = NULL;
- pwm_inputs.reset = zybo_pwm_input_reset;
- pwm_inputs.read = zybo_pwm_input_read;
- return pwm_inputs;
+struct RCReceiverDriver create_zybo_rc_receiver() {
+ struct RCReceiverDriver rc_receiver;
+ rc_receiver.state = NULL;
+ rc_receiver.reset = zybo_rc_receiver_reset;
+ rc_receiver.read = zybo_rc_receiver_read;
+ return rc_receiver;
}
struct I2CDriver create_zybo_i2c() {
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h
index c721307bc08674e414683630564a2f7adf0dd2da..29d0ff8f388e0780f59aeefa63515ca0e8031522 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo.h
@@ -22,11 +22,11 @@ int zybo_uart_reset(struct UARTDriver *self);
int zybo_uart_write(struct UARTDriver *self, unsigned char c);
int zybo_uart_read(struct UARTDriver *self, unsigned char *c);
-int zybo_pwm_output_reset(struct PWMOutputDriver *self);
-int zybo_pwm_output_write(struct PWMOutputDriver *self, unsigned int channel, unsigned long pulse_width_us);
+int zybo_motor_reset(struct MotorDriver *self);
+int zybo_motor_write(struct MotorDriver *self, unsigned int channel, float magnitude);
-int zybo_pwm_input_reset(struct PWMInputDriver *self);
-int zybo_pwm_input_read(struct PWMInputDriver *self, unsigned int channel, unsigned long *pulse_width_us);
+int zybo_rc_receiver_reset(struct RCReceiverDriver *self);
+int zybo_rc_receiver_read(struct RCReceiverDriver *self, unsigned int channel, float *magnitude);
int zybo_i2c_reset(struct I2CDriver *self);
int zybo_i2c_write(struct I2CDriver *self,
@@ -54,8 +54,8 @@ int zybo_system_reset(struct SystemDriver *self);
int zybo_system_sleep(struct SystemDriver *self, unsigned long us);
struct UARTDriver create_zybo_uart();
-struct PWMOutputDriver create_zybo_pwm_outputs();
-struct PWMInputDriver create_zybo_pwm_inputs();
+struct MotorDriver create_zybo_motors();
+struct RCReceiverDriver create_zybo_rc_receiver();
struct I2CDriver create_zybo_i2c();
struct TimerDriver create_zybo_global_timer();
struct TimerDriver create_zybo_axi_timer();
@@ -64,6 +64,6 @@ struct SystemDriver create_zybo_system();
int test_zybo_i2c();
int test_zybo_mio7_led_and_system();
-int test_zybo_pwm_inputs();
+int test_zybo_rc_receivers();
#endif
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_motor.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_motor.c
new file mode 100644
index 0000000000000000000000000000000000000000..576c1ecaf353bf3caf7f0f63999ac500b8353a84
--- /dev/null
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_motor.c
@@ -0,0 +1,55 @@
+#include "hw_impl_zybo.h"
+
+#define SYS_CLOCK_RATE 100000000 // ticks per second
+#define FREQUENCY 450
+#define PERIOD_WIDTH SYS_CLOCK_RATE/FREQUENCY
+#define PULSE_WIDTH_LOW SYS_CLOCK_RATE/1000 // 1 ms
+#define PULSE_WIDTH_HIGH SYS_CLOCK_RATE/500 // 2 ms
+#define PULSE_WIDTH_ADDR_OFFSET 1
+
+struct MotorDriverState {
+ int *outputs[4];
+};
+
+int zybo_motor_reset(struct MotorDriver *self) {
+ if (self->state == NULL) {
+ self->state = malloc(sizeof(struct MotorDriverState));
+ if (self->state == NULL) {
+ return -1;
+ }
+ }
+ struct MotorDriverState *state = self->state;
+
+ state->outputs[0] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_0_BASEADDR;
+ state->outputs[1] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_1_BASEADDR;
+ state->outputs[2] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_2_BASEADDR;
+ state->outputs[3] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_3_BASEADDR;
+
+ // Set period width of PWM pulse
+ *(state->outputs[0]) = PERIOD_WIDTH;
+ *(state->outputs[1]) = PERIOD_WIDTH;
+ *(state->outputs[2]) = PERIOD_WIDTH;
+ *(state->outputs[3]) = PERIOD_WIDTH;
+
+ // Set a low pulse (1 ms) so that outputs are off
+ *(state->outputs[0] + PULSE_WIDTH_ADDR_OFFSET) = PULSE_WIDTH_LOW;
+ *(state->outputs[1] + PULSE_WIDTH_ADDR_OFFSET) = PULSE_WIDTH_LOW;
+ *(state->outputs[2] + PULSE_WIDTH_ADDR_OFFSET) = PULSE_WIDTH_LOW;
+ *(state->outputs[3] + PULSE_WIDTH_ADDR_OFFSET) = PULSE_WIDTH_LOW;
+
+ usleep(1000000);
+ return 0;
+}
+
+int zybo_motor_write(struct MotorDriver *self,
+ unsigned int channel,
+ float magnitude) {
+ if (magnitude > 1)
+ magnitude = 1;
+ if (magnitude < 0)
+ magnitude = 0;
+ struct MotorDriverState *state = self->state;
+ unsigned long pulse_width_ticks = (unsigned long) (magnitude * (float) (PULSE_WIDTH_HIGH - PULSE_WIDTH_LOW)) + PULSE_WIDTH_LOW;
+ *(state->outputs[channel] + PULSE_WIDTH_ADDR_OFFSET) = pulse_width_ticks;
+ return 0;
+}
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_pwm_input.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_pwm_input.c
deleted file mode 100644
index b0c3cd98ef90a33fc0a416f2f02a1ed6b92f57cf..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_pwm_input.c
+++ /dev/null
@@ -1,32 +0,0 @@
-#include "hw_impl_zybo.h"
-
-struct PWMInputDriverState {
- int *channels[6];
-};
-
-int zybo_pwm_input_reset(struct PWMInputDriver *self) {
- if (self->state == NULL) {
- self->state = malloc(sizeof(struct PWMInputDriverState));
- if (self->state == NULL) {
- return -1;
- }
- }
-
- // Save the addresses of the input PWM recorders
- struct PWMInputDriverState *state = self->state;
- state->channels[0] = (int *) XPAR_PWM_RECORDER_0_BASEADDR;
- state->channels[1] = (int *) XPAR_PWM_RECORDER_1_BASEADDR;
- state->channels[2] = (int *) XPAR_PWM_RECORDER_2_BASEADDR;
- state->channels[3] = (int *) XPAR_PWM_RECORDER_3_BASEADDR;
- state->channels[4] = (int *) XPAR_PWM_RECORDER_4_BASEADDR;
- state->channels[5] = (int *) XPAR_PWM_RECORDER_5_BASEADDR;
- return 0;
-}
-
-int zybo_pwm_input_read(struct PWMInputDriver *self,
- unsigned int channel,
- unsigned long *pulse_width_us) {
- struct PWMInputDriverState *state = self->state;
- *pulse_width_us = (long) *((int *) state->channels[channel]);
- return 0;
-}
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_pwm_output.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_pwm_output.c
deleted file mode 100644
index 3dd09e54379432529a3ce05614dedf648dffb3e2..0000000000000000000000000000000000000000
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_pwm_output.c
+++ /dev/null
@@ -1,54 +0,0 @@
-#include "hw_impl_zybo.h"
-
-#define SYS_CLOCK_RATE 100000000 // ticks per second
-#define FREQUENCY 450
-#define PERIOD_WIDTH SYS_CLOCK_RATE/FREQUENCY
-#define THROTTLE_PULSE_WIDTH_LOW SYS_CLOCK_RATE/1000 // 1 ms
-#define THROTTLE_PULSE_WIDTH_HIGH SYS_CLOCK_RATE/500 // 2 ms
-#define PULSE_WIDTH_ADDR_OFFSET 1
-
-struct PWMOutputDriverState {
- int *outputs[4];
-};
-
-int zybo_pwm_output_reset(struct PWMOutputDriver *self) {
- if (self->state == NULL) {
- self->state = malloc(sizeof(struct PWMOutputDriverState));
- if (self->state == NULL) {
- return -1;
- }
- }
- struct PWMOutputDriverState *state = self->state;
-
- state->outputs[0] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_0_BASEADDR;
- state->outputs[1] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_1_BASEADDR;
- state->outputs[2] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_2_BASEADDR;
- state->outputs[3] = (int *) XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_3_BASEADDR;
-
- // Set period width of PWM pulse
- *(state->outputs[0]) = PERIOD_WIDTH;
- *(state->outputs[1]) = PERIOD_WIDTH;
- *(state->outputs[2]) = PERIOD_WIDTH;
- *(state->outputs[3]) = PERIOD_WIDTH;
-
- // Set a low pulse (1 ms) so that outputs are off
- *(state->outputs[0] + PULSE_WIDTH_ADDR_OFFSET) = THROTTLE_PULSE_WIDTH_LOW;
- *(state->outputs[1] + PULSE_WIDTH_ADDR_OFFSET) = THROTTLE_PULSE_WIDTH_LOW;
- *(state->outputs[2] + PULSE_WIDTH_ADDR_OFFSET) = THROTTLE_PULSE_WIDTH_LOW;
- *(state->outputs[3] + PULSE_WIDTH_ADDR_OFFSET) = THROTTLE_PULSE_WIDTH_LOW;
-
- usleep(1000000);
- return 0;
-}
-
-int zybo_pwm_output_write(struct PWMOutputDriver *self,
- unsigned int channel,
- unsigned long pulse_width_us) {
- if (pulse_width_us > THROTTLE_PULSE_WIDTH_HIGH)
- pulse_width_us = THROTTLE_PULSE_WIDTH_HIGH;
- if (pulse_width_us < THROTTLE_PULSE_WIDTH_LOW)
- pulse_width_us = THROTTLE_PULSE_WIDTH_LOW;
- struct PWMOutputDriverState *state = self->state;
- *(state->outputs[channel] + PULSE_WIDTH_ADDR_OFFSET) = pulse_width_us;
- return 0;
-}
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_rc_receiver.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_rc_receiver.c
new file mode 100644
index 0000000000000000000000000000000000000000..63d6ab525b330789670b251008bbcbc7d863b0ad
--- /dev/null
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_rc_receiver.c
@@ -0,0 +1,40 @@
+#include "hw_impl_zybo.h"
+
+#define SYS_CLOCK_RATE 100000000 // ticks per second
+#define FREQUENCY 450
+#define PERIOD_WIDTH SYS_CLOCK_RATE/FREQUENCY
+#define PULSE_WIDTH_LOW SYS_CLOCK_RATE/1000 // 1 ms
+#define PULSE_WIDTH_HIGH SYS_CLOCK_RATE/500 // 2 ms
+#define PULSE_WIDTH_ADDR_OFFSET 1
+
+struct RCReceiverDriverState {
+ int *channels[6];
+};
+
+int zybo_rc_receiver_reset(struct RCReceiverDriver *self) {
+ if (self->state == NULL) {
+ self->state = malloc(sizeof(struct RCReceiverDriverState));
+ if (self->state == NULL) {
+ return -1;
+ }
+ }
+
+ // Save the addresses of the input PWM recorders
+ struct RCReceiverDriverState *state = self->state;
+ state->channels[0] = (int *) XPAR_PWM_RECORDER_0_BASEADDR;
+ state->channels[1] = (int *) XPAR_PWM_RECORDER_1_BASEADDR;
+ state->channels[2] = (int *) XPAR_PWM_RECORDER_2_BASEADDR;
+ state->channels[3] = (int *) XPAR_PWM_RECORDER_3_BASEADDR;
+ state->channels[4] = (int *) XPAR_PWM_RECORDER_4_BASEADDR;
+ state->channels[5] = (int *) XPAR_PWM_RECORDER_5_BASEADDR;
+ return 0;
+}
+
+int zybo_rc_receiver_read(struct RCReceiverDriver *self,
+ unsigned int channel,
+ float *magnitude) {
+ struct RCReceiverDriverState *state = self->state;
+ unsigned long pulse_width_ticks = (long) *((int *) state->channels[channel]);
+ *magnitude = (float) (pulse_width_ticks - PULSE_WIDTH_LOW) / (float) (PULSE_WIDTH_HIGH - PULSE_WIDTH_LOW);
+ return 0;
+}
diff --git a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
index 2a4a32c8063c34b5c4bf2f56651e7ee9de57b7d7..5971d023b9f4026040217b8368e6ee5d7967ce4a 100644
--- a/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
+++ b/quad/xsdk_workspace/real_quad/src/hw_impl_zybo_tests.c
@@ -159,18 +159,18 @@ int test_zybo_i2c_all() {
* 4) Set the RUN_TESTS macro in main.c
* 5) Uncomment only this test in main.c
* 6) Debug main.
- * 7) Observe the values of pwm_inputs in debugger chaning as you use the
+ * 7) Observe the values of pwm_inputs in debugger changing as you use the
* Spektrum RC controller.
*/
-int test_zybo_pwm_inputs() {
- struct PWMInputDriver pwm_inputs = create_zybo_pwm_inputs();
- pwm_inputs.reset(&pwm_inputs);
+int test_zybo_rc_receiver() {
+ struct RCReceiverDriver rc_receiver = create_zybo_rc_receiver();
+ rc_receiver.reset(&rc_receiver);
- unsigned long pwms[6];
+ float pwms[6];
while (1) {
int i;
for (i = 0; i < 6; i += 1) {
- pwm_inputs.read(&pwm_inputs, i, &pwms[i]);
+ rc_receiver.read(&rc_receiver, i, &pwms[i]);
}
continue;
}
@@ -188,22 +188,21 @@ int test_zybo_pwm_inputs() {
* 6) Run main.
* 7) Observe the PWM width of those PMOD pins changing with time
*/
-int test_zybo_pwm_outputs() {
- struct PWMOutputDriver pwm_outputs = create_zybo_pwm_outputs();
- pwm_outputs.reset(&pwm_outputs);
+int test_zybo_motors() {
+ struct MotorDriver motors = create_zybo_motors();
+ motors.reset(&motors);
struct SystemDriver sys = create_zybo_system();
sys.reset(&sys);
+ double j = 0;
while (1) {
- int j;
- for (j = 100000; j < 200000; j += 1) {
- int i;
+ for (j = 0; j < 1.0; j += 0.01) {
+ int i = 0;
for (i = 0; i < 4; i += 1) {
- pwm_outputs.write(&pwm_outputs, i, j);
- sys.sleep(&sys, 50);
+ motors.write(&motors, i, j);
+ sys.sleep(&sys, 50000);
}
}
- continue;
}
return 0;
}
diff --git a/quad/xsdk_workspace/real_quad/src/main.c b/quad/xsdk_workspace/real_quad/src/main.c
index ef3971feb9c63c531073afa30651d7cb9f3e9959..5892393e53137ad97d0c735c77d768c8f3a70d23 100644
--- a/quad/xsdk_workspace/real_quad/src/main.c
+++ b/quad/xsdk_workspace/real_quad/src/main.c
@@ -8,8 +8,8 @@
int setup_hardware(hardware_t *hardware) {
hardware->i2c = create_zybo_i2c();
- hardware->pwm_inputs = create_zybo_pwm_inputs();
- hardware->pwm_outputs = create_zybo_pwm_outputs();
+ hardware->rc_receiver = create_zybo_rc_receiver();
+ hardware->motors = create_zybo_motors();
hardware->uart = create_zybo_uart();
hardware->global_timer = create_zybo_global_timer();
hardware->axi_timer = create_zybo_axi_timer();
@@ -29,8 +29,8 @@ int main()
//test_zybo_i2c_imu();
//test_zybo_i2c_px4flow();
//test_zybo_i2c_all();
- //test_zybo_pwm_inputs();
- //test_zybo_pwm_outputs();
+ //test_zybo_rc_receiver();
+ test_zybo_motors();
//test_zybo_uart();
//test_zybo_axi_timer();
//test_zybo_uart();