diff --git a/quad/.gitattributes b/quad/.gitattributes
new file mode 100644
index 0000000000000000000000000000000000000000..16008a9770535114262510a59cf903c5218689e2
--- /dev/null
+++ b/quad/.gitattributes
@@ -0,0 +1,4 @@
+# Always use lf for c and header files
+*.c eol=lf
+*.h eol=lf
+
diff --git a/quad/ci-test.sh b/quad/ci-test.sh
index bb2cc16665d0687f7f3cdb553e2d39e8c1ff1b9c..b1e40c7ef4c6424ba48aec4745aab5317ff982fa 100644
--- a/quad/ci-test.sh
+++ b/quad/ci-test.sh
@@ -11,3 +11,7 @@ make || exit 1
cd $QUAD_ROOT/sw/modular_quad_pid/test
make || exit 1
./test_uart_buff || exit 1
+
+cd $QUAD_ROOT/computation_graph/test
+make || exit 1
+./test_computation_graph || exit 1
diff --git a/quad/computation_graph/.gitignore b/quad/computation_graph/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..429b549db1ac8959e01fbdaf54012317473f58ee
--- /dev/null
+++ b/quad/computation_graph/.gitignore
@@ -0,0 +1,4 @@
+bin/
+build/
+computation_graph
+test/test_computation_graph
diff --git a/quad/computation_graph/Makefile b/quad/computation_graph/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..44c7e592619900de5251a68e4a8acb989422a6fc
--- /dev/null
+++ b/quad/computation_graph/Makefile
@@ -0,0 +1,220 @@
+#### PROJECT SETTINGS ####
+# The name of the executable to be created
+BIN_NAME := computation_graph
+# Compiler used
+CC ?= gcc
+# Extension of source files used in the project
+SRC_EXT = c
+# Path to the source directory, relative to the makefile
+SRC_PATH = ./src
+# Space-separated pkg-config libraries used by this project
+LIBS =
+# General compiler flags
+COMPILE_FLAGS = -std=c99 -Wall -Wextra -g
+# Additional release-specific flags
+RCOMPILE_FLAGS = -D NDEBUG
+# Additional debug-specific flags
+DCOMPILE_FLAGS = -D DEBUG
+# Add additional include paths
+INCLUDES = -I $(SRC_PATH)
+# General linker settings
+LINK_FLAGS = -lm
+# Additional release-specific linker settings
+RLINK_FLAGS =
+# Additional debug-specific linker settings
+DLINK_FLAGS =
+# Destination directory, like a jail or mounted system
+DESTDIR = /
+# Install path (bin/ is appended automatically)
+INSTALL_PREFIX = usr/local
+#### END PROJECT SETTINGS ####
+
+# Generally should not need to edit below this line
+
+# Obtains the OS type, either 'Darwin' (OS X) or 'Linux'
+UNAME_S:=$(shell uname -s)
+
+# Function used to check variables. Use on the command line:
+# make print-VARNAME
+# Useful for debugging and adding features
+print-%: ; @echo $*=$($*)
+
+# Shell used in this makefile
+# bash is used for 'echo -en'
+SHELL = /bin/bash
+# Clear built-in rules
+.SUFFIXES:
+# Programs for installation
+INSTALL = install
+INSTALL_PROGRAM = $(INSTALL)
+INSTALL_DATA = $(INSTALL) -m 644
+
+# Append pkg-config specific libraries if need be
+ifneq ($(LIBS),)
+ COMPILE_FLAGS += $(shell pkg-config --cflags $(LIBS))
+ LINK_FLAGS += $(shell pkg-config --libs $(LIBS))
+endif
+
+# Verbose option, to output compile and link commands
+export V := false
+export CMD_PREFIX := @
+ifeq ($(V),true)
+ CMD_PREFIX :=
+endif
+
+# Combine compiler and linker flags
+release: export CFLAGS := $(CFLAGS) $(COMPILE_FLAGS) $(RCOMPILE_FLAGS)
+release: export LDFLAGS := $(LDFLAGS) $(LINK_FLAGS) $(RLINK_FLAGS)
+debug: export CFLAGS := $(CFLAGS) $(COMPILE_FLAGS) $(DCOMPILE_FLAGS)
+debug: export LDFLAGS := $(LDFLAGS) $(LINK_FLAGS) $(DLINK_FLAGS)
+
+# Build and output paths
+release: export BUILD_PATH := build/release
+release: export BIN_PATH := bin/release
+debug: export BUILD_PATH := build/debug
+debug: export BIN_PATH := bin/debug
+install: export BIN_PATH := bin/release
+
+# Find all source files in the source directory, sorted by most
+# recently modified
+ifeq ($(UNAME_S),Darwin)
+ SOURCES = $(shell find $(SRC_PATH) -name '*.$(SRC_EXT)' | sort -k 1nr | cut -f2-)
+else
+ SOURCES = $(shell find $(SRC_PATH) -name '*.$(SRC_EXT)' -printf '%T@\t%p\n' \
+ | sort -k 1nr | cut -f2-)
+endif
+
+# fallback in case the above fails
+rwildcard = $(foreach d, $(wildcard $1*), $(call rwildcard,$d/,$2) \
+ $(filter $(subst *,%,$2), $d))
+ifeq ($(SOURCES),)
+ SOURCES := $(call rwildcard, $(SRC_PATH), *.$(SRC_EXT))
+endif
+
+# Set the object file names, with the source directory stripped
+# from the path, and the build path prepended in its place
+OBJECTS = $(SOURCES:$(SRC_PATH)/%.$(SRC_EXT)=$(BUILD_PATH)/%.o)
+# Set the dependency files that will be used to add header dependencies
+DEPS = $(OBJECTS:.o=.d)
+
+# Macros for timing compilation
+ifeq ($(UNAME_S),Darwin)
+ CUR_TIME = awk 'BEGIN{srand(); print srand()}'
+ TIME_FILE = $(dir $@).$(notdir $@)_time
+ START_TIME = $(CUR_TIME) > $(TIME_FILE)
+ END_TIME = read st < $(TIME_FILE) ; \
+ $(RM) $(TIME_FILE) ; \
+ st=$$((`$(CUR_TIME)` - $$st)) ; \
+ echo $$st
+else
+ TIME_FILE = $(dir $@).$(notdir $@)_time
+ START_TIME = date '+%s' > $(TIME_FILE)
+ END_TIME = read st < $(TIME_FILE) ; \
+ $(RM) $(TIME_FILE) ; \
+ st=$$((`date '+%s'` - $$st - 86400)) ; \
+ echo `date -u -d @$$st '+%H:%M:%S'`
+endif
+
+# Version macros
+# Comment/remove this section to remove versioning
+USE_VERSION := false
+# If this isn't a git repo or the repo has no tags, git describe will return non-zero
+ifeq ($(shell git describe > /dev/null 2>&1 ; echo $$?), 0)
+ USE_VERSION := true
+ VERSION := $(shell git describe --tags --long --dirty --always | \
+ sed 's/v\([0-9]*\)\.\([0-9]*\)\.\([0-9]*\)-\?.*-\([0-9]*\)-\(.*\)/\1 \2 \3 \4 \5/g')
+ VERSION_MAJOR := $(word 1, $(VERSION))
+ VERSION_MINOR := $(word 2, $(VERSION))
+ VERSION_PATCH := $(word 3, $(VERSION))
+ VERSION_REVISION := $(word 4, $(VERSION))
+ VERSION_HASH := $(word 5, $(VERSION))
+ VERSION_STRING := \
+ "$(VERSION_MAJOR).$(VERSION_MINOR).$(VERSION_PATCH).$(VERSION_REVISION)-$(VERSION_HASH)"
+ override CFLAGS := $(CFLAGS) \
+ -D VERSION_MAJOR=$(VERSION_MAJOR) \
+ -D VERSION_MINOR=$(VERSION_MINOR) \
+ -D VERSION_PATCH=$(VERSION_PATCH) \
+ -D VERSION_REVISION=$(VERSION_REVISION) \
+ -D VERSION_HASH=\"$(VERSION_HASH)\"
+endif
+
+# Standard, non-optimized release build
+.PHONY: release
+release: dirs
+ifeq ($(USE_VERSION), true)
+ @echo "Beginning release build v$(VERSION_STRING)"
+else
+ @echo "Beginning release build"
+endif
+ @$(START_TIME)
+ @$(MAKE) all --no-print-directory
+ @echo -n "Total build time: "
+ @$(END_TIME)
+
+# Debug build for gdb debugging
+.PHONY: debug
+debug: dirs
+ifeq ($(USE_VERSION), true)
+ @echo "Beginning debug build v$(VERSION_STRING)"
+else
+ @echo "Beginning debug build"
+endif
+ @$(START_TIME)
+ @$(MAKE) all --no-print-directory
+ @echo -n "Total build time: "
+ @$(END_TIME)
+
+# Create the directories used in the build
+.PHONY: dirs
+dirs:
+ @echo "Creating directories"
+ @mkdir -p $(dir $(OBJECTS))
+ @mkdir -p $(BIN_PATH)
+
+# Installs to the set path
+.PHONY: install
+install:
+ @echo "Installing to $(DESTDIR)$(INSTALL_PREFIX)/bin"
+ @$(INSTALL_PROGRAM) $(BIN_PATH)/$(BIN_NAME) $(DESTDIR)$(INSTALL_PREFIX)/bin
+
+# Uninstalls the program
+.PHONY: uninstall
+uninstall:
+ @echo "Removing $(DESTDIR)$(INSTALL_PREFIX)/bin/$(BIN_NAME)"
+ @$(RM) $(DESTDIR)$(INSTALL_PREFIX)/bin/$(BIN_NAME)
+
+# Removes all build files
+.PHONY: clean
+clean:
+ @echo "Deleting $(BIN_NAME) symlink"
+ @$(RM) $(BIN_NAME)
+ @echo "Deleting directories"
+ @$(RM) -r build
+ @$(RM) -r bin
+
+# Main rule, checks the executable and symlinks to the output
+all: $(BIN_PATH)/$(BIN_NAME)
+ @echo "Making symlink: $(BIN_NAME) -> $<"
+ @$(RM) $(BIN_NAME)
+ @ln -s $(BIN_PATH)/$(BIN_NAME) $(BIN_NAME)
+
+# Link the executable
+$(BIN_PATH)/$(BIN_NAME): $(OBJECTS)
+ @echo "Linking: $@"
+ @$(START_TIME)
+ $(CMD_PREFIX)$(CC) $(OBJECTS) $(LDFLAGS) -o $@
+ @echo -en "\t Link time: "
+ @$(END_TIME)
+
+# Add dependency files, if they exist
+-include $(DEPS)
+
+# Source file rules
+# After the first compilation they will be joined with the rules from the
+# dependency files to provide header dependencies
+$(BUILD_PATH)/%.o: $(SRC_PATH)/%.$(SRC_EXT)
+ @echo "Compiling: $< -> $@"
+ @$(START_TIME)
+ $(CMD_PREFIX)$(CC) $(CFLAGS) $(INCLUDES) -MP -MMD -c $< -o $@
+ @echo -en "\t Compile time: "
+ @$(END_TIME)
diff --git a/quad/computation_graph/src/computation_graph.c b/quad/computation_graph/src/computation_graph.c
new file mode 100644
index 0000000000000000000000000000000000000000..5fcfd4c5c76ad7c221da927c410f45184c5c0578
--- /dev/null
+++ b/quad/computation_graph/src/computation_graph.c
@@ -0,0 +1,248 @@
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+#include "computation_graph.h"
+
+#define GRAPH_MAX_DEPTH 20
+#define GRAPH_MAX_INPUTS 20
+
+// Array to store input values for passing to the execute function of each node
+static double exec_input_vals[GRAPH_MAX_INPUTS];
+
+struct computation_graph *create_graph() {
+ struct computation_graph *the_graph = malloc(sizeof(struct computation_graph));
+ if (!the_graph) {return NULL;}
+ the_graph->nodes = malloc(sizeof(struct graph_node));
+ if (!the_graph->nodes) { return NULL; }
+ the_graph->n_nodes = 0;
+ the_graph->size = 1;
+ return the_graph;
+}
+
+static void reset_node_rec(struct computation_graph* graph, int node_id, int depth) {
+ if (depth > GRAPH_MAX_DEPTH) {
+ return;
+ }
+ struct graph_node* node = &graph->nodes[node_id];
+ // Don't reset nodes that are already connected to something else
+ // Don't reset nodes that have already been reset or discovered
+ if (node->n_children != 1 || node->processed_state != UNPROCESSED) {
+ return;
+ }
+ node->processed_state = DISCOVERED;
+ int input_id;
+ for (input_id = 0; input_id < node->type->n_inputs; input_id++) {
+ int src_node_id = node->input_srcs[input_id].controller_id;
+ if (src_node_id != -1) {
+ reset_node_rec(graph, src_node_id, depth + 1);
+ }
+ }
+ // Reset this node
+ if (node->type->reset != NULL) {
+ node->type->reset(node->state);
+ }
+ node->updated = 1;
+ node->processed_state = PROCESSED;
+}
+
+int reset_node(struct computation_graph* graph, int node_id) {
+ if (node_id >= graph->n_nodes) {
+ return -1;
+ }
+ int i;
+ for (i = 0; i < graph->n_nodes; i++) {
+ graph->nodes[i].processed_state = UNPROCESSED;
+ }
+ reset_node_rec(graph, node_id, 0);
+ return 0;
+}
+
+int graph_set_source(struct computation_graph *graph,
+ int dest_node_id, int dest_input, int src_node_id, int src_output) {
+ if (dest_node_id >= graph->n_nodes || src_node_id >= graph->n_nodes) {
+ return -1;
+ }
+ struct graph_node *dest_node = &graph->nodes[dest_node_id];
+ struct graph_node *src_node = &graph->nodes[src_node_id];
+ if (dest_input >= dest_node->type->n_inputs || src_output >= src_node->type->n_outputs) {
+ return -1;
+ }
+
+ // If a previous source exists, remove one from its children count
+ int prev_src_id = dest_node->input_srcs[dest_input].controller_id;
+ if (prev_src_id != -1) {
+ graph->nodes[prev_src_id].n_children -= 1;
+ }
+ src_node->n_children += 1; // Count destination node as a child
+ dest_node->input_srcs[dest_input].controller_id = src_node_id;
+ dest_node->input_srcs[dest_input].controller_output = src_output;
+ dest_node->updated = 1;
+ reset_node(graph, src_node_id);
+ return 0;
+}
+
+int graph_add_node(struct computation_graph *graph,
+ const char* name,
+ const struct graph_node_type *type,
+ void *state) {
+ assert(type->n_inputs <= GRAPH_MAX_INPUTS);
+ int new_id = graph->n_nodes;
+ if (new_id >= graph->size) {
+ int new_capacity = graph->n_nodes == 0 ? 1 : graph->n_nodes * 2;
+ struct graph_node *node_arr = realloc(graph->nodes, sizeof(struct graph_node) * new_capacity);
+ if (!node_arr) {
+ return -1;
+ }
+ graph->size = new_capacity;
+ graph->nodes = node_arr;
+ }
+ struct graph_node *new_node = &graph->nodes[new_id];
+ new_node->name = strdup(name);
+ new_node->type = type;
+ new_node->state = state;
+ new_node->n_children = 0;
+ new_node->updated = 1;
+ new_node->output_values = malloc(type->n_outputs * sizeof(double));
+ new_node->param_values = malloc(type->n_params * sizeof(double));
+ new_node->input_srcs = malloc(type->n_inputs * sizeof(struct input_type));
+ // 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)) {
+ return -1;
+ }
+ int i;
+ for (i = 0; i < type->n_inputs; i++) {
+ new_node->input_srcs[i].controller_id = -1;
+ }
+ graph->n_nodes += 1;
+ // Reset block upon creation
+ if (new_node->type->reset != NULL) {
+ new_node->type->reset(new_node->state);
+ }
+ return new_id;
+}
+
+int graph_set_param_val(struct computation_graph *graph, int node_id, int param_id, double value) {
+ if (node_id >= graph->n_nodes || param_id >= graph->nodes[node_id].type->n_params) {
+ return -1;
+ }
+ graph->nodes[node_id].param_values[param_id] = value;
+ graph->nodes[node_id].updated = 1;
+ return 0;
+}
+
+double graph_get_param_val(const struct computation_graph *graph, int node_id, int param_id) {
+ if (node_id >= graph->n_nodes || param_id >= graph->nodes[node_id].type->n_params) {
+ return NAN;
+ }
+ return graph->nodes[node_id].param_values[param_id];
+}
+
+double graph_get_output(const struct computation_graph *graph, int node_id, int output_id) {
+ if (node_id >= graph->n_nodes || output_id >= graph->nodes[node_id].type->n_outputs) {
+ return NAN;
+ }
+ return graph->nodes[node_id].output_values[output_id];
+}
+
+void graph_compute_node_rec(struct computation_graph *graph, int node_id, int depth) {
+ if (depth >= GRAPH_MAX_DEPTH) {
+ assert(1 == 0); // TODO :Xil_Assert false
+ return;
+ }
+ if (node_id >= graph->n_nodes) {
+ return;
+ }
+ struct graph_node *node = &graph->nodes[node_id];
+ if (node->processed_state != UNPROCESSED) {
+ return;
+ }
+ node->processed_state = DISCOVERED;
+ int input_id;
+ for (input_id = 0; input_id < node->type->n_inputs; input_id++) {
+ int src_cntl_id = node->input_srcs[input_id].controller_id;
+ if (src_cntl_id != -1) {
+ graph_compute_node_rec(graph, src_cntl_id, depth + 1);
+ node->updated |= graph->nodes[src_cntl_id].updated;
+ }
+ }
+ if (node->updated) {
+ // Populate the exec_input_vals array for computation
+ for (input_id = 0; input_id < node->type->n_inputs; input_id++) {
+ int src_cntl_id = node->input_srcs[input_id].controller_id;
+ int src_output_id = node->input_srcs[input_id].controller_output;
+ if (src_cntl_id != -1) {
+ exec_input_vals[input_id] = graph->nodes[src_cntl_id].output_values[src_output_id];
+ }
+ else {
+ // Set input value to 0 if not connected
+ exec_input_vals[input_id] = 0;
+ }
+ }
+ if (node->type->execute != NULL) {
+ (*node->type->execute)(node->state, node->param_values, exec_input_vals, node->output_values);
+ }
+ }
+ node->processed_state = PROCESSED;
+}
+
+void graph_compute_nodes(struct computation_graph *graph, int* node_ids, int n_nodes) {
+ int i;
+ for (i = 0; i < graph->n_nodes; i++) {
+ graph->nodes[i].processed_state = UNPROCESSED;
+ }
+ for (i = 0; i < n_nodes; i++) {
+ int node_id = node_ids[i];
+ if (node_id < graph->n_nodes) {
+ graph_compute_node_rec(graph, node_id, 0);
+ }
+ }
+ // Clear all the updated flags for nodes that were actually executed
+ for (i = 0; i < graph->n_nodes; i++) {
+ struct graph_node* node = &graph->nodes[i];
+ if (node->processed_state == PROCESSED) {
+ node->updated = 0;
+ }
+ }
+}
+
+int export_dot(const struct computation_graph* graph, FILE* of, int print_outputs) {
+ fprintf(of, "digraph G {\n"); // Header
+ fprintf(of, "rankdir=\"LR\"\n"); // Horizontal layout
+
+ // Draw all the nodes and their inputs
+ int i;
+ for (i = 0; i < graph->n_nodes; i++) {
+ struct graph_node *node = &graph->nodes[i];
+ // Create node
+ fprintf(of, "\"%s\"[shape=record\nlabel=\"", node->name);
+ fprintf(of, "<f0>%s ", node->name); // Node name is port 0
+ int j;
+ // Create ports for inputs
+ for (j = 0; j < node->type->n_inputs; j++) {
+ fprintf(of, " |<f%d> --\\>%s", j+1, node->type->input_names[j]);
+ }
+ // Create ports for parameters
+ for (j = 0; j < node->type->n_params; j++) {
+ fprintf(of, " |<f%d> [%s=%.3f]", j+1+node->type->n_inputs, node->type->param_names[j],node->param_values[j]);
+ }
+ fprintf(of, "\"]\n"); // Close label bracket
+ // Make connections from
+ for (j = 0; j < node->type->n_inputs; j++) {
+ int input_id = node->input_srcs[j].controller_id;
+ if (input_id != -1) {
+ struct graph_node* src_node = &graph->nodes[input_id];
+ int output_id = node->input_srcs[j].controller_output;
+ const char* output_name = src_node->type->output_names[output_id];
+ fprintf(of, "\"%s\" -> \"%s\":f%d [label=\"%s", src_node->name, node->name, j+1, output_name);
+ if (print_outputs) {
+ fprintf(of, "=%.3f", src_node->output_values[output_id]);
+ }
+ fprintf(of, "\"]\n");
+ }
+ }
+ }
+ fprintf(of, "}"); // Close graph
+ return 0;
+}
diff --git a/quad/computation_graph/src/computation_graph.h b/quad/computation_graph/src/computation_graph.h
new file mode 100644
index 0000000000000000000000000000000000000000..e5e769fd28abcb90f66ef2df40434e132c496c62
--- /dev/null
+++ b/quad/computation_graph/src/computation_graph.h
@@ -0,0 +1,111 @@
+#ifndef __COMPUTATION_GRAPH_H__
+#define __COMPUTATION_GRAPH_H__
+
+#include <stdio.h>
+#include <math.h>
+
+typedef void (*execute_node_t)(void *state,
+ const double* params,
+ const double *inputs,
+ double *outputs);
+
+typedef void (*reset_node_t)(void *state);
+
+enum node_processed_state {
+ UNPROCESSED,
+ DISCOVERED,
+ PROCESSED
+};
+
+struct computation_graph {
+ int n_nodes;
+ int size;
+ struct graph_node *nodes;
+};
+
+// Declares a node type
+struct graph_node_type {
+ const char* const* input_names;
+ const char* const* output_names;
+ const char* const* param_names;
+ int n_inputs;
+ int n_outputs;
+ int n_params;
+ execute_node_t execute;
+ reset_node_t reset;
+};
+
+// Declares an instance of a node
+struct graph_node {
+ const char *name; // Name of this node instance
+ const struct graph_node_type* type; // Type of this node
+ double *output_values; // Computed output values
+ double *param_values; // Values of parameters set for this node
+ int n_children; // The number of connected children
+ void *state; // Pointer to the state instance
+ int processed_state; // State of the node with respecct to the graph traversal
+ struct input_type { // Array of tuples indicating the source for each input to this node
+ int controller_id;
+ int controller_output;
+ } *input_srcs;
+ int updated; // 1 if this node has had an input or parameter change
+};
+
+/*
+ * Creates an empty computation graph
+ * May return NULL on failure
+ */
+struct computation_graph *create_graph();
+
+/*
+ * Defines which node's output gets its value passed into the input of a different node.
+ * Will call reset for each node which was previously orphaned, but is now connected to the graph
+ * dest_node_id: The ID of the node to which the input belongs
+ * dest_input: The ID of the input for node dest_cntl to pass the value into
+ * src_node_id: The node ID where the value is coming from
+ * src_output: The ID of the output on <src_node_id> where the value comes from
+ * Returns 0 for success
+ */
+int graph_set_source(struct computation_graph *graph, int dest_node_id, int dest_input, int src_node_id, int src_output);
+
+/*
+ * Creates a new node with the given data, and adds it to the graph.
+ * Returns a negative integer upon failure.
+ * Otherwise returns a positive integer which is the ID of the new node
+ */
+int graph_add_node(struct computation_graph *graph,
+ const char *name,
+ const struct graph_node_type *type,
+ void *state);
+
+/*
+ * Returns the value at the output of the requested node for the requested output.
+ * Returns 0 if the given node or output IDs are invalid
+ */
+double graph_get_output(const struct computation_graph *graph, int node_id, int output_id);
+
+/*
+ * Sets a parameter given by param_id on node node_id to the given value
+ * Returns 0 upon success
+ */
+int graph_set_param_val(struct computation_graph *graph, int node_id, int param_id, double value);
+
+/*
+ * Returns the value of the param at param_id for the given node
+ * Will return NaN if the given node or parameter IDs are not valid
+ */
+double graph_get_param_val(const struct computation_graph *graph, int node_id, int param_id);
+
+/*
+ * Computes the nodes given by node_id.
+ * To do so, computes all nodes which are ancestors of each given node_id in topological order, with
+ * the final computation being node_id itself.
+ */
+void graph_compute_nodes(struct computation_graph *graph, int* node_ids, int n_nodes);
+
+/*
+ * Writes a graphical representation of the given graph to <of> in the DOT language
+ */
+int export_dot(const struct computation_graph* graph, FILE* of, int print_outputs);
+
+#endif // __COMPUTATION_GRAPH_H__
diff --git a/quad/computation_graph/src/graph_blocks/node_accumulator.c b/quad/computation_graph/src/graph_blocks/node_accumulator.c
new file mode 100644
index 0000000000000000000000000000000000000000..17a36ee92dffc796e9ea592f28ecd1f4737085a1
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_accumulator.c
@@ -0,0 +1,39 @@
+#include "node_accumulator.h"
+#include <stdlib.h>
+
+struct accum_state {
+ double accumulated;
+};
+
+static void accum_nodes(void *state, const double* params, const double *inputs, double *outputs) {
+ struct accum_state* my_state = (struct accum_state*)state;
+ my_state->accumulated += inputs[ACCUM_IN];
+ outputs[ACCUMULATED] = my_state->accumulated;
+}
+
+static void reset(void *state) {
+ ((struct accum_state*)state)->accumulated = 0;
+}
+
+
+static const char* const in_names[2] = {"Accumulator in"};
+static const char* const out_names[1] = {"Accumulated"};
+static const char* const param_names[0] = {};
+const struct graph_node_type node_accum_type = {
+ .input_names = in_names,
+ .output_names = out_names,
+ .param_names = param_names,
+ .n_inputs = 1,
+ .n_outputs = 1,
+ .n_params = 0,
+ .execute = accum_nodes,
+ .reset = reset
+};
+
+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/computation_graph/src/graph_blocks/node_accumulator.h b/quad/computation_graph/src/graph_blocks/node_accumulator.h
new file mode 100644
index 0000000000000000000000000000000000000000..2df06c9d1c87473ed2bbb4fc6f8d40aa4b606318
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_accumulator.h
@@ -0,0 +1,16 @@
+#ifndef __NODE_ACCUMULATOR_H__
+#define __NODE_ACCUMULATOR_H__
+#include "../computation_graph.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 {
+ ACCUM_IN,
+};
+
+enum graph_node_accum_outputs {
+ ACCUMULATED
+};
+#endif // __NODE_ACCUMULATOR_H__
diff --git a/quad/computation_graph/src/graph_blocks/node_add.c b/quad/computation_graph/src/graph_blocks/node_add.c
new file mode 100644
index 0000000000000000000000000000000000000000..eefb22fde9ae9a5cc72d2eb679905f2e70038ac9
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_add.c
@@ -0,0 +1,25 @@
+#include "node_add.h"
+#include <stdlib.h>
+
+static void add_nodes(void *state, const double* params, const double *inputs, double *outputs) {
+ outputs[ADD_SUM] = inputs[ADD_SUMMAND1] + inputs[ADD_SUMMAND2];
+}
+static void reset(void *state) {}
+
+static const char* const in_names[2] = {"Summand 1", "Summand 2"};
+static const char* const out_names[1] = {"Sum"};
+static const char* const param_names[0] = {};
+const struct graph_node_type node_add_type = {
+ .input_names = in_names,
+ .output_names = out_names,
+ .param_names = param_names,
+ .n_inputs = 2,
+ .n_outputs = 1,
+ .n_params = 0,
+ .execute = add_nodes,
+ .reset = reset
+};
+
+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/computation_graph/src/graph_blocks/node_add.h b/quad/computation_graph/src/graph_blocks/node_add.h
new file mode 100644
index 0000000000000000000000000000000000000000..cfd2b71e8b25aecfa38898dbce8427ad97bf6ff9
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_add.h
@@ -0,0 +1,17 @@
+#ifndef __NODE_ADD_H__
+#define __NODE_ADD_H__
+#include "../computation_graph.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 {
+ ADD_SUMMAND1,
+ ADD_SUMMAND2,
+};
+
+enum graph_node_add_outputs {
+ ADD_SUM
+};
+#endif // __NODE_ADD_H__
diff --git a/quad/computation_graph/src/graph_blocks/node_constant.c b/quad/computation_graph/src/graph_blocks/node_constant.c
new file mode 100644
index 0000000000000000000000000000000000000000..8ad9f8c5afdb5dc8f4cfacac00f2725ad388a59a
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_constant.c
@@ -0,0 +1,25 @@
+#include "node_constant.h"
+#include <stdlib.h>
+
+static void output_const(void *state, const double *params, const double *inputs, double *outputs) {
+ outputs[CONST_VAL] = params[CONST_SET];
+}
+static void reset(void *state) {}
+
+static const char* const in_names[0] = {};
+static const char* const out_names[1] = {"Constant"};
+static const char* const param_names[1] = {"Constant"};
+const struct graph_node_type node_const_type = {
+ .input_names = in_names,
+ .output_names = out_names,
+ .param_names = param_names,
+ .n_inputs = 0,
+ .n_outputs = 1,
+ .n_params = 1,
+ .execute = output_const,
+ .reset = reset
+};
+
+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/computation_graph/src/graph_blocks/node_constant.h b/quad/computation_graph/src/graph_blocks/node_constant.h
new file mode 100644
index 0000000000000000000000000000000000000000..91cd6d055b6bbf64340df4282ac9f9539301f0a5
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_constant.h
@@ -0,0 +1,16 @@
+#ifndef __NODE_CONSTANT_H__
+#define __NODE_CONSTANT_H__
+#include "../computation_graph.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 {
+ CONST_SET
+};
+
+enum graph_node_const_outputs {
+ CONST_VAL
+};
+#endif //__NODE_CONSTANT_H__
diff --git a/quad/computation_graph/src/graph_blocks/node_gain.c b/quad/computation_graph/src/graph_blocks/node_gain.c
new file mode 100644
index 0000000000000000000000000000000000000000..35bb3702cf2aa36d2f08f85ed580d12fadbe8fa8
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_gain.c
@@ -0,0 +1,25 @@
+#include "node_gain.h"
+#include <stdlib.h>
+
+static void scale_nodes(void *state, const double* params, const double *inputs, double *outputs) {
+ outputs[GAIN_RESULT] = inputs[GAIN_INPUT] * params[GAIN_GAIN];
+}
+static void reset(void *state) {}
+
+static const char* const in_names[1] = {"Input"};
+static const char* const out_names[1] = {"Amplified"};
+static const char* const param_names[1] = {"Gain"};
+const struct graph_node_type node_gain_type = {
+ .input_names = in_names,
+ .output_names = out_names,
+ .param_names = param_names,
+ .n_inputs = 1,
+ .n_outputs = 1,
+ .n_params = 1,
+ .execute = scale_nodes,
+ .reset = reset
+};
+
+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/computation_graph/src/graph_blocks/node_gain.h b/quad/computation_graph/src/graph_blocks/node_gain.h
new file mode 100644
index 0000000000000000000000000000000000000000..b26d3bfe393468c686f8b5b73957e1604594e5d0
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_gain.h
@@ -0,0 +1,20 @@
+#ifndef __NODE_GAIN_H__
+#define __NODE_GAIN_H__
+#include "../computation_graph.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 {
+ GAIN_INPUT
+};
+
+enum graph_node_pow_params {
+ GAIN_GAIN
+};
+
+enum graph_node_gain_outputs {
+ GAIN_RESULT
+};
+#endif // __NODE_GAIN_H__
diff --git a/quad/computation_graph/src/graph_blocks/node_mult.c b/quad/computation_graph/src/graph_blocks/node_mult.c
new file mode 100644
index 0000000000000000000000000000000000000000..2696719fdc83a89ae346d4dc788df693d97b2ddb
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_mult.c
@@ -0,0 +1,25 @@
+#include "node_mult.h"
+#include <stdlib.h>
+
+static void mult_nodes(void *state, const double* params, const double *inputs, double *outputs) {
+ outputs[MULT_PRODUCT] = inputs[MULT_MULTIPLICAND1] * inputs[MULT_MULTIPLICAND2];
+}
+static void reset(void *state) {}
+
+static const char* const in_names[2] = {"Multiplicand 1", "Multiplicand 2"};
+static const char* const out_names[1] = {"Product"};
+static const char* const param_names[0] = {};
+const struct graph_node_type node_mult_type = {
+ .input_names = in_names,
+ .output_names = out_names,
+ .param_names = param_names,
+ .n_inputs = 2,
+ .n_outputs = 1,
+ .n_params = 0,
+ .execute = mult_nodes,
+ .reset = reset
+};
+
+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/computation_graph/src/graph_blocks/node_mult.h b/quad/computation_graph/src/graph_blocks/node_mult.h
new file mode 100644
index 0000000000000000000000000000000000000000..32d2d873d359ae10bdff6dc857bab6d835c04598
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_mult.h
@@ -0,0 +1,18 @@
+#ifndef __NODE_MULT_H__
+#define __NODE_MULT_H__
+#include "../computation_graph.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 {
+ MULT_MULTIPLICAND1,
+ MULT_MULTIPLICAND2,
+};
+
+enum graph_node_mult_outputs {
+ MULT_PRODUCT
+};
+
+#endif // __NODE_MULT_H__
diff --git a/quad/computation_graph/src/graph_blocks/node_pow.c b/quad/computation_graph/src/graph_blocks/node_pow.c
new file mode 100644
index 0000000000000000000000000000000000000000..dbbc1d707d7833254e5a96a4dc8ba2b9c954ae4d
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_pow.c
@@ -0,0 +1,26 @@
+#include "node_pow.h"
+#include <stdlib.h>
+#include <math.h>
+
+static void pow_nodes(void *state, const double* params, const double *inputs, double *outputs) {
+ outputs[POW_RESULT] = pow(inputs[POW_BASE], params[POW_EXP]);
+}
+static void reset(void *state) {}
+
+static const char* const in_names[1] = {"Base"};
+static const char* const out_names[1] = {"Result"};
+static const char* const param_names[1] = {"Exponent"};
+const struct graph_node_type node_pow_type = {
+ .input_names = in_names,
+ .output_names = out_names,
+ .param_names = param_names,
+ .n_inputs = 1,
+ .n_outputs = 1,
+ .n_params = 1,
+ .execute = pow_nodes,
+ .reset = reset
+};
+
+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/computation_graph/src/graph_blocks/node_pow.h b/quad/computation_graph/src/graph_blocks/node_pow.h
new file mode 100644
index 0000000000000000000000000000000000000000..11bdd9173c7522683e3b8af7fec809632b856c19
--- /dev/null
+++ b/quad/computation_graph/src/graph_blocks/node_pow.h
@@ -0,0 +1,20 @@
+#ifndef __NODE_POW_H__
+#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 {
+ POW_BASE
+};
+
+enum graph_node_pow_params {
+ POW_EXP
+};
+
+enum graph_node_pow_outputs {
+ POW_RESULT
+};
+#endif // __NODE_POW_H__
diff --git a/quad/computation_graph/src/main.c b/quad/computation_graph/src/main.c
new file mode 100644
index 0000000000000000000000000000000000000000..b7fb2d3302228793a67465812e155e37e2f3b1f8
--- /dev/null
+++ b/quad/computation_graph/src/main.c
@@ -0,0 +1,42 @@
+#include <stdio.h>
+#include "computation_graph.h"
+#include "graph_blocks/node_add.h"
+#include "graph_blocks/node_mult.h"
+#include "graph_blocks/node_constant.h"
+#include "graph_blocks/node_gain.h"
+#include "tests.h"
+
+int main() {
+// struct computation_graph *graph = create_graph();
+//
+// int const1 = graph_add_node_const(graph, "Const 2");
+// graph_set_param_val(graph, const1, CONST_SET, 2);
+// int const2 = graph_add_node_const(graph, "Const 1");
+// graph_set_param_val(graph, const2, CONST_SET, 3);
+//
+// int add1_id = graph_add_node_add(graph, "Add");
+// graph_set_source(graph, add1_id, ADD_SUMMAND1, const1, CONST_VAL);
+// graph_set_source(graph, add1_id, ADD_SUMMAND2, const2, CONST_VAL);
+//
+// int gain1_id = graph_add_node_gain(graph, "Gain");
+// graph_set_param_val(graph, gain1_id, GAIN_GAIN, 3);
+// graph_set_source(graph, gain1_id, GAIN_INPUT, add1_id, ADD_SUM);
+//
+// int mult1_id = graph_add_node_mult(graph, "Mult");
+// graph_set_source(graph, mult1_id, MULT_MULTIPLICAND2, gain1_id, GAIN_RESULT);
+// graph_set_source(graph, mult1_id, MULT_MULTIPLICAND1, const1, CONST_VAL);
+//
+// graph_compute_node(graph, mult1_id);
+
+// FILE* dot_fp;
+// dot_fp = fopen("..\\comp_graph.dot", "w");
+// export_dot(graph, dot_fp);
+// fclose(dot_fp);
+// printf("Sum is %f\n", graph_get_output(graph, mult1_id, GAIN_RESULT));
+
+ int success = graph_run_tests();
+ printf("Success: %s\n", success == 0 ? "Yes" : "No");
+ fflush(stdout);
+
+ return 0;
+}
diff --git a/quad/computation_graph/src/tests.c b/quad/computation_graph/src/tests.c
new file mode 100644
index 0000000000000000000000000000000000000000..2f120a2693f4b2ee8116d192b4494299f93f44a8
--- /dev/null
+++ b/quad/computation_graph/src/tests.c
@@ -0,0 +1,151 @@
+//
+// Created by dawehr on 2/9/2017.
+//
+
+#include "tests.h"
+#define GRAPH_TEST_EPS 0.00001
+
+static int nequal(double val1, double val2) {
+ if (fabs(val1 - val2) < GRAPH_TEST_EPS) {
+ return 0;
+ }
+ return -1;
+}
+
+int graph_run_tests() {
+ int success = 0;
+ success |= graph_test_one_add();
+ success |= graph_test_circular_runs();
+ success |= graph_test_circular_resets();
+ success |= graph_test_accumulator();
+ success |= graph_test_single_run();
+ success |= graph_test_reset_rules();
+ success |= graph_test_self_loop();
+ return success;
+}
+
+int graph_test_one_add() {
+ struct computation_graph *graph = create_graph();
+ int block = graph_add_node_add(graph, "Add");
+ int cblock3 = graph_add_node_const(graph, "3");
+ graph_set_param_val(graph, cblock3, CONST_SET, 3);
+ int cblock4 = graph_add_node_const(graph, "4");
+ graph_set_param_val(graph, cblock4, CONST_SET, 4);
+ graph_set_source(graph, block, ADD_SUMMAND1, cblock3, CONST_VAL);
+ graph_set_source(graph, block, ADD_SUMMAND2, cblock4, CONST_VAL);
+ int to_compute_for[1] = {block};
+ graph_compute_nodes(graph, to_compute_for, 1);
+ double result = graph_get_output(graph, block, ADD_SUM);
+ return nequal(result, 7);
+}
+
+
+int graph_test_circular_runs() {
+ struct computation_graph *graph = create_graph();
+ int gain1 = graph_add_node_gain(graph, "gain1");
+ int gain2 = graph_add_node_gain(graph, "gain2");
+ graph_set_source(graph, gain2, GAIN_INPUT, gain1, GAIN_RESULT);
+ graph_set_source(graph, gain1, GAIN_INPUT, gain2, GAIN_RESULT);
+ int to_compute_for[1] = {gain2};
+ graph_compute_nodes(graph, to_compute_for, 1);
+ // If no infinite loop, then success. Value is undefined for circular graphs
+ return 0;
+}
+
+int graph_test_circular_resets() {
+ struct computation_graph *graph = create_graph();
+ int acum1 = graph_add_node_accum(graph, "accumulator1");
+ int acum2 = graph_add_node_accum(graph, "accumulator2");
+ graph_set_source(graph, acum2, ACCUM_IN, acum1, ACCUMULATED);
+ graph_set_source(graph, acum1, ACCUM_IN, acum2, ACCUMULATED);
+ return 0; // Passes if no infinite loop
+}
+
+// Tests the accumulator block, thereby testing reset and state changes
+int graph_test_accumulator() {
+ struct computation_graph *graph = create_graph();
+ int cblock = graph_add_node_const(graph, "const");
+ int acum_b = graph_add_node_accum(graph, "accumulator");
+ graph_set_source(graph, acum_b, ACCUM_IN, cblock, CONST_VAL);
+
+ int to_compute_for[1] = {acum_b};
+ graph_set_param_val(graph, cblock, CONST_SET, 3);
+ graph_compute_nodes(graph, to_compute_for, 1);
+ graph_set_param_val(graph, cblock, CONST_SET, 8);
+ graph_compute_nodes(graph, to_compute_for, 1);
+ graph_set_param_val(graph, cblock, CONST_SET, -2);
+ graph_compute_nodes(graph, to_compute_for, 1);
+
+ double result = graph_get_output(graph, acum_b, ACCUMULATED);
+ if (nequal(result, 9)) {
+ printf("graph_test_accumulator failed on step 1, equals %f\n", result);
+ return -1;
+ }
+
+ // Test reset on source set
+ int gain_b = graph_add_node_gain(graph, "Gain");
+ graph_set_param_val(graph, gain_b, GAIN_GAIN, 1);
+ graph_set_source(graph, gain_b, GAIN_INPUT, acum_b, ACCUMULATED);
+ to_compute_for[0] = gain_b;
+ graph_compute_nodes(graph, to_compute_for, 1);
+ result = graph_get_output(graph, gain_b, GAIN_RESULT);
+ if (nequal(result, -2)) {
+ printf("graph_test_accumulator failed on step 2\n");
+ return -2;
+ }
+ return 0;
+}
+
+// Tests that a block will only execute once per compute,
+// even if its output is connected to multiple inputs
+int graph_test_single_run() {
+ struct computation_graph *graph = create_graph();
+ int acum_b = graph_add_node_accum(graph, "accumulator");
+ int add_block = graph_add_node_add(graph, "Add");
+ int cblock = graph_add_node_const(graph, "const");
+ graph_set_param_val(graph, cblock, CONST_SET, 2);
+
+
+ graph_set_source(graph, acum_b, ACCUM_IN, cblock, CONST_VAL);
+ graph_set_source(graph, add_block, ADD_SUMMAND1, acum_b, ACCUMULATED);
+ graph_set_source(graph, add_block, ADD_SUMMAND2, acum_b, ACCUMULATED);
+
+ int to_compute_for[1] = {add_block};
+ graph_compute_nodes(graph, to_compute_for, 1);
+ double result = graph_get_output(graph, add_block, ADD_SUM);
+ return nequal(result, 4);
+}
+
+// Tests that upon connection of a second child, a block will not reset
+int graph_test_reset_rules() {
+ struct computation_graph *graph = create_graph();
+ int cblock = graph_add_node_const(graph, "5");
+ graph_set_param_val(graph, cblock, CONST_SET, 5);
+ int acum_b = graph_add_node_accum(graph, "accumulator");
+ int gain1 = graph_add_node_gain(graph, "gain1");
+ graph_set_param_val(graph, gain1, GAIN_GAIN, 1);
+
+ graph_set_source(graph, gain1, GAIN_INPUT, acum_b, ACCUMULATED);
+ graph_set_source(graph, acum_b, ACCUM_IN, cblock, CONST_VAL);
+ int to_compute_for[1] = {gain1};
+ graph_compute_nodes(graph, to_compute_for, 1);
+ // state of acum_b is now 5
+
+ int gain2 = graph_add_node_gain(graph, "gain2");
+ graph_set_param_val(graph, gain2, GAIN_GAIN, 1);
+ // Connect gain 2, and accumulator should not get reset
+ graph_set_source(graph, gain2, GAIN_INPUT, acum_b, ACCUMULATED);
+ to_compute_for[0] = gain2;
+ graph_compute_nodes(graph, to_compute_for, 1);
+ double result = graph_get_output(graph, gain2, GAIN_RESULT);
+ return nequal(result, 10);
+}
+
+int graph_test_self_loop() {
+ struct computation_graph *graph = create_graph();
+ int gain1 = graph_add_node_gain(graph, "gain1");
+ graph_set_source(graph, gain1, GAIN_INPUT, gain1, GAIN_RESULT);
+ int to_compute_for[1] = {gain1};
+ graph_compute_nodes(graph, to_compute_for, 1);
+ return 0;
+}
diff --git a/quad/computation_graph/src/tests.h b/quad/computation_graph/src/tests.h
new file mode 100644
index 0000000000000000000000000000000000000000..b30b7d806b88f3811b74f264742d4094cd4387c8
--- /dev/null
+++ b/quad/computation_graph/src/tests.h
@@ -0,0 +1,32 @@
+//
+// Created by dawehr on 2/9/2017.
+//
+
+#ifndef COMPUTATION_GRAPH_TESTS_H
+#define COMPUTATION_GRAPH_TESTS_H
+
+#include "computation_graph.h"
+#include "graph_blocks/node_add.h"
+#include "graph_blocks/node_mult.h"
+#include "graph_blocks/node_constant.h"
+#include "graph_blocks/node_gain.h"
+#include "graph_blocks/node_accumulator.h"
+#include <math.h>
+
+int graph_run_tests();
+
+int graph_test_one_add();
+
+int graph_test_circular_runs();
+
+int graph_test_circular_resets();
+
+int graph_test_self_loop();
+
+int graph_test_accumulator();
+
+int graph_test_single_run();
+
+int graph_test_reset_rules();
+
+#endif //COMPUTATION_GRAPH_TESTS_H
diff --git a/quad/computation_graph/test/Makefile b/quad/computation_graph/test/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..4fb6cf11603044352c832c27ce2b2a21edc58147
--- /dev/null
+++ b/quad/computation_graph/test/Makefile
@@ -0,0 +1,12 @@
+# QUAD_ROOT is obtained from environment
+SRC = $(QUAD_ROOT)/computation_graph/src/graph_blocks/*.c $(QUAD_ROOT)/computation_graph/src/computation_graph.c
+INC = $(QUAD_ROOT)/computation_graph/src
+BLOCKS_INC = $(QUAD_ROOT)/computation_graph/src/graph_blocks
+LIB = $(QUAD_ROOT)/lib/test
+
+test_computation_graph: test_computation_graph.c $(SRC)
+ gcc -o test_computation_graph -I. -I$(INC) -I$(BLOCKS_INC) -I$(LIB) $(LIB)/test.o test_computation_graph.c $(SRC) -lm
+
+.PHONY: clean
+clean:
+ rm test_computation_graph
diff --git a/quad/computation_graph/test/test_computation_graph.c b/quad/computation_graph/test/test_computation_graph.c
new file mode 100644
index 0000000000000000000000000000000000000000..9d7c96bbc4c42349978b13d62c7d7036d20b516c
--- /dev/null
+++ b/quad/computation_graph/test/test_computation_graph.c
@@ -0,0 +1,233 @@
+#include "test.h"
+
+
+#include "computation_graph.h"
+#include "graph_blocks/node_add.h"
+#include "graph_blocks/node_mult.h"
+#include "graph_blocks/node_constant.h"
+#include "graph_blocks/node_gain.h"
+#include "graph_blocks/node_accumulator.h"
+
+#define GRAPH_TEST_EPS 0.00001
+
+static int nequal(double val1, double val2) {
+ if (fabs(val1 - val2) < GRAPH_TEST_EPS) {
+ return 0;
+ }
+ return -1;
+}
+
+int graph_test_one_add() {
+ struct computation_graph *graph = create_graph();
+ int block = graph_add_node_add(graph, "Add");
+ int cblock3 = graph_add_node_const(graph, "3");
+ graph_set_param_val(graph, cblock3, CONST_SET, 3);
+ int cblock4 = graph_add_node_const(graph, "4");
+ graph_set_param_val(graph, cblock4, CONST_SET, 4);
+ graph_set_source(graph, block, ADD_SUMMAND1, cblock3, CONST_VAL);
+ graph_set_source(graph, block, ADD_SUMMAND2, cblock4, CONST_VAL);
+ int to_compute_for[1] = {block};
+ graph_compute_nodes(graph, to_compute_for, 1);
+ double result = graph_get_output(graph, block, ADD_SUM);
+ return nequal(result, 7);
+}
+
+
+int graph_test_circular_runs() {
+ struct computation_graph *graph = create_graph();
+ int gain1 = graph_add_node_gain(graph, "gain1");
+ int gain2 = graph_add_node_gain(graph, "gain2");
+ graph_set_source(graph, gain2, GAIN_INPUT, gain1, GAIN_RESULT);
+ graph_set_source(graph, gain1, GAIN_INPUT, gain2, GAIN_RESULT);
+ int to_compute_for[1] = {gain2};
+ graph_compute_nodes(graph, to_compute_for, 1);
+ // If no infinite loop, then success. Value is undefined for circular graphs
+ return 0;
+}
+
+int graph_test_circular_resets() {
+ struct computation_graph *graph = create_graph();
+ int acum1 = graph_add_node_accum(graph, "accumulator1");
+ int acum2 = graph_add_node_accum(graph, "accumulator2");
+ graph_set_source(graph, acum2, ACCUM_IN, acum1, ACCUMULATED);
+ graph_set_source(graph, acum1, ACCUM_IN, acum2, ACCUMULATED);
+ return 0; // Passes if no infinite loop
+}
+
+// Tests the accumulator block, thereby testing reset and state changes
+int graph_test_accumulator() {
+ struct computation_graph *graph = create_graph();
+ int cblock = graph_add_node_const(graph, "const");
+ int acum_b = graph_add_node_accum(graph, "accumulator");
+ graph_set_source(graph, acum_b, ACCUM_IN, cblock, CONST_VAL);
+
+ int to_compute_for[1] = {acum_b};
+ graph_set_param_val(graph, cblock, CONST_SET, 3);
+ graph_compute_nodes(graph, to_compute_for, 1);
+ graph_set_param_val(graph, cblock, CONST_SET, 8);
+ graph_compute_nodes(graph, to_compute_for, 1);
+ graph_set_param_val(graph, cblock, CONST_SET, -2);
+ graph_compute_nodes(graph, to_compute_for, 1);
+
+ double result = graph_get_output(graph, acum_b, ACCUMULATED);
+ if (nequal(result, 9)) {
+ printf("graph_test_accumulator failed on step 1, equals %f\n", result);
+ return -1;
+ }
+
+ // Test reset on source set
+ int gain_b = graph_add_node_gain(graph, "Gain");
+ graph_set_param_val(graph, gain_b, GAIN_GAIN, 1);
+ graph_set_source(graph, gain_b, GAIN_INPUT, acum_b, ACCUMULATED);
+ to_compute_for[0] = gain_b;
+ graph_compute_nodes(graph, to_compute_for, 1);
+ result = graph_get_output(graph, gain_b, GAIN_RESULT);
+ if (nequal(result, -2)) {
+ printf("graph_test_accumulator failed on step 2\n");
+ return -2;
+ }
+ return 0;
+}
+
+// Tests that a block will only execute once per compute,
+// even if its output is connected to multiple inputs
+int graph_test_single_run() {
+ struct computation_graph *graph = create_graph();
+ int acum_b = graph_add_node_accum(graph, "accumulator");
+ int add_block = graph_add_node_add(graph, "Add");
+ int cblock = graph_add_node_const(graph, "const");
+ graph_set_param_val(graph, cblock, CONST_SET, 2);
+
+
+ graph_set_source(graph, acum_b, ACCUM_IN, cblock, CONST_VAL);
+ graph_set_source(graph, add_block, ADD_SUMMAND1, acum_b, ACCUMULATED);
+ graph_set_source(graph, add_block, ADD_SUMMAND2, acum_b, ACCUMULATED);
+
+ int to_compute_for[1] = {add_block};
+ graph_compute_nodes(graph, to_compute_for, 1);
+ double result = graph_get_output(graph, add_block, ADD_SUM);
+ return nequal(result, 4);
+}
+
+// Tests that upon connection of a second child, a block will not reset
+int graph_test_reset_rules() {
+ struct computation_graph *graph = create_graph();
+ int cblock = graph_add_node_const(graph, "5");
+ graph_set_param_val(graph, cblock, CONST_SET, 5);
+ int acum_b = graph_add_node_accum(graph, "accumulator");
+ int gain1 = graph_add_node_gain(graph, "gain1");
+ graph_set_param_val(graph, gain1, GAIN_GAIN, 1);
+
+ graph_set_source(graph, gain1, GAIN_INPUT, acum_b, ACCUMULATED);
+ graph_set_source(graph, acum_b, ACCUM_IN, cblock, CONST_VAL);
+ int to_compute_for[1] = {gain1};
+ graph_compute_nodes(graph, to_compute_for, 1);
+ // state of acum_b is now 5
+
+ int gain2 = graph_add_node_gain(graph, "gain2");
+ graph_set_param_val(graph, gain2, GAIN_GAIN, 1);
+ // Connect gain 2, and accumulator should not get reset
+ graph_set_source(graph, gain2, GAIN_INPUT, acum_b, ACCUMULATED);
+ to_compute_for[0] = gain2;
+ graph_compute_nodes(graph, to_compute_for, 1);
+ double result = graph_get_output(graph, gain2, GAIN_RESULT);
+ // Equals 5 and not 10 because the inputs to the accumulator did not change,
+ // so it didn't run again'
+ return nequal(result, 5);
+}
+
+int graph_test_self_loop() {
+ struct computation_graph *graph = create_graph();
+ int gain1 = graph_add_node_gain(graph, "gain1");
+ graph_set_source(graph, gain1, GAIN_INPUT, gain1, GAIN_RESULT);
+ int to_compute_for[1] = {gain1};
+ graph_compute_nodes(graph, to_compute_for, 1);
+ return 0;
+}
+
+int graph_test_update_rules() {
+ struct computation_graph *graph = create_graph();
+ int cblock = graph_add_node_const(graph, "const");
+ int acum_b = graph_add_node_accum(graph, "accumulator");
+ graph_set_source(graph, acum_b, ACCUM_IN, cblock, CONST_VAL);
+
+ graph_set_param_val(graph, cblock, CONST_SET, 3);
+ int to_compute_for[1] = {acum_b};
+ graph_compute_nodes(graph, to_compute_for, 1);
+ graph_compute_nodes(graph, to_compute_for, 1);
+
+ double result = graph_get_output(graph, acum_b, ACCUMULATED);
+ return nequal(result, 3);
+}
+
+/*
+C1 --->| accum_b1 --->|
+ | Add --->
+C2 --->| accum_b2 --->|
+*/
+int graph_test_update_propagation() {
+ struct computation_graph *graph = create_graph();
+ int cblock1 = graph_add_node_const(graph, "const1");
+ int cblock2 = graph_add_node_const(graph, "const2");
+ int accum_b1 = graph_add_node_accum(graph, "accumulator1");
+ int accum_b2 = graph_add_node_accum(graph, "accumulator2");
+ int add_b = graph_add_node_add(graph, "add");
+ graph_set_source(graph, accum_b1, ACCUM_IN, cblock1, CONST_VAL);
+ graph_set_source(graph, accum_b2, ACCUM_IN, cblock2, CONST_VAL);
+ graph_set_source(graph, add_b, ADD_SUMMAND1, accum_b1, ACCUMULATED);
+ graph_set_source(graph, add_b, ADD_SUMMAND2, accum_b2, ACCUMULATED);
+ graph_set_param_val(graph, cblock1, CONST_SET, 5);
+ graph_set_param_val(graph, cblock2, CONST_SET, 2);
+ int to_compute_for[] = {add_b};
+ graph_compute_nodes(graph, to_compute_for, 1);
+ double result1 = graph_get_output(graph, add_b, ADD_SUM);
+ graph_set_param_val(graph, cblock1, CONST_SET, 1);
+ graph_compute_nodes(graph, to_compute_for, 1);
+ double result2 = graph_get_output(graph, add_b, ADD_SUM);
+
+ return nequal(result1, 7) || nequal(result2, 8);
+}
+
+/*
+Tests for a really subtle edge case
+If a node is disconnected from the computation path, then it will not get computed, even if it is "updated"
+After computation, nodes get their updated flag cleared. It used to clear all nodes, even if they weren't processed (now it only clears if they were processed)
+This caused problems, because if a node had its output to two things, then it wouldn't get marked updated by the reset propagation.
+Since it didn't get marked as "updated" when it got connected to the computation path, and it had its original "updated" flag cleared,
+the node would never get set.
+*/
+int graph_test_update_disconnected() {
+ printf("\n\n---------\n");
+ struct computation_graph *graph = create_graph();
+ int d_block = graph_add_node_const(graph, "const1");
+ int gain_block = graph_add_node_gain(graph, "gain");
+ int gain2_block = graph_add_node_gain(graph, "gain2");
+ int const_b = graph_add_node_const(graph, "const2");
+ graph_set_source(graph, gain_block, GAIN_INPUT, const_b, CONST_VAL);
+ graph_set_source(graph, gain2_block, GAIN_INPUT, d_block, CONST_VAL); // We need this so d_block doesn't get updated
+ graph_set_param_val(graph, gain_block, GAIN_GAIN, 2);
+ graph_set_param_val(graph, d_block, CONST_SET, 1.2345); // Make sure this gets set even if disconnected
+
+ int to_compute_for[] = {gain_block};
+ graph_compute_nodes(graph, to_compute_for, 1);
+ graph_set_source(graph, gain_block, GAIN_INPUT, d_block, CONST_VAL);
+ graph_compute_nodes(graph, to_compute_for, 1);
+
+ double set_val = graph_get_output(graph, gain_block, GAIN_RESULT);
+ return nequal(set_val, 2*1.2345);
+}
+
+int main() {
+ int success = 0;
+ test(graph_test_one_add, "Test adding 2 numbers");
+ test(graph_test_circular_runs, "Test computing cycles");
+ test(graph_test_circular_resets, "Test resetting cycles");
+ test(graph_test_accumulator, "Test accumulator (state)");
+ test(graph_test_single_run, "Test that blocks only get executed once");
+ test(graph_test_reset_rules, "Tests that already connected blocks don't get reset");
+ test(graph_test_self_loop, "Tests that a self-loop computation terminates");
+ test(graph_test_update_rules, "Tests that nodes only update when their inputs change");
+ test(graph_test_update_propagation, "Tests that updates propagate only to their children");
+ test(graph_test_update_disconnected, "Tests that nodes get executed when updated, even if disconnected");
+ test_summary();
+}
diff --git a/quad/scripts/tcp_stress_tests.py b/quad/scripts/tcp_stress_tests.py
new file mode 100644
index 0000000000000000000000000000000000000000..7619a75147ea695fee2b444fc53ba0744c169313
--- /dev/null
+++ b/quad/scripts/tcp_stress_tests.py
@@ -0,0 +1,85 @@
+import socket
+import time
+import uart_stress_tests
+
+TCP_IP = "192.168.1.1"
+TCP_PORT = 8080
+
+send_delay = 0.001
+TEST_ITERS = 100000
+REPORT_STEP = int(TEST_ITERS / 10)
+payload_size = 28
+
+received = []
+def read_n(n_to_read):
+ global received
+ while len(received) < n_to_read:
+ try:
+ just_received = sock.recv(1024)
+ if len(just_received) == 0:
+ print("Socket broken")
+ quit()
+ received.extend(just_received)
+ except:
+ pass
+ to_ret = bytes(received[0:n_to_read])
+ received = received[n_to_read:]
+ return to_ret
+
+def read_packet(raw=False):
+ header = read_n(7)
+ length = int.from_bytes(header[5:7], byteorder='little')
+ data = read_n(length)
+ checksum = read_n(1)
+ if raw:
+ return header + data + checksum
+ else:
+ return data
+
+def get_quad_status():
+ print("Getting quad status...")
+ query_msg = uart_stress_tests.create_msg(2, 0, b'')
+ sock.send(query_msg)
+ resp = read_packet()
+ return uart_stress_tests.parse_query_response(resp)
+ print("Got quad status")
+
+
+if __name__ == '__main__':
+ # connect
+ print("Connecting...")
+ sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ sock.settimeout(2)
+ try:
+ sock.connect((TCP_IP, TCP_PORT))
+ sock.setblocking(True)
+ except:
+ print("Failed to connect")
+ quit()
+ print("connected")
+
+ initial_status = get_quad_status()
+
+ message = uart_stress_tests.create_test_packet(payload_size)
+ overall_start_time = time.perf_counter()
+ for i in range(TEST_ITERS):
+ if i % REPORT_STEP == 0:
+ print("Sent {} messages".format(i))
+ start_time = time.perf_counter()
+ sock.send(message)
+ # Busy waiting
+ # to_sleep_for = send_delay - (time.time() - start_time)
+ # if to_sleep_for > 1e-6:
+ # time.sleep(to_sleep_for)
+ while (time.perf_counter() - start_time <= send_delay):
+ pass
+ # Reporting
+ avg_time = (time.perf_counter() - overall_start_time) / TEST_ITERS
+ print("Average send time was {}".format(avg_time))
+
+ after_status = get_quad_status()
+ diff_messages = after_status[0] - initial_status[0]
+ diff_payload = after_status[1] - initial_status[1]
+ print("Sent {} messages, total payload of {}".format(TEST_ITERS, TEST_ITERS*payload_size))
+ print("Recv {} messages, total payload of {}".format(diff_messages, diff_payload))
+ print("Lost {} messages, {} bytes".format(TEST_ITERS - diff_messages, (TEST_ITERS*payload_size) - diff_payload))
diff --git a/quad/scripts/stress_tests.py b/quad/scripts/uart_stress_tests.py
similarity index 97%
rename from quad/scripts/stress_tests.py
rename to quad/scripts/uart_stress_tests.py
index b038c8af18a2a29245afc4773ef226b2a67dd7b9..ecbb27b246b7732cd514aa2299f7eb575d8b00ce 100755
--- a/quad/scripts/stress_tests.py
+++ b/quad/scripts/uart_stress_tests.py
@@ -14,7 +14,7 @@ def create_msg(msg_type, msg_id, data):
msg += msg_id.to_bytes(2, 'little')
msg += len(data).to_bytes(2, 'little')
msg += data
-
+
checksum = 0
for b in msg:
checksum ^= b
@@ -35,6 +35,13 @@ def read_packet(ser, raw=False):
else:
return data
+def parse_query_response(data):
+ received_str = data[:-1].decode('ascii')
+ if len(received_str) == 0:
+ print("Timed out")
+ return (-1,-1)
+ return tuple(map(int, received_str.split(',')))
+
def query_received(ser):
# Send request
query_msg = create_msg(2, 0, b'')
@@ -42,11 +49,7 @@ def query_received(ser):
ser.flush()
sleep(0.1)
resp = read_packet(ser)
- received_str = resp[:-1].decode('ascii')
- if len(received_str) == 0:
- print("Timed out")
- return (-1,-1)
- return tuple(map(int, received_str.split(',')))
+ return parse_query_response(resp)
def check_test(ser, n_sent, size_sent, old_status):
new_n, new_size = query_received(ser)
@@ -68,7 +71,7 @@ def test_checksum(ser):
computed_checksum = 0
for i in range(len(raw_data) - 1):
computed_checksum ^= raw_data[i]
-
+
valid = computed_checksum == given_checksum
ret_status = query_received(ser)
if not valid:
@@ -124,27 +127,26 @@ def test_get_set(ser):
resp_cntl = resp[0]
resp_cnst = resp[1]
resp_val = struct.unpack('f', resp[2:6])[0]
-
+
if resp_cntl != cntl_id or resp_cnst != const_id or abs(resp_val - to_set) > 1e-5:
- print("Failed get/set test. Expected controller " +
+ print("Failed get/set test. Expected controller " +
str(cntl_id) + ", constant " + str(const_id) + ", value " + str(to_set))
print(" Received " + str(resp_cntl) + ", constant " + str(resp_cnst) + ", value " + str(resp_val))
passed = False
ret_status = query_received(ser)
return passed, ret_status
-
+
if __name__ == '__main__':
with serial.Serial('/dev/ttyUSB0', 921600, timeout=5) as ser:
ser.reset_input_buffer()
while (ser.in_waiting != 0):
ser.read()
status = query_received(ser)
-
+
passed, status = test_partial_packet(ser, status)
passed, status = test_blast(ser, status)
passed, status = test_blast(ser, status, 150, 80)
passed, status = test_bad_checksum(ser, status)
passed, status = test_checksum(ser)
passed, status = test_get_set(ser)
-
diff --git a/quad/sw/modular_quad_pid/.cproject b/quad/sw/modular_quad_pid/.cproject
index c203f31db275e586aceec2167d98420327afe1ed..2a974bb61e6abb6f71e0b057d0beb3c3c2a5b789 100644
--- a/quad/sw/modular_quad_pid/.cproject
+++ b/quad/sw/modular_quad_pid/.cproject
@@ -71,7 +71,7 @@
</toolChain>
</folderInfo>
<sourceEntries>
- <entry excluding="test" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
+ <entry excluding="gen_diagram|test" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
</sourceEntries>
</configuration>
</storageModule>
@@ -147,7 +147,7 @@
</toolChain>
</folderInfo>
<sourceEntries>
- <entry excluding="test" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
+ <entry excluding="gen_diagram|test" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
</sourceEntries>
</configuration>
</storageModule>
diff --git a/quad/sw/modular_quad_pid/gen_diagram/.gitignore b/quad/sw/modular_quad_pid/gen_diagram/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..3309c19da797a4bb0267176e58ce348e67656baf
--- /dev/null
+++ b/quad/sw/modular_quad_pid/gen_diagram/.gitignore
@@ -0,0 +1 @@
+gen_diagram
diff --git a/quad/sw/modular_quad_pid/gen_diagram/Makefile b/quad/sw/modular_quad_pid/gen_diagram/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..9c7cb2a11c2cb0bc77ee5b7b6eb0ce3bf18c3048
--- /dev/null
+++ b/quad/sw/modular_quad_pid/gen_diagram/Makefile
@@ -0,0 +1,9 @@
+
+QUAD_BLOCKS = ../src/graph_blocks
+
+gen_diagram: generate.c ../src/control_algorithm.c ../src/computation_graph.c
+ gcc -o gen_diagram -I. -I../src/ generate.c ../src/computation_graph.c ../src/control_algorithm.c ../src/graph_blocks/*.c -Dread_flap=freadflap
+
+.PHONY: clean
+clean:
+ rm gen_diagram
diff --git a/quad/sw/modular_quad_pid/gen_diagram/README.md b/quad/sw/modular_quad_pid/gen_diagram/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..87b45825befd9e43f373eab8b973ba098e9cd147
--- /dev/null
+++ b/quad/sw/modular_quad_pid/gen_diagram/README.md
@@ -0,0 +1,9 @@
+The gen_diagram project can create an image of the controller network. Below outlines the process of how to use it.
+
+Within the `quad/sw/modular_quad_pid` directory:
+
+1. Within the `gen_diagram` folder, run `make` to build the new code.
+2. Now execute the program with `./gen_diagram`
+3. If you have graphviz installed, you can run `dot -Tpng network.dot -o network.png` to generate a PNG file of the graph.
+
+Enjoy your new diagram!
diff --git a/quad/sw/modular_quad_pid/gen_diagram/gen_diagram b/quad/sw/modular_quad_pid/gen_diagram/gen_diagram
new file mode 100755
index 0000000000000000000000000000000000000000..3babc8f1ab7b00d2bdd2ed28c38a08c8976f1fca
Binary files /dev/null and b/quad/sw/modular_quad_pid/gen_diagram/gen_diagram differ
diff --git a/quad/sw/modular_quad_pid/gen_diagram/generate.c b/quad/sw/modular_quad_pid/gen_diagram/generate.c
new file mode 100644
index 0000000000000000000000000000000000000000..d88d8598b3963b768966c16a46231e7587aa130d
--- /dev/null
+++ b/quad/sw/modular_quad_pid/gen_diagram/generate.c
@@ -0,0 +1,23 @@
+#include <stdio.h>
+
+#include "computation_graph.h"
+#include "graph_blocks/node_pid.h"
+#include "graph_blocks/node_bounds.h"
+#include "graph_blocks/node_constant.h"
+#include "graph_blocks/node_mixer.h"
+#include "PID.h"
+#include "control_algorithm.h"
+
+#define ROLL_PITCH_MAX_ANGLE 0.3490 // 20 degrees
+parameter_t ps;
+
+//int control_algorithm_init(parameter_t * ps);
+int freadflap(int i) {return i;}
+
+int main() {
+ control_algorithm_init(&ps);
+ FILE* dot_fp;
+ dot_fp = fopen("network.dot", "w");
+ export_dot(ps.graph, dot_fp, 0);
+ fclose(dot_fp);
+}
diff --git a/quad/sw/modular_quad_pid/gen_diagram/local_PID.h b/quad/sw/modular_quad_pid/gen_diagram/local_PID.h
new file mode 100644
index 0000000000000000000000000000000000000000..d0ff70e210bcfb8833d123808049291174bf1d01
--- /dev/null
+++ b/quad/sw/modular_quad_pid/gen_diagram/local_PID.h
@@ -0,0 +1,83 @@
+/*
+ * PID.h
+ *
+ * Created on: Nov 10, 2014
+ * Author: ucart
+ */
+
+#ifndef PID_H_
+#define PID_H_
+
+// Yaw constants
+
+// when using units of degrees
+//#define YAW_ANGULAR_VELOCITY_KP 40.0f
+//#define YAW_ANGULAR_VELOCITY_KI 0.0f
+//#define YAW_ANGULAR_VELOCITY_KD 0.0f
+//#define YAW_ANGLE_KP 2.6f
+//#define YAW_ANGLE_KI 0.0f
+//#define YAW_ANGLE_KD 0.0f
+
+// when using units of radians
+#define YAW_ANGULAR_VELOCITY_KP 190.0f * 2292.0f//200.0f * 2292.0f
+#define YAW_ANGULAR_VELOCITY_KI 0.0f
+#define YAW_ANGULAR_VELOCITY_KD 0.0f
+#define YAW_ANGLE_KP 2.6f
+#define YAW_ANGLE_KI 0.0f
+#define YAW_ANGLE_KD 0.0f
+
+// Roll constants
+//#define ROLL_ANGULAR_VELOCITY_KP 0.95f
+//#define ROLL_ANGULAR_VELOCITY_KI 0.0f
+//#define ROLL_ANGULAR_VELOCITY_KD 0.13f//0.4f//0.7f
+//#define ROLL_ANGLE_KP 17.0f //9.0f
+//#define ROLL_ANGLE_KI 0.0f
+//#define ROLL_ANGLE_KD 0.3f // 0.2f
+//#define YPOS_KP 0.0f
+//#define YPOS_KI 0.0f
+//#define YPOS_KD 0.0f
+
+// when using units of radians
+#define ROLL_ANGULAR_VELOCITY_KP 100.0f*46.0f//102.0f*46.0f//9384.0f//204.0f * 46.0f
+#define ROLL_ANGULAR_VELOCITY_KI 0.0f
+#define ROLL_ANGULAR_VELOCITY_KD 100.f*5.5f//102.0f*6.8f//1387.2//204.0f * 6.8f
+#define ROLL_ANGLE_KP 15.0f
+#define ROLL_ANGLE_KI 0.0f
+#define ROLL_ANGLE_KD 0.2f
+#define YPOS_KP 0.015f
+#define YPOS_KI 0.005f
+#define YPOS_KD 0.03f
+
+
+//Pitch constants
+
+// when using units of degrees
+//#define PITCH_ANGULAR_VELOCITY_KP 0.95f
+//#define PITCH_ANGULAR_VELOCITY_KI 0.0f
+//#define PITCH_ANGULAR_VELOCITY_KD 0.13f//0.35f//0.7f
+//#define PITCH_ANGLE_KP 17.0f // 7.2f
+//#define PITCH_ANGLE_KI 0.0f
+//#define PITCH_ANGLE_KD 0.3f //0.3f
+//#define XPOS_KP 40.0f
+//#define XPOS_KI 0.0f
+//#define XPOS_KD 10.0f//0.015f
+
+// when using units of radians
+#define PITCH_ANGULAR_VELOCITY_KP 100.0f*46.0f//101.0f*46.0f//9292.0f//202.0f * 46.0f
+#define PITCH_ANGULAR_VELOCITY_KI 0.0f
+#define PITCH_ANGULAR_VELOCITY_KD 100.0f*5.5f//101.0f*6.8f//1373.6//202.0f * 6.8f
+#define PITCH_ANGLE_KP 15.0f
+#define PITCH_ANGLE_KI 0.0f
+#define PITCH_ANGLE_KD 0.2f
+#define XPOS_KP -0.015f
+#define XPOS_KI -0.005f
+#define XPOS_KD -0.03f
+
+
+//Throttle constants
+#define ALT_ZPOS_KP 9804.0f
+#define ALT_ZPOS_KI 817.0f
+#define ALT_ZPOS_KD 7353.0f
+
+
+#endif /* PID_H_ */
diff --git a/quad/sw/modular_quad_pid/gen_diagram/network.dot b/quad/sw/modular_quad_pid/gen_diagram/network.dot
new file mode 100644
index 0000000000000000000000000000000000000000..516c4a31b125323ad1be1cd32b7e128459789e3f
--- /dev/null
+++ b/quad/sw/modular_quad_pid/gen_diagram/network.dot
@@ -0,0 +1,111 @@
+digraph G {
+rankdir="LR"
+"Roll PID"[shape=record
+label="<f0>Roll PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200]"]
+"Roll" -> "Roll PID":f1 [label="Constant"]
+"RC Roll" -> "Roll PID":f2 [label="Constant"]
+"Ts_angle" -> "Roll PID":f3 [label="Constant"]
+"Pitch PID"[shape=record
+label="<f0>Pitch PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200]"]
+"Pitch" -> "Pitch PID":f1 [label="Constant"]
+"RC Pitch" -> "Pitch PID":f2 [label="Constant"]
+"Ts_angle" -> "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]"]
+"Yaw" -> "Yaw PID":f1 [label="Constant"]
+"Yaw Setpoint" -> "Yaw PID":f2 [label="Constant"]
+"Ts_angle" -> "Yaw PID":f3 [label="Constant"]
+"Roll Rate PID"[shape=record
+label="<f0>Roll Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=4600.000] |<f5> [Ki=0.000] |<f6> [Kd=550.000]"]
+"dPhi" -> "Roll Rate PID":f1 [label="Constant"]
+"Roll PID" -> "Roll Rate PID":f2 [label="Correction"]
+"Ts_angle" -> "Roll Rate PID":f3 [label="Constant"]
+"Pitch Rate PID"[shape=record
+label="<f0>Pitch Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=4600.000] |<f5> [Ki=0.000] |<f6> [Kd=550.000]"]
+"dTheta" -> "Pitch Rate PID":f1 [label="Constant"]
+"Pitch PID" -> "Pitch Rate PID":f2 [label="Correction"]
+"Ts_angle" -> "Pitch Rate PID":f3 [label="Constant"]
+"Yaw Rate PID"[shape=record
+label="<f0>Yaw Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=435480.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000]"]
+"dPsi" -> "Yaw Rate PID":f1 [label="Constant"]
+"RC Yaw" -> "Yaw Rate PID":f2 [label="Constant"]
+"Ts_angle" -> "Yaw Rate PID":f3 [label="Constant"]
+"X pos PID"[shape=record
+label="<f0>X pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-0.015] |<f5> [Ki=-0.005] |<f6> [Kd=-0.030]"]
+"VRPN X" -> "X pos PID":f1 [label="Constant"]
+"X Setpoint" -> "X pos PID":f2 [label="Constant"]
+"Ts_position" -> "X pos PID":f3 [label="Constant"]
+"Y pos PID"[shape=record
+label="<f0>Y pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.015] |<f5> [Ki=0.005] |<f6> [Kd=0.030]"]
+"VRPN Y" -> "Y pos PID":f1 [label="Constant"]
+"Y Setpoint" -> "Y pos PID":f2 [label="Constant"]
+"Ts_position" -> "Y pos PID":f3 [label="Constant"]
+"Altitude PID"[shape=record
+label="<f0>Altitude PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=9804.000] |<f5> [Ki=817.000] |<f6> [Kd=7353.000]"]
+"VRPN Alt" -> "Altitude PID":f1 [label="Constant"]
+"Alt Setpoint" -> "Altitude PID":f2 [label="Constant"]
+"Ts_position" -> "Altitude PID":f3 [label="Constant"]
+"X Setpoint"[shape=record
+label="<f0>X Setpoint |<f1> [Constant=0.000]"]
+"Y Setpoint"[shape=record
+label="<f0>Y Setpoint |<f1> [Constant=0.000]"]
+"Alt Setpoint"[shape=record
+label="<f0>Alt Setpoint |<f1> [Constant=0.000]"]
+"Yaw Setpoint"[shape=record
+label="<f0>Yaw Setpoint |<f1> [Constant=0.000]"]
+"Throttle trim"[shape=record
+label="<f0>Throttle trim |<f1> [Constant=0.000]"]
+"T trim add"[shape=record
+label="<f0>T trim add |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
+"Altitude PID" -> "T trim add":f1 [label="Correction"]
+"Throttle trim" -> "T trim add":f2 [label="Constant"]
+"Pitch"[shape=record
+label="<f0>Pitch |<f1> [Constant=0.000]"]
+"Roll"[shape=record
+label="<f0>Roll |<f1> [Constant=0.000]"]
+"Yaw"[shape=record
+label="<f0>Yaw |<f1> [Constant=0.000]"]
+"dTheta"[shape=record
+label="<f0>dTheta |<f1> [Constant=0.000]"]
+"dPhi"[shape=record
+label="<f0>dPhi |<f1> [Constant=0.000]"]
+"dPsi"[shape=record
+label="<f0>dPsi |<f1> [Constant=0.000]"]
+"P PWM Clamp"[shape=record
+label="<f0>P PWM Clamp |<f1> --\>Bounds in |<f2> [Min=-20000.000] |<f3> [Max=20000.000]"]
+"Pitch Rate PID" -> "P PWM Clamp":f1 [label="Correction"]
+"R PWM Clamp"[shape=record
+label="<f0>R PWM Clamp |<f1> --\>Bounds in |<f2> [Min=-20000.000] |<f3> [Max=20000.000]"]
+"Roll Rate PID" -> "R PWM Clamp":f1 [label="Correction"]
+"Y PWM Clamp"[shape=record
+label="<f0>Y PWM Clamp |<f1> --\>Bounds in |<f2> [Min=-20000.000] |<f3> [Max=20000.000]"]
+"Yaw Rate PID" -> "Y PWM Clamp":f1 [label="Correction"]
+"VRPN X"[shape=record
+label="<f0>VRPN X |<f1> [Constant=0.000]"]
+"VRPN Y"[shape=record
+label="<f0>VRPN Y |<f1> [Constant=0.000]"]
+"VRPN Alt"[shape=record
+label="<f0>VRPN Alt |<f1> [Constant=0.000]"]
+"VRPN Pitch"[shape=record
+label="<f0>VRPN Pitch |<f1> [Constant=0.000]"]
+"VRPN Roll"[shape=record
+label="<f0>VRPN Roll |<f1> [Constant=0.000]"]
+"RC Pitch"[shape=record
+label="<f0>RC Pitch |<f1> [Constant=0.000]"]
+"RC Roll"[shape=record
+label="<f0>RC Roll |<f1> [Constant=0.000]"]
+"RC Yaw"[shape=record
+label="<f0>RC Yaw |<f1> [Constant=0.000]"]
+"RC Throttle"[shape=record
+label="<f0>RC Throttle |<f1> [Constant=0.000]"]
+"Signal Mixer"[shape=record
+label="<f0>Signal Mixer |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4> --\>Yaw"]
+"RC Throttle" -> "Signal Mixer":f1 [label="Constant"]
+"P PWM Clamp" -> "Signal Mixer":f2 [label="Bounded"]
+"R PWM Clamp" -> "Signal Mixer":f3 [label="Bounded"]
+"Y PWM Clamp" -> "Signal Mixer":f4 [label="Bounded"]
+"Ts_angle"[shape=record
+label="<f0>Ts_angle |<f1> [Constant=0.005]"]
+"Ts_position"[shape=record
+label="<f0>Ts_position |<f1> [Constant=0.100]"]
+}
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/gen_diagram/network.png b/quad/sw/modular_quad_pid/gen_diagram/network.png
new file mode 100644
index 0000000000000000000000000000000000000000..47437dd878b0765f9547545d6b4ba736cad20810
Binary files /dev/null and b/quad/sw/modular_quad_pid/gen_diagram/network.png differ
diff --git a/quad/sw/modular_quad_pid/src/PID.h b/quad/sw/modular_quad_pid/src/PID.h
index d0e69ddcc9450d9888b8e7a424e45aabb46b2039..a7701ab2a8a8701343d4e65d5688759d70aba2b9 100644
--- a/quad/sw/modular_quad_pid/src/PID.h
+++ b/quad/sw/modular_quad_pid/src/PID.h
@@ -81,7 +81,4 @@
#define ALT_ZPOS_KI 817.0f
#define ALT_ZPOS_KD 7353.0f
-// Computes control error and correction
-PID_values pid_computation(PID_t *pid);
-
#endif /* PID_H_ */
diff --git a/quad/sw/modular_quad_pid/src/actuator_command_processing.c b/quad/sw/modular_quad_pid/src/actuator_command_processing.c
index 64fce135a7eb95bb5a80962e72d64572dbccf574..e27dbf434d1aba16341a67fc3115f3ea2d88725f 100644
--- a/quad/sw/modular_quad_pid/src/actuator_command_processing.c
+++ b/quad/sw/modular_quad_pid/src/actuator_command_processing.c
@@ -7,6 +7,7 @@
#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)
{
diff --git a/quad/sw/modular_quad_pid/src/callbacks.c b/quad/sw/modular_quad_pid/src/callbacks.c
index 0ae4320d078fd749d269e3147eb211d51177fc25..36f393434b9da690ec03e2532e415055955f1719 100644
--- a/quad/sw/modular_quad_pid/src/callbacks.c
+++ b/quad/sw/modular_quad_pid/src/callbacks.c
@@ -2,6 +2,7 @@
#include "commands.h"
#include "type_def.h"
#include "uart.h"
+#include "computation_graph.h"
/*
* Static variables used to keep track of packet counts
@@ -15,8 +16,17 @@ static size_t total_payload_received = 0;
/**
* Currently does nothing.
*/
-int debug(modular_structs_t *structs)
+int cb_debug(modular_structs_t *structs)
{
+ char buf[255];
+
+ // Get the node ID, parameter ID, parameter value
+ u8 node_id = uart_buff_data_get_u8(0);
+ struct computation_graph* graph = structs->parameter_struct.graph;
+ float param_val = graph_get_output(graph, node_id, 0);
+
+ int length = snprintf(buf, sizeof buf, "%f", param_val);
+ send_data(DEBUG_ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
return 0;
}
@@ -116,34 +126,14 @@ int cb_setparam(modular_structs_t *structs)
{
return -1;
}
+ struct computation_graph* graph = structs->parameter_struct.graph;
- // Get the controller ID, parameter ID, parameter value
- u8 controller_id = uart_buff_data_get_u8(0);
+ // Get the node ID, parameter ID, parameter value
+ u8 node_id = uart_buff_data_get_u8(0);
u8 param_id = uart_buff_data_get_u8(1);
float param_val = uart_buff_data_get_float(2);
- // Check to make sure the IDs are in bounds
- if (controller_id >= MAX_CONTROLLER_ID ||
- param_id >= MAX_CONTROL_PARAM_ID)
- {
- return -1;
- }
-
- // Set the param_val into the controller by controller_id, param_id
- switch(param_id)
- {
- case KP_ID:
- structs->parameter_struct.pid_controllers[controller_id].Kp = param_val;
- break;
- case KI_ID:
- structs->parameter_struct.pid_controllers[controller_id].Ki = param_val;
- break;
- case KD_ID:
- structs->parameter_struct.pid_controllers[controller_id].Kd = param_val;
- break;
- case SP_ID:
- structs->parameter_struct.pid_controllers[controller_id].setpoint = param_val;
- break;
- }
+ // Set the value for that parameter on that node
+ graph_set_param_val(graph, node_id, param_id, param_val);
return 0;
}
@@ -184,39 +174,15 @@ int cb_getparam(modular_structs_t* structs)
}
// Get the controller ID, parameter ID
- u8 controller_id = uart_buff_data_get_u8(0);
+ u8 node_id = uart_buff_data_get_u8(0);
u8 param_id = uart_buff_data_get_u8(1);
- // Check to make sure the IDs are in bounds
- if (controller_id >= MAX_CONTROLLER_ID ||
- param_id >= MAX_CONTROL_PARAM_ID)
- {
- return -1;
- }
-
- // Make the variable to send
- float param_val;
- // Set the param_val equal to the parameter value stored in the controller by
- // controller_id, param_id
- switch(param_id)
- {
- case KP_ID:
- param_val = structs->parameter_struct.pid_controllers[controller_id].Kp;
- break;
- case KI_ID:
- param_val = structs->parameter_struct.pid_controllers[controller_id].Ki;
- break;
- case KD_ID:
- param_val = structs->parameter_struct.pid_controllers[controller_id].Kd;
- break;
- case SP_ID:
- param_val = structs->parameter_struct.pid_controllers[controller_id].setpoint;
- break;
- }
+ struct computation_graph* graph = structs->parameter_struct.graph;
+ float param_val = graph_get_param_val(graph, node_id, param_id);
// Format the response data
char resp_data[6];
// Controller ID
- resp_data[0] = controller_id;
+ resp_data[0] = node_id;
// Parameter ID
resp_data[1] = param_id;
// Parameter value (4 byte float)
diff --git a/quad/sw/modular_quad_pid/src/computation_graph.c b/quad/sw/modular_quad_pid/src/computation_graph.c
new file mode 120000
index 0000000000000000000000000000000000000000..c9f4c52019cf7cf8763b84aa9954a943f37ba2a1
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/computation_graph.c
@@ -0,0 +1 @@
+../../../computation_graph/src/computation_graph.c
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/computation_graph.h b/quad/sw/modular_quad_pid/src/computation_graph.h
new file mode 120000
index 0000000000000000000000000000000000000000..eb8fe7bfea6e123f35220c668690d99d290daa72
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/computation_graph.h
@@ -0,0 +1 @@
+../../../computation_graph/src/computation_graph.h
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/control_algorithm.c b/quad/sw/modular_quad_pid/src/control_algorithm.c
index 202e035b3740c87ec7513938bb853c62ddec5946..e54a4101d4649bde1c488423339ada0672121ac8 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.c
+++ b/quad/sw/modular_quad_pid/src/control_algorithm.c
@@ -6,59 +6,202 @@
*/
// This implemented modular quadrotor software implements a PID control algorithm
-
+
#include "control_algorithm.h"
-#include "communication.h"
+#include "graph_blocks/node_pid.h"
+#include "graph_blocks/node_bounds.h"
+#include "graph_blocks/node_constant.h"
+#include "graph_blocks/node_mixer.h"
+#include "graph_blocks/node_add.h"
+#include "PID.h"
+#include "util.h"
#define ROLL_PITCH_MAX_ANGLE 1.5708 // 90 degrees
#define PWM_DIFF_BOUNDS 30000
- int control_algorithm_init(parameter_t * parameter_struct)
- {
- // HUMAN Piloted (RC) PID DEFINITIONS //////
- // RC PIDs for roll (2 loops: angle --> angular velocity)
- parameter_struct->pid_controllers[ROLL_ID].dt = 0.005; parameter_struct->pid_controllers[ROLL_RATE_ID].dt = 0.005; // 5 ms calculation period
-
- // RC PIDs for pitch (2 loops: angle --> angular velocity)
- parameter_struct->pid_controllers[PITCH_ID].dt = 0.005; parameter_struct->pid_controllers[PITCH_RATE_ID].dt = 0.005; // 5 ms calculation period
-
- // initialize Yaw PID_t and PID constants
- // RC PID for yaw (1 loop angular velocity)
- parameter_struct->pid_controllers[YAW_RATE_ID].dt = 0.005; // 5 ms calculation period
-
- // AUTOMATIC Pilot (Position) PID DEFINITIONS //////
- // Local X PID using a translation from X camera system data to quad local X position (3 loops: local y position --> angle --> angular velocity)
- parameter_struct->pid_controllers[LOCAL_X_ID].dt = 0.100;
-
- // Local Y PID using a translation from Y camera system data to quad local Y position(3 loops: local x position --> angle --> angular velocity)
- parameter_struct->pid_controllers[LOCAL_Y_ID].dt = 0.100;
-
- // CAM PIDs for yaw (2 loops angle --> angular velocity)
- parameter_struct->pid_controllers[YAW_ID].dt = 0.100;
-
- // CAM PID for altitude (1 loop altitude)
- parameter_struct->pid_controllers[ALT_ID].dt = 0.100;
-
- // PID coeffiecients (Position)
- setPIDCoeff(&(parameter_struct->pid_controllers[LOCAL_Y_ID]), YPOS_KP, YPOS_KI, YPOS_KD);
- setPIDCoeff(&(parameter_struct->pid_controllers[LOCAL_X_ID]), XPOS_KP, XPOS_KI, XPOS_KD);
- setPIDCoeff(&(parameter_struct->pid_controllers[ALT_ID]), ALT_ZPOS_KP, ALT_ZPOS_KI, ALT_ZPOS_KD);
-
- // PID coefficients (Angle)
- setPIDCoeff(&(parameter_struct->pid_controllers[PITCH_ID]), PITCH_ANGLE_KP, PITCH_ANGLE_KI, PITCH_ANGLE_KD);
- setPIDCoeff(&(parameter_struct->pid_controllers[ROLL_ID]), ROLL_ANGLE_KP, ROLL_ANGLE_KI, ROLL_ANGLE_KD);
- setPIDCoeff(&(parameter_struct->pid_controllers[YAW_ID]), YAW_ANGLE_KP, YAW_ANGLE_KI, YAW_ANGLE_KD);
-
- // PID coefficients (Angular Velocity)
- setPIDCoeff(&(parameter_struct->pid_controllers[PITCH_RATE_ID]), PITCH_ANGULAR_VELOCITY_KP, PITCH_ANGULAR_VELOCITY_KI, PITCH_ANGULAR_VELOCITY_KD);
- setPIDCoeff(&(parameter_struct->pid_controllers[ROLL_RATE_ID]), ROLL_ANGULAR_VELOCITY_KP, ROLL_ANGULAR_VELOCITY_KI, ROLL_ANGULAR_VELOCITY_KD);
- setPIDCoeff(&(parameter_struct->pid_controllers[YAW_RATE_ID]), YAW_ANGULAR_VELOCITY_KP, YAW_ANGULAR_VELOCITY_KI, YAW_ANGULAR_VELOCITY_KD);
+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->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
+ //graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->yaw_pid, PID_CORRECTION);
+ // TODO: Change this to use VRPN time differences
+ // NOTE: This is being set here because if we set it in the initialization, it is
+ // never marked as "updated", and therefore, doesn't get computed. Yeah, I know, that's bad...
+ graph_set_param_val(graph, ps->pos_time, CONST_SET, 0.1);
+}
+
+void connect_manual(parameter_t* ps) {
+ struct computation_graph* graph = ps->graph;
+ graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->rc_pitch, CONST_VAL);
+ graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->rc_roll, CONST_VAL);
+ graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, CONST_VAL);
+ graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->rc_yaw, CONST_VAL);
+ // TODO: Change this to use LOOP_TIME
+ graph_set_param_val(graph, ps->angle_time, CONST_SET, 0.005);
+}
+
+int control_algorithm_init(parameter_t * ps)
+{
+ struct computation_graph* graph = create_graph();
+ ps->graph = graph;
+
+ // Create all the PID blocks
+ ps->roll_pid = graph_add_node_pid(graph, "Roll PID");
+ ps->pitch_pid = graph_add_node_pid(graph, "Pitch PID");
+ ps->yaw_pid = graph_add_node_pid(graph, "Yaw PID");
+ ps->roll_r_pid = graph_add_node_pid(graph, "Roll Rate PID");
+ ps->pitch_r_pid = graph_add_node_pid(graph, "Pitch Rate PID");
+ ps->yaw_r_pid = graph_add_node_pid(graph, "Yaw Rate PID");
+ ps->x_pos_pid = graph_add_node_pid(graph, "X pos PID");
+ ps->y_pos_pid = graph_add_node_pid(graph, "Y pos PID");
+ ps->alt_pid = graph_add_node_pid(graph, "Altitude PID");
+ ps->x_set = graph_add_node_const(graph, "X Setpoint"); // ID 9
+ ps->y_set = graph_add_node_const(graph, "Y Setpoint");
+ ps->alt_set = graph_add_node_const(graph, "Alt Setpoint");
+ ps->yaw_set = graph_add_node_const(graph, "Yaw Setpoint");
+ ps->throttle_trim = graph_add_node_const(graph, "Throttle trim");
+ ps->throttle_trim_add = graph_add_node_add(graph, "T trim add");
+
+ // Create blocks for sensor inputs
+ ps->cur_pitch = graph_add_node_const(graph, "Pitch"); // ID 20
+ ps->cur_roll = graph_add_node_const(graph, "Roll");
+ ps->cur_yaw = graph_add_node_const(graph, "Yaw");
+ // Yaw angular velocity PID
+ // theta_dot is the angular velocity about the y-axis
+ // phi_dot is the angular velocity about the x-axis
+ // psi_dot is the angular velocity about the z-axis
+ // These are calculated from using the gimbal equations
+ ps->theta_dot = graph_add_node_const(graph, "dTheta");
+ ps->phi_dot = graph_add_node_const(graph, "dPhi");
+ ps->psi_dot = graph_add_node_const(graph, "dPsi");
+ ps->clamp_d_pwmP = graph_add_node_bounds(graph, "P PWM Clamp");
+ ps->clamp_d_pwmR = graph_add_node_bounds(graph, "R PWM Clamp");
+ ps->clamp_d_pwmY = graph_add_node_bounds(graph, "Y PWM Clamp");
+
+ // Create blocks for VRPN data
+ ps->vrpn_x = graph_add_node_const(graph, "VRPN X");
+ ps->vrpn_y = graph_add_node_const(graph, "VRPN Y");
+ ps->vrpn_alt = graph_add_node_const(graph, "VRPN Alt");
+ ps->vrpn_pitch = graph_add_node_const(graph, "VRPN Pitch");
+ ps->vrpn_roll = graph_add_node_const(graph, "VRPN Roll");
+
+ // Create blocks for RC controller
+ ps->rc_pitch = graph_add_node_const(graph, "RC Pitch");
+ ps->rc_roll = graph_add_node_const(graph, "RC Roll");
+ ps->rc_yaw = graph_add_node_const(graph, "RC Yaw");
+ ps->rc_throttle = graph_add_node_const(graph, "RC Throttle");
+
+ ps->mixer = graph_add_node_mixer(graph, "Signal Mixer");
+
+ ps->angle_time = graph_add_node_const(graph, "Ts_angle");
+ ps->pos_time = graph_add_node_const(graph, "Ts_position");
+
+ // Connect pitch PID chain
+ graph_set_source(graph, ps->pitch_r_pid, PID_SETPOINT, ps->pitch_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->pitch_r_pid, PID_CUR_POINT, ps->theta_dot, CONST_VAL);
+ graph_set_source(graph, ps->pitch_r_pid, PID_DT, ps->angle_time, CONST_VAL);
+ //graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->rc_pitch, CONST_VAL);
+ graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->cur_pitch, CONST_VAL);
+ graph_set_source(graph, ps->pitch_pid, PID_DT, ps->angle_time, CONST_VAL);
+
+ // Connect roll PID chain
+ graph_set_source(graph, ps->roll_r_pid, PID_SETPOINT, ps->roll_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->roll_r_pid, PID_CUR_POINT, ps->phi_dot, CONST_VAL);
+ graph_set_source(graph, ps->roll_r_pid, PID_DT, ps->angle_time, CONST_VAL);
+ //graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->rc_roll, CONST_VAL);
+ graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->cur_roll, CONST_VAL);
+ graph_set_source(graph, ps->roll_pid, PID_DT, ps->angle_time, CONST_VAL);
+
+ // Connect yaw PID chain
+ graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->rc_yaw, PID_CORRECTION);
+ graph_set_source(graph, ps->yaw_r_pid, PID_CUR_POINT, ps->psi_dot, CONST_VAL);
+ graph_set_source(graph, ps->yaw_r_pid, PID_DT, ps->angle_time, CONST_VAL);
+
+
+ // X autonomous
+ graph_set_source(graph, ps->x_pos_pid, PID_DT, ps->pos_time, CONST_VAL);
+ graph_set_source(graph, ps->x_pos_pid, PID_CUR_POINT, ps->vrpn_x, CONST_VAL);
+ graph_set_source(graph, ps->x_pos_pid, PID_SETPOINT, ps->x_set, CONST_VAL);
+ // Y autonomous
+ graph_set_source(graph, ps->y_pos_pid, PID_DT, ps->pos_time, CONST_VAL);
+ graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->vrpn_y, CONST_VAL);
+ graph_set_source(graph, ps->y_pos_pid, PID_SETPOINT, ps->y_set, CONST_VAL);
+ // Alt autonomous
+ graph_set_source(graph, ps->alt_pid, PID_DT, ps->pos_time, CONST_VAL);
+ graph_set_source(graph, ps->alt_pid, PID_CUR_POINT, ps->vrpn_alt, CONST_VAL);
+ graph_set_source(graph, ps->alt_pid, PID_SETPOINT, ps->alt_set, CONST_VAL);
+ graph_set_source(graph, ps->alt_set, CONST_VAL, ps->vrpn_alt, CONST_VAL);
+ graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND1, ps->alt_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND2, ps->throttle_trim, CONST_VAL);
+ // Yaw autonomous
+ graph_set_source(graph, ps->yaw_pid, PID_DT, ps->angle_time, CONST_VAL);
+ graph_set_source(graph, ps->yaw_pid, PID_SETPOINT, ps->yaw_set, CONST_VAL);
+ graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->cur_yaw, CONST_VAL);
+
+ // Connect PWM clamping blocks
+ graph_set_source(graph, ps->clamp_d_pwmP, BOUNDS_IN, ps->pitch_r_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->clamp_d_pwmR, BOUNDS_IN, ps->roll_r_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->clamp_d_pwmY, BOUNDS_IN, ps->yaw_r_pid, PID_CORRECTION);
+
+ // Connect signal mixer
+ //graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, CONST_VAL);
+ graph_set_source(graph, ps->mixer, MIXER_PITCH, ps->clamp_d_pwmP, BOUNDS_OUT);
+ graph_set_source(graph, ps->mixer, MIXER_ROLL, ps->clamp_d_pwmR, BOUNDS_OUT);
+ graph_set_source(graph, ps->mixer, MIXER_YAW, ps->clamp_d_pwmY, BOUNDS_OUT);
+
+ // Set pitch PID constants
+ graph_set_param_val(graph, ps->pitch_pid, PID_KP, PITCH_ANGLE_KP);
+ graph_set_param_val(graph, ps->pitch_pid, PID_KI, PITCH_ANGLE_KI);
+ graph_set_param_val(graph, ps->pitch_pid, PID_KD, PITCH_ANGLE_KD);
+ graph_set_param_val(graph, ps->pitch_r_pid, PID_KP, PITCH_ANGULAR_VELOCITY_KP);
+ graph_set_param_val(graph, ps->pitch_r_pid, PID_KI, PITCH_ANGULAR_VELOCITY_KI);
+ graph_set_param_val(graph, ps->pitch_r_pid, PID_KD, PITCH_ANGULAR_VELOCITY_KD);
+
+ // Set roll PID constants
+ graph_set_param_val(graph, ps->roll_pid, PID_KP, ROLL_ANGLE_KP);
+ graph_set_param_val(graph, ps->roll_pid, PID_KI, ROLL_ANGLE_KI);
+ graph_set_param_val(graph, ps->roll_pid, PID_KD, ROLL_ANGLE_KD);
+ graph_set_param_val(graph, ps->roll_r_pid, PID_KP, ROLL_ANGULAR_VELOCITY_KP);
+ graph_set_param_val(graph, ps->roll_r_pid, PID_KI, ROLL_ANGULAR_VELOCITY_KI);
+ graph_set_param_val(graph, ps->roll_r_pid, PID_KD, ROLL_ANGULAR_VELOCITY_KD);
+
+ // Set yaw PID constants
+ graph_set_param_val(graph, ps->yaw_pid, PID_KP, YAW_ANGLE_KP);
+ graph_set_param_val(graph, ps->yaw_pid, PID_KI, YAW_ANGLE_KI);
+ graph_set_param_val(graph, ps->yaw_pid, PID_KD, YAW_ANGLE_KD);
+ graph_set_param_val(graph, ps->yaw_r_pid, PID_KP, YAW_ANGULAR_VELOCITY_KP);
+ graph_set_param_val(graph, ps->yaw_r_pid, PID_KI, YAW_ANGULAR_VELOCITY_KI);
+ graph_set_param_val(graph, ps->yaw_r_pid, PID_KD, YAW_ANGULAR_VELOCITY_KD);
+
+ // Set X/Y/Alt constants
+ graph_set_param_val(graph, ps->x_pos_pid, PID_KP, XPOS_KP);
+ graph_set_param_val(graph, ps->x_pos_pid, PID_KI, XPOS_KI);
+ graph_set_param_val(graph, ps->x_pos_pid, PID_KD, XPOS_KD);
+ graph_set_param_val(graph, ps->y_pos_pid, PID_KP, YPOS_KP);
+ graph_set_param_val(graph, ps->y_pos_pid, PID_KI, YPOS_KI);
+ graph_set_param_val(graph, ps->y_pos_pid, PID_KD, YPOS_KD);
+ graph_set_param_val(graph, ps->alt_pid, PID_KP, ALT_ZPOS_KP);
+ graph_set_param_val(graph, ps->alt_pid, PID_KI, ALT_ZPOS_KI);
+ graph_set_param_val(graph, ps->alt_pid, PID_KD, ALT_ZPOS_KD);
+
+ // Set PWM difference clamping limits
+ graph_set_param_val(graph, ps->clamp_d_pwmP, BOUNDS_MIN, -20000);
+ graph_set_param_val(graph, ps->clamp_d_pwmP, BOUNDS_MAX, 20000);
+ graph_set_param_val(graph, ps->clamp_d_pwmR, BOUNDS_MIN, -20000);
+ graph_set_param_val(graph, ps->clamp_d_pwmR, BOUNDS_MAX, 20000);
+ graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MIN, -20000);
+ graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MAX, 20000);
+
+ // Set initial mode
+ connect_manual(ps);
return 0;
}
-
- int control_algorithm(log_t* log_struct, user_input_t * user_input_struct, sensor_t* sensor_struct, setpoint_t* setpoint_struct, parameter_t* parameter_struct, user_defined_t* user_defined_struct, raw_actuator_t* raw_actuator_struct, modular_structs_t* structs)
+
+ int control_algorithm(log_t* log_struct, user_input_t * user_input_struct, sensor_t* sensor_struct, setpoint_t* setpoint_struct, parameter_t* ps, user_defined_t* user_defined_struct, actuator_command_t* actuator_struct, modular_structs_t* structs)
{
+ struct computation_graph* graph = ps->graph;
// use the 'flap' switch as the flight mode selector
int cur_fm_switch = read_flap(user_input_struct->rc_commands[FLAP]);
static int last_fm_switch = MANUAL_FLIGHT_MODE;
@@ -66,31 +209,19 @@
// reset flight_mode to MANUAL right away if the flap switch is in manual position
// to engage AUTO mode the code waits for a new packet after the flap is switched to auto
// before actually engaging AUTO mode
- if(cur_fm_switch == MANUAL_FLIGHT_MODE)
+ if(cur_fm_switch == MANUAL_FLIGHT_MODE) {
+ if (last_fm_switch == AUTO_FLIGHT_MODE) {
+ connect_manual(ps);
+ }
user_defined_struct->flight_mode = MANUAL_FLIGHT_MODE;
-
- static float roll_trim = 0.0;
- static float pitch_trim = 0.0;
+ }
// flap switch was just toggled to auto flight mode
if((last_fm_switch != cur_fm_switch) && (cur_fm_switch == AUTO_FLIGHT_MODE))
{
user_defined_struct->engaging_auto = 1;
- // Read in trimmed values because it should read trim values right when the pilot flips the flight mode switch
- pitch_trim = user_input_struct->pitch_angle_manual_setpoint; //rc_commands[PITCH] - PITCH_CENTER;
- roll_trim = user_input_struct->roll_angle_manual_setpoint; //rc_commands[ROLL] - ROLL_CENTER;
- //sensor_struct->trimmedRCValues.yaw = yaw_manual_setpoint; //rc_commands[YAW] - YAW_CENTER;
-
-// sensor_struct->trims.roll = raw_actuator_struct->controller_corrected_motor_commands[ROLL];
-// sensor_struct->trims.pitch = raw_actuator_struct->controller_corrected_motor_commands[PITCH];
-// sensor_struct->trims.yaw = raw_actuator_struct->controller_corrected_motor_commands[YAW];
- sensor_struct->trims.throttle = user_input_struct->rc_commands[THROTTLE];
-
- log_struct->trims.roll = sensor_struct->trims.roll;
- log_struct->trims.pitch = sensor_struct->trims.pitch;
- log_struct->trims.yaw = sensor_struct->trims.yaw;
- log_struct->trims.throttle = sensor_struct->trims.throttle;
+ graph_set_param_val(graph, ps->throttle_trim, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
}
if(user_input_struct->locationFresh && user_defined_struct->engaging_auto == 1)
@@ -102,27 +233,17 @@
// also reset the previous error and accumulated error from the position PIDs
if((cur_fm_switch == AUTO_FLIGHT_MODE) && (user_defined_struct->engaging_auto == 2))
{
- // zero out the accumulated error so the I terms don't cause wild things to happen
- parameter_struct->pid_controllers[ALT_ID].acc_error = 0.0;
- parameter_struct->pid_controllers[LOCAL_X_ID].acc_error = 0.0;
- parameter_struct->pid_controllers[LOCAL_Y_ID].acc_error = 0.0;
-
- // make previous error equal to the current so the D term doesn't spike
- parameter_struct->pid_controllers[ALT_ID].prev_error = 0.0;
- parameter_struct->pid_controllers[LOCAL_X_ID].prev_error = 0.0;
- parameter_struct->pid_controllers[LOCAL_Y_ID].prev_error = 0.0;
-
- setpoint_struct->desiredQuadPosition.alt_pos = sensor_struct->currentQuadPosition.alt_pos;
- setpoint_struct->desiredQuadPosition.x_pos = sensor_struct->currentQuadPosition.x_pos;
- setpoint_struct->desiredQuadPosition.y_pos = sensor_struct->currentQuadPosition.y_pos;
- setpoint_struct->desiredQuadPosition.yaw = 0.0;//currentQuadPosition.yaw;
+ graph_set_param_val(graph, ps->x_set, CONST_SET, sensor_struct->currentQuadPosition.x_pos);
+ graph_set_param_val(graph, ps->y_set, CONST_SET, sensor_struct->currentQuadPosition.y_pos);
+ graph_set_param_val(graph, ps->alt_set, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
+ graph_set_param_val(graph, ps->yaw_set, CONST_SET, sensor_struct->currentQuadPosition.yaw);
// reset the flag that engages auto mode
user_defined_struct->engaging_auto = 0;
-
// finally engage the AUTO_FLIGHT_MODE
// this ensures that we've gotten a new update packet right after the switch was set to auto mode
user_defined_struct->flight_mode = AUTO_FLIGHT_MODE;
+ connect_autonomous(ps);
}
//PIDS///////////////////////////////////////////////////////////////////////
@@ -132,203 +253,56 @@
* a pitch or roll for the angle loop PIDs
*/
-// static int counter_between_packets = 0;
-
if(user_input_struct->locationFresh)
{
- parameter_struct->pid_controllers[LOCAL_Y_ID].current_point = sensor_struct->currentQuadPosition.y_pos;
- parameter_struct->pid_controllers[LOCAL_Y_ID].setpoint = setpoint_struct->desiredQuadPosition.y_pos;
-
- parameter_struct->pid_controllers[LOCAL_X_ID].current_point = sensor_struct->currentQuadPosition.x_pos;
- parameter_struct->pid_controllers[LOCAL_X_ID].setpoint = setpoint_struct->desiredQuadPosition.x_pos;
-
- parameter_struct->pid_controllers[ALT_ID].current_point = sensor_struct->currentQuadPosition.alt_pos;
- parameter_struct->pid_controllers[ALT_ID].setpoint = setpoint_struct->desiredQuadPosition.alt_pos;
-
- //logging and PID computation
- log_struct->local_y_PID_values = pid_computation(&(parameter_struct->pid_controllers[LOCAL_Y_ID]));
- log_struct->local_x_PID_values = pid_computation(&(parameter_struct->pid_controllers[LOCAL_X_ID]));
- log_struct->altitude_PID_values = pid_computation(&(parameter_struct->pid_controllers[ALT_ID]));
-
- // yaw angular position PID calculation
- parameter_struct->pid_controllers[YAW_ID].current_point = sensor_struct->currentQuadPosition.yaw;// in radians
- parameter_struct->pid_controllers[YAW_ID].setpoint = setpoint_struct->desiredQuadPosition.yaw; // constant setpoint
-
- //logging and PID computation
- log_struct->angle_yaw_PID_values = pid_computation(&(parameter_struct->pid_controllers[YAW_ID]));
-
+ // VRPN values
+ graph_set_param_val(graph, ps->vrpn_x, CONST_SET, sensor_struct->currentQuadPosition.x_pos);
+ graph_set_param_val(graph, ps->vrpn_y, CONST_SET, sensor_struct->currentQuadPosition.y_pos);
+ graph_set_param_val(graph, ps->vrpn_alt, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
+ graph_set_param_val(graph, ps->vrpn_pitch, CONST_SET, sensor_struct->currentQuadPosition.pitch);
+ graph_set_param_val(graph, ps->vrpn_roll, CONST_SET, sensor_struct->currentQuadPosition.roll);
+ graph_set_param_val(graph, ps->cur_yaw, CONST_SET, sensor_struct->currentQuadPosition.yaw);
}
-
- /* Angle loop
- * Calculates current orientation, and outputs
- * a pitch, roll, or yaw velocity for the angular velocity loop PIDs
- */
-
- //angle boundaries
- if(parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction > ROLL_PITCH_MAX_ANGLE)
- {
- parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction = ROLL_PITCH_MAX_ANGLE;
- }
- if(parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction < -ROLL_PITCH_MAX_ANGLE)
- {
- parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction = -ROLL_PITCH_MAX_ANGLE;
- }
- if(parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction > ROLL_PITCH_MAX_ANGLE)
- {
- parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction = ROLL_PITCH_MAX_ANGLE;
- }
- if(parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction < -ROLL_PITCH_MAX_ANGLE)
- {
- parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction = -ROLL_PITCH_MAX_ANGLE;
- }
-
- parameter_struct->pid_controllers[PITCH_ID].current_point = sensor_struct->pitch_angle_filtered;
- parameter_struct->pid_controllers[PITCH_ID].setpoint =
- (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
- (parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction) + pitch_trim : user_input_struct->pitch_angle_manual_setpoint;
-
- parameter_struct->pid_controllers[ROLL_ID].current_point = sensor_struct->roll_angle_filtered;
- parameter_struct->pid_controllers[ROLL_ID].setpoint =
- (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
- (parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction) + roll_trim : user_input_struct->roll_angle_manual_setpoint;
-
-
- //logging and PID computation
- log_struct->angle_pitch_PID_values = pid_computation(&(parameter_struct->pid_controllers[PITCH_ID]));
- log_struct->angle_roll_PID_values = pid_computation(&(parameter_struct->pid_controllers[ROLL_ID]));
-
-
- /* Angular Velocity Loop
- * Takes the desired angular velocity from the angle loop,
- * and calculates a PID correction with the current angular velocity
- */
-
- // theta_dot is the angular velocity about the y-axis
- // it is calculated from using the gimbal equations
- parameter_struct->pid_controllers[PITCH_RATE_ID].current_point = sensor_struct->theta_dot;
- parameter_struct->pid_controllers[PITCH_RATE_ID].setpoint = parameter_struct->pid_controllers[PITCH_ID].pid_correction;
-
- // phi_dot is the angular velocity about the x-axis
- // it is calculated from using the gimbal equations
- parameter_struct->pid_controllers[ROLL_RATE_ID].current_point = sensor_struct->phi_dot;
- parameter_struct->pid_controllers[ROLL_RATE_ID].setpoint = parameter_struct->pid_controllers[ROLL_ID].pid_correction;
-
- // Yaw angular velocity PID
- // psi_dot is the angular velocity about the z-axis
- // it is calculated from using the gimbal equations
- parameter_struct->pid_controllers[YAW_RATE_ID].current_point = sensor_struct->psi_dot;
- parameter_struct->pid_controllers[YAW_RATE_ID].setpoint = /*(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
- parameter_struct->pid_controllers[YAW_ID].pid_correction :*/ user_input_struct->yaw_manual_setpoint; // no trim added because the controller already works well
-
- //logging and PID computation
- log_struct->ang_vel_pitch_PID_values = pid_computation(&(parameter_struct->pid_controllers[PITCH_RATE_ID]));
- log_struct->ang_vel_roll_PID_values = pid_computation(&(parameter_struct->pid_controllers[ROLL_RATE_ID]));
- log_struct->ang_vel_yaw_PID_values = pid_computation(&(parameter_struct->pid_controllers[YAW_RATE_ID]));
-
- //END PIDs///////////////////////////////////////////////////////////////////////
-
+ // Sensor values
+ graph_set_param_val(graph, ps->cur_pitch, CONST_SET, sensor_struct->pitch_angle_filtered);
+ graph_set_param_val(graph, ps->cur_roll, CONST_SET, sensor_struct->roll_angle_filtered);
+ graph_set_param_val(graph, ps->theta_dot, CONST_SET, sensor_struct->theta_dot);
+ graph_set_param_val(graph, ps->phi_dot, CONST_SET, sensor_struct->phi_dot);
+ graph_set_param_val(graph, ps->psi_dot, CONST_SET, sensor_struct->psi_dot);
+ // RC values
+ graph_set_param_val(graph, ps->rc_pitch, CONST_SET, user_input_struct->pitch_angle_manual_setpoint);
+ graph_set_param_val(graph, ps->rc_roll, CONST_SET, user_input_struct->roll_angle_manual_setpoint);
+ graph_set_param_val(graph, ps->rc_yaw, CONST_SET, user_input_struct->yaw_manual_setpoint);
+ graph_set_param_val(graph, ps->rc_throttle, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
+
+ // Compute VRPN blocks so they can be logged
+ int outputs[4] = {ps->mixer, ps->vrpn_alt, ps->vrpn_pitch, ps->vrpn_roll};
+ graph_compute_nodes(graph, outputs, 4);
// here for now so in case any flight command is not PID controlled, it will default to rc_command value:
- memcpy(raw_actuator_struct->controller_corrected_motor_commands, user_input_struct->rc_commands, sizeof(int) * 6);
+ //memcpy(raw_actuator_struct->controller_corrected_motor_commands, user_input_struct->rc_commands, sizeof(int) * 6);
// don't use the PID corrections if the throttle is less than about 10% of its range
- // NOTE!!!!!!!!!!!!!!!!!!!!!!
- // re-enable the check for AUTO_FLIGHT_MODE when autonomous is fully enabled
- if((user_input_struct->rc_commands[THROTTLE] >
- 118000))// || (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE))
- {
- if(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)
- {
+ // re-enable the check for AUTO_FLIGHT_MODE when autonomous is fully enabled!!!
+ if((user_input_struct->rc_commands[THROTTLE] > 118000))
+ {
//THROTTLE
- //raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] =
- // ((int)(parameter_struct->pid_controllers[ALT_ID].pid_correction)) + sensor_struct->trims.throttle;
-
- //ROLL
- raw_actuator_struct->controller_corrected_motor_commands[ROLL] =
- parameter_struct->pid_controllers[ROLL_RATE_ID].pid_correction; // + sensor_struct->trims.roll;
-
- //PITCH
- raw_actuator_struct->controller_corrected_motor_commands[PITCH] =
- parameter_struct->pid_controllers[PITCH_RATE_ID].pid_correction; // + sensor_struct->trims.pitch;
-
- //YAW
- raw_actuator_struct->controller_corrected_motor_commands[YAW] =
- parameter_struct->pid_controllers[YAW_RATE_ID].pid_correction;// + sensor_struct->trims.yaw;
-
-// static int slow_down = 0;
-// slow_down++;
-// if(slow_down % 50 == 0)
-// printf("X: %.3f\tY: %.3f\tZ: %.3f\tX_s: %.3f\tX_c: %.3f\tY_s: %.3f\tY_c: %.3f\tZ_s: %.3f\tZ_c: %.3f\t\n",
-// parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction,
-// parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction,
-// parameter_struct->pid_controllers[ALT_ID].pid_correction,
-// parameter_struct->pid_controllers[LOCAL_X_ID].setpoint, parameter_struct->pid_controllers[LOCAL_X_ID].current_point,
-// parameter_struct->pid_controllers[LOCAL_Y_ID].setpoint, parameter_struct->pid_controllers[LOCAL_Y_ID].current_point,
-// parameter_struct->pid_controllers[ALT_ID].setpoint, parameter_struct->pid_controllers[ALT_ID].current_point);
- }
- else{
- //ROLL
- raw_actuator_struct->controller_corrected_motor_commands[ROLL] =
- parameter_struct->pid_controllers[ROLL_RATE_ID].pid_correction;
-
- //PITCH
- raw_actuator_struct->controller_corrected_motor_commands[PITCH] =
- parameter_struct->pid_controllers[PITCH_RATE_ID].pid_correction;
-
- //YAW
- raw_actuator_struct->controller_corrected_motor_commands[YAW] =
- parameter_struct->pid_controllers[YAW_RATE_ID].pid_correction;
- }
-
- //BOUNDS CHECKING
- if(raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] < 0)
- raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] = 0;
-
- //BOUNDS CHECKING
- if(raw_actuator_struct->controller_corrected_motor_commands[ROLL] > PWM_DIFF_BOUNDS)
- raw_actuator_struct->controller_corrected_motor_commands[ROLL] = PWM_DIFF_BOUNDS;
-
- if(raw_actuator_struct->controller_corrected_motor_commands[ROLL] < -PWM_DIFF_BOUNDS)
- raw_actuator_struct->controller_corrected_motor_commands[ROLL] = -PWM_DIFF_BOUNDS;
-
- if(raw_actuator_struct->controller_corrected_motor_commands[PITCH] > PWM_DIFF_BOUNDS)
- raw_actuator_struct->controller_corrected_motor_commands[PITCH] = PWM_DIFF_BOUNDS;
-
- if(raw_actuator_struct->controller_corrected_motor_commands[PITCH] < -PWM_DIFF_BOUNDS)
- raw_actuator_struct->controller_corrected_motor_commands[PITCH] = -PWM_DIFF_BOUNDS;
-
- if(raw_actuator_struct->controller_corrected_motor_commands[YAW] > PWM_DIFF_BOUNDS)
- raw_actuator_struct->controller_corrected_motor_commands[YAW] = PWM_DIFF_BOUNDS;
-
- if(raw_actuator_struct->controller_corrected_motor_commands[YAW] < -PWM_DIFF_BOUNDS)
- raw_actuator_struct->controller_corrected_motor_commands[YAW] = -PWM_DIFF_BOUNDS;
+ 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);
}
else
{
- raw_actuator_struct->controller_corrected_motor_commands[ROLL] = 0;
- raw_actuator_struct->controller_corrected_motor_commands[PITCH] = 0;
- raw_actuator_struct->controller_corrected_motor_commands[YAW] = 0;
+ 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];
}
- //logging
- // here we are not actually duplicating the logging from the PID computation
- // the PID computation logs PID_values struct where this logs the PID struct
- // they contain different sets of data
- log_struct->local_y_PID = parameter_struct->pid_controllers[LOCAL_Y_ID];
- log_struct->local_x_PID = parameter_struct->pid_controllers[LOCAL_X_ID];
- log_struct->altitude_PID = parameter_struct->pid_controllers[ALT_ID];
-
- log_struct->angle_roll_PID = parameter_struct->pid_controllers[ROLL_ID];
- log_struct->angle_pitch_PID = parameter_struct->pid_controllers[PITCH_ID];
- log_struct->angle_yaw_PID = parameter_struct->pid_controllers[YAW_ID];
-
- log_struct->ang_vel_roll_PID = parameter_struct->pid_controllers[ROLL_RATE_ID];
- log_struct->ang_vel_pitch_PID = parameter_struct->pid_controllers[PITCH_RATE_ID];
- log_struct->ang_vel_yaw_PID = parameter_struct->pid_controllers[YAW_RATE_ID];
-
last_fm_switch = cur_fm_switch;
// Make location stale now
@@ -336,7 +310,7 @@
return 0;
}
-
+
void setPIDCoeff(PID_t* p, float pValue, float iValue, float dValue) {
p->Kp = pValue;
@@ -344,4 +318,3 @@
p->Kd = dValue;
}
-
diff --git a/quad/sw/modular_quad_pid/src/control_algorithm.h b/quad/sw/modular_quad_pid/src/control_algorithm.h
index 463e0f512ef64f3b3d5841d3ed6396db17830579..8e8a15b79b705fe025c57965b45f8f975eadfb1b 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.h
+++ b/quad/sw/modular_quad_pid/src/control_algorithm.h
@@ -57,7 +57,7 @@ int control_algorithm(log_t* log_struct,
setpoint_t* setpoint_struct,
parameter_t* parameter_struct,
user_defined_t* user_defined_struct,
- raw_actuator_t* raw_actuator_struct,
+ actuator_command_t* actuator_struct,
modular_structs_t* structs);
/**
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_add.c b/quad/sw/modular_quad_pid/src/graph_blocks/node_add.c
new file mode 120000
index 0000000000000000000000000000000000000000..d7025e0c28ea1028022b77e23924a7734ee55e00
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_add.c
@@ -0,0 +1 @@
+../../../../computation_graph/src/graph_blocks/node_add.c
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_add.h b/quad/sw/modular_quad_pid/src/graph_blocks/node_add.h
new file mode 120000
index 0000000000000000000000000000000000000000..4f4b0ea11eb74e5c45426122afb754f13b098266
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_add.h
@@ -0,0 +1 @@
+../../../../computation_graph/src/graph_blocks/node_add.h
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_bounds.c b/quad/sw/modular_quad_pid/src/graph_blocks/node_bounds.c
new file mode 100644
index 0000000000000000000000000000000000000000..aae4a67957a3d76c89d44480962e7b426b94f235
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_bounds.c
@@ -0,0 +1,33 @@
+#include "node_bounds.h"
+#include <stdlib.h>
+
+static void bounds_computation(void *state, const double* params, const double *inputs, double *outputs) {
+ double to_be_bounded = inputs[BOUNDS_IN];
+ if (to_be_bounded < params[BOUNDS_MIN]) {
+ to_be_bounded = params[BOUNDS_MIN];
+ }
+ if (to_be_bounded > params[BOUNDS_MAX]) {
+ to_be_bounded = params[BOUNDS_MAX];
+ }
+ outputs[BOUNDS_OUT] = to_be_bounded;
+}
+
+static void reset_bounds(void *state) {}
+
+static const char* const in_names[1] = {"Bounds in"};
+static const char* const out_names[1] = {"Bounded"};
+static const char* const param_names[2] = {"Min", "Max"};
+const struct graph_node_type node_bounds_type = {
+ .input_names = in_names,
+ .output_names = out_names,
+ .param_names = param_names,
+ .n_inputs = 1,
+ .n_outputs = 1,
+ .n_params = 2,
+ .execute = bounds_computation,
+ .reset = reset_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/sw/modular_quad_pid/src/graph_blocks/node_bounds.h b/quad/sw/modular_quad_pid/src/graph_blocks/node_bounds.h
new file mode 100644
index 0000000000000000000000000000000000000000..516ebc28ac065508e77296c0c2942ab35e9249fb
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_bounds.h
@@ -0,0 +1,22 @@
+#ifndef __NODE_BOUNDS_H__
+#define __NODE_BOUNDS_H__
+#include "../computation_graph.h"
+
+int graph_add_node_bounds(struct computation_graph *graph, const char* name);
+
+extern const struct graph_node_type node_bounds_type;
+
+enum graph_node_bounds_inputs {
+ BOUNDS_IN, // Input to be bounded
+};
+
+enum graph_node_bounds_outputs {
+ BOUNDS_OUT // Bounded output
+};
+
+enum graph_node_bounds_params {
+ BOUNDS_MIN,
+ BOUNDS_MAX
+};
+
+#endif // __NODE_BOUNDS_H__
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c
new file mode 120000
index 0000000000000000000000000000000000000000..5091326ebef1cc7c79bc07820f2069960704809d
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c
@@ -0,0 +1 @@
+../../../../computation_graph/src/graph_blocks/node_constant.c
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h
new file mode 120000
index 0000000000000000000000000000000000000000..d751234bdd8d5b88435e8c5f49c35bc15e55bbf0
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h
@@ -0,0 +1 @@
+../../../../computation_graph/src/graph_blocks/node_constant.h
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_mixer.c b/quad/sw/modular_quad_pid/src/graph_blocks/node_mixer.c
new file mode 100644
index 0000000000000000000000000000000000000000..8b702b360654b3e957d19743abc24b433f58f64d
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_mixer.c
@@ -0,0 +1,42 @@
+#include "node_mixer.h"
+#include <stdlib.h>
+
+static int pwm_min = 100000;
+static int pwm_max = 200000;
+
+static int pwm_clamp(int val) {
+ if (val < pwm_min) {val = pwm_min;}
+ if (val > pwm_max) {val = pwm_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);
+}
+
+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 param_names[0] = {};
+const struct graph_node_type node_mixer_type = {
+ .input_names = in_names,
+ .output_names = out_names,
+ .param_names = param_names,
+ .n_inputs = 4,
+ .n_outputs = 4,
+ .n_params = 0,
+ .execute = mixer_computation,
+ .reset = reset_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/sw/modular_quad_pid/src/graph_blocks/node_mixer.h b/quad/sw/modular_quad_pid/src/graph_blocks/node_mixer.h
new file mode 100644
index 0000000000000000000000000000000000000000..1d58cf54d0531725b8ad2c99195dd23fecbe713f
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_mixer.h
@@ -0,0 +1,23 @@
+#ifndef __NODE_MIXER_H__
+#define __NODE_MIXER_H__
+#include "../computation_graph.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 {
+ MIXER_THROTTLE,
+ MIXER_PITCH,
+ MIXER_ROLL,
+ MIXER_YAW,
+};
+
+enum graph_node_mixer_outputs {
+ MIXER_PWM0,
+ MIXER_PWM1,
+ MIXER_PWM2,
+ MIXER_PWM3,
+};
+
+#endif // __NODE_MIXER_H__
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.c b/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.c
new file mode 100644
index 0000000000000000000000000000000000000000..acf77b5b4051c446067dfaea395a05cf89d1f916
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.c
@@ -0,0 +1,93 @@
+#include "node_pid.h"
+#include <stdlib.h>
+#include <math.h>
+
+static double FLT_EPSILON = 0.0001;
+
+
+struct pid_node_state {
+ double prev_error; // Previous error
+ double acc_error; // Accumulated error
+};
+
+// The generic PID diagram. This function takes in pid inputs (CUR_POINT and SETOINT) and calculates the output "pid_correction"
+// part based on those parameters.
+//
+// + --- error ------------------ P + --- ----------------------------
+// setpoint ---> / sum \ --------->| Kp * error |--------------->/ sum \ -------->| output: "pid_correction" |
+// \ / | ------------------ \ / ----------------------------
+// --- | --- ||
+// - ^ | + ^ ^ + ||
+// | | ------------------------------- | | ------- \/------------
+// | |----->| Ki * accumulated error * dt |----+ | | |
+// | | ------------------------------- I | | SYSTEM |
+// | | | | |
+// | | | --------||------------
+// | | | ||
+// | | ---------------------------------- | ||
+// | |----->| Kd * (error - last error) / dt |----+ ||
+// | ---------------------------------- D ||
+// | ||
+// | -----------\/-----------
+// |____________________________________________________________| Sensor measurements: |
+// | "current point" |
+// ------------------------
+//
+static void pid_computation(void *state, const double* params, const double *inputs, double *outputs) {
+ struct pid_node_state* pid_state = (struct pid_node_state*)state;
+
+ double P = 0.0, I = 0.0, D = 0.0;
+
+ // calculate the current error
+ double error = inputs[PID_SETPOINT] - inputs[PID_CUR_POINT];
+
+ // Accumulate the error (if Ki is less than epsilon, rougly 0,
+ // then reset the accumulated error for safety)
+ if (fabs(params[PID_KI]) <= FLT_EPSILON) {
+ pid_state->acc_error = 0;
+ } else {
+ pid_state->acc_error += error;
+ }
+
+ double change_in_error = error - pid_state->prev_error;
+
+ // Compute each term's contribution
+ P = params[PID_KP] * error;
+ I = params[PID_KI] * pid_state->acc_error * inputs[PID_DT];
+ D = params[PID_KD] * (change_in_error / inputs[PID_DT]);
+
+ pid_state->prev_error = error; // Store the current error into the state
+
+ outputs[PID_CORRECTION] = P + I + D; // Store the computed correction
+}
+
+// This function sets the accumulated error and previous error to 0
+// to prevent previous errors from affecting output after a reset
+static void reset_pid(void *state) {
+ struct pid_node_state* pid_state = (struct pid_node_state*)state;
+ pid_state->acc_error = 0;
+ pid_state->prev_error = 0;
+}
+
+
+static const char* const in_names[3] = {"Cur point", "Setpoint", "dt"};
+static const char* const out_names[1] = {"Correction"};
+static const char* const param_names[3] = {"Kp", "Ki", "Kd"};
+const struct graph_node_type node_pid_type = {
+ .input_names = in_names,
+ .output_names = out_names,
+ .param_names = param_names,
+ .n_inputs = 3,
+ .n_outputs = 1,
+ .n_params = 3,
+ .execute = pid_computation,
+ .reset = reset_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/sw/modular_quad_pid/src/graph_blocks/node_pid.h b/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.h
new file mode 100644
index 0000000000000000000000000000000000000000..75ded4af02edbc2cacbc68035f5a0970d65e38dc
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.h
@@ -0,0 +1,25 @@
+#ifndef __NODE_PID_H__
+#define __NODE_PID_H__
+#include "../computation_graph.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 {
+ PID_CUR_POINT, // Current value of the system
+ PID_SETPOINT, // Desired value of the system
+ PID_DT // sample period
+};
+
+enum graph_node_pid_outputs {
+ PID_CORRECTION // Correction factor computed by the PID
+};
+
+enum graph_node_pid_params {
+ PID_KP, // Proportional constant
+ PID_KI, // Integral constant
+ PID_KD // Derivative constant
+};
+
+#endif // __NODE_PID_H__
diff --git a/quad/sw/modular_quad_pid/src/initialize_components.c b/quad/sw/modular_quad_pid/src/initialize_components.c
index 742270a8e476ac8a203a8fff7f6632126cce534a..9c79db823252d961b57acd621fb7a09d662f1d2d 100644
--- a/quad/sw/modular_quad_pid/src/initialize_components.c
+++ b/quad/sw/modular_quad_pid/src/initialize_components.c
@@ -7,6 +7,7 @@
#include "initialize_components.h"
#include "communication.h"
+#include "sensor.h"
//#define BENCH_TEST
@@ -45,6 +46,9 @@ int initializeAllComponents(user_input_t * user_input_struct, log_t * log_struct
// Initialize the controller
control_algorithm_init(parameter_struct);
+ // Initialize the logging
+ initialize_logging(log_struct, parameter_struct);
+
// Xilinx given initialization
init_platform();
diff --git a/quad/sw/modular_quad_pid/src/log_data.c b/quad/sw/modular_quad_pid/src/log_data.c
index f49808bfe41b08122194e8e41055bb6a7a19733e..b01698b3c3635d1abf9bee7ef4af4c426de7b6ee 100644
--- a/quad/sw/modular_quad_pid/src/log_data.c
+++ b/quad/sw/modular_quad_pid/src/log_data.c
@@ -5,68 +5,108 @@
* Author: ucart
*/
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "PID.h"
+#include "type_def.h"
+#include "uart.h"
+#include "sleep.h"
#include "log_data.h"
#include "communication.h"
+#include "computation_graph.h"
+#include "graph_blocks/node_pid.h"
+#include "graph_blocks/node_constant.h"
+#include "graph_blocks/node_mixer.h"
// Current index of the log array
int arrayIndex = 0;
// Size of the array
int arraySize = LOG_STARTING_SIZE;
-int resized = 0;
-// The number of times we resized the array
-int resizeCount = 0;
+struct graph_tuple { // Tuple for
+ int block_id;
+ int sub_id;
+};
-// Pointer to point to the array with all the logging information
-// for now its not dynamic
-log_t logArray[LOG_STARTING_SIZE * 3];// up to 60 seconds of log
+struct graph_tuple log_outputs[MAX_LOG_NUM];
+struct graph_tuple log_params[MAX_LOG_NUM];
+size_t n_outputs;
+size_t n_params;
-int log_data(log_t* log_struct)
-{
- updateLog(*log_struct);
- return 0;
+float* logArray;
+int row_size;
+
+void addOutputToLog(log_t* log_struct, int controller_id, int output_id) {
+ if (n_outputs < MAX_LOG_NUM) {
+ log_outputs[n_outputs].block_id = controller_id;
+ log_outputs[n_outputs].sub_id = output_id;
+ n_outputs++;
+ }
+}
+void addParamToLog(log_t* log_struct, int controller_id, int param_id) {
+ if (n_params < MAX_LOG_NUM) {
+ log_params[n_params].block_id = controller_id;
+ log_params[n_params].sub_id = param_id;
+ n_params++;
+ }
+}
+
+void initialize_logging(log_t* log_struct, parameter_t* ps) {
+ addOutputToLog(log_struct, ps->alt_pid, PID_CORRECTION);
+ addOutputToLog(log_struct, ps->x_pos_pid, PID_CORRECTION);
+ addOutputToLog(log_struct, ps->y_pos_pid, PID_CORRECTION);
+ addOutputToLog(log_struct, ps->pitch_pid, PID_CORRECTION);
+ addOutputToLog(log_struct, ps->roll_pid, PID_CORRECTION);
+ addOutputToLog(log_struct, ps->yaw_pid, PID_CORRECTION);
+ addOutputToLog(log_struct, ps->pitch_r_pid, PID_CORRECTION);
+ addOutputToLog(log_struct, ps->roll_r_pid, PID_CORRECTION);
+ addOutputToLog(log_struct, ps->yaw_r_pid, PID_CORRECTION);
+ addOutputToLog(log_struct, ps->vrpn_x, CONST_VAL);
+ addOutputToLog(log_struct, ps->vrpn_y, CONST_VAL);
+ addOutputToLog(log_struct, ps->vrpn_alt, CONST_VAL);
+ addOutputToLog(log_struct, ps->vrpn_pitch, CONST_VAL);
+ addOutputToLog(log_struct, ps->vrpn_roll, CONST_VAL);
+ addOutputToLog(log_struct, ps->x_set, CONST_VAL);
+ addOutputToLog(log_struct, ps->y_set, CONST_VAL);
+ addOutputToLog(log_struct, ps->alt_set, CONST_VAL);
+ addOutputToLog(log_struct, ps->mixer, MIXER_PWM0);
+ addOutputToLog(log_struct, ps->mixer, MIXER_PWM1);
+ addOutputToLog(log_struct, ps->mixer, MIXER_PWM2);
+ addOutputToLog(log_struct, ps->mixer, MIXER_PWM3);
+
+ // TODO: Make this not stupid. Adding 6 for IMU and 1 for timestamp
+ row_size = n_outputs + n_params + 6 + 1;
+ logArray = malloc(sizeof(float) * row_size * LOG_STARTING_SIZE);
}
-/**
- * Fills up an xbox hueg amount of memory with log data
- */
-void updateLog(log_t log_struct){
- // If the first iteration, allocate enough memory for "arraySize" elements of logging
-// if(logArray == NULL){
-// // size in memory is 1,720,320 bytes (1.64 megabytes) because logging struct is 420 bytes each
-// // up to 20 seconds of log before resizing
-// logArray = malloc(LOG_STARTING_SIZE * sizeof(log_t));
-// uart0_sendStr("initialized log array.\n");
-// sleep(1);
-// }
-
- // semi dynamic log
-// if((arrayIndex >= arraySize - 1) && (!resized)){
-// realloc(logArray, LOG_STARTING_SIZE * 3 * sizeof(log_t)); // up to 60 seconds of log
-// resized = 1;
-// arraySize = LOG_STARTING_SIZE * 3;
-// uart0_sendStr("resized log array.\n");
-// sleep(1);
-// }
+int log_data(log_t* log_struct, parameter_t* ps)
+{
if(arrayIndex >= arraySize - 1)
{
- return;
+ return 1;
+ }
+ float* thisRow = &logArray[arrayIndex * row_size * sizeof(float)];
+ int offset = 0;
+ thisRow[offset++] = log_struct->time_stamp;
+ thisRow[offset++] = log_struct->gam.accel_x;
+ thisRow[offset++] = log_struct->gam.accel_y;
+ thisRow[offset++] = log_struct->gam.accel_z;
+ thisRow[offset++] = log_struct->gam.gyro_xVel_p;
+ thisRow[offset++] = log_struct->gam.gyro_yVel_q;
+ thisRow[offset++] = log_struct->gam.gyro_zVel_r;
+
+ int i;
+ for (i = 0; i < n_params; i++) {
+ thisRow[offset++] = graph_get_param_val(ps->graph, log_params[i].block_id, log_params[i].sub_id);
+ }
+ for (i = 0; i < n_outputs; i++) {
+ thisRow[offset++] = graph_get_output(ps->graph, log_outputs[i].block_id, log_outputs[i].sub_id);
}
- // Add log to the array
- logArray[arrayIndex++] = log_struct;
-
- // If the index is too big, reallocate memory to double the size as before
-// if(arrayIndex == arraySize){
-// arraySize *= 2;
-// logArray = (log_t *) realloc(logArray, arraySize * sizeof(log_t));
-// ++resizeCount;
-// }
-// else if(arrayIndex > arraySize){
-// // Something fishy has occured
-// xil_printf("Array index is out of bounds. This shouldn't happen but somehow you did the impossible\n\r");
-// }
+ arrayIndex++;
+ return 0;
}
@@ -76,143 +116,54 @@ void updateLog(log_t log_struct){
* TODO: This should probably be transmitting in binary instead of ascii
*/
-void printLogging(){
+void printLogging(log_t* log_struct, parameter_t* ps){
int i;//, j;
- char buf[2304] = {};
- char comments[256] = {};
- char header[1024] = {};
- char units [1024] = {};
-
- sprintf(comments, "# MicroCART On-board Quad Log\r\n# Sample size: %d\r\n", arrayIndex);
- sprintf(header, "%%Time\t" "LoopPeriod\t"
-
- //current points (measurements)
- "X_Current_Position\t" "Y_Current_Position\t" "Z_Current_Position\t"
- "Cam_Meas_Roll\tCam_Meas_Pitch\tCam_Meas_Yaw\t"
- "Quad_Meas_Roll\tQuad_Meas_Pitch\t"
- "roll_velocity\tpitch_velocity\tyaw_velocity\t"
-
- //setpoints
- "X_setpoint\t" "Y_setpoint\t" "Z_setpoint\t"
- "Roll_setpoint\tPitch_setpoint\tYaw_setpoint\t"
- "Roll_vel_setpoint\tPitch_vel_setpoint\tYaw_vel_setpoint\t"
-
- //corrections
- "PID_x\t"
- "PID_y\t"
- "PID_z\t"
- "PID_roll\t"
- "PID_pitch\t"
- "PID_yaw\t"
- "PID_roll_vel\t"
- "PID_pitch_vel\t"
- "PID_yaw_vel\t"
-
- //trims
- "Roll_trim\tPitch_trim\tYaw_trim\tThrottle_trim\t"
-
- //motor commands
- "Motor_0\tMotor_1\tMotor_2\tMotor_3\n"
-
- );
-
-
- sprintf(units, "&sec\tsec\t"
-
- //current points
- "meters\tmeters\tmeters\t"
- "radians\tradians\tradians\t"
- "radians\tradians\t"
- "radians//sec\tradians//sec\tradians//sec\t"
-
- //setpoints
- "meters\tmeters\tmeters\t"
- "radians\tradians\tradians\t"
- "radians//sec\tradians//sec\tradians//sec\t"
-
- //corrections
- "radians\tradians\tradians\t"
- "radians//sec\tradians//sec\tradians//sec\t"
- "none\tnone\tnone\t"
-
- //trims
- "none\tnone\tnone\tnone\t"
-
- //motors
- "none\tnone\tnone\tnone\n"
+ char buf[2048] = {};
+ buf[0] = 0; // Mark buffer as size 0
+ char header1[256] = {};
+ char header2[1024] = {};
+
+ sprintf(header1, "time,accel_x,accel_y,accel_z,gyro_x,gyro_y,gyro_z");
+
+ int end = 0;
+ // Print all the recorded block parameters
+ for (i = 0; i < n_params; i++) {
+ const char* block_name = ps->graph->nodes[log_params[i].block_id].name;
+ const char* output_name = ps->graph->nodes[log_params[i].block_id].type->param_names[log_params[i].sub_id];
+ end += sprintf(&header2[end], ",%s-%s", block_name, output_name);
+ }
+ // Print all the recorded block outputs
+ for (i = 0; i < n_outputs; i++) {
+ const char* block_name = ps->graph->nodes[log_outputs[i].block_id].name;
+ const char* param_name = ps->graph->nodes[log_outputs[i].block_id].type->output_names[log_outputs[i].sub_id];
+ end += sprintf(&header2[end], ",%s-%s", block_name, param_name);
+ }
+ end += sprintf(&header2[end], "\n");
- );
- //strcat(buf,comments);
- strcat(buf,header);
- strcat(buf,units);
+ strcat(buf,header1);
+ strcat(buf,header2);
- send_data(LOG_ID, 0, buf, strlen(buf) + 1);
- //uart0_sendBytes(buf, strlen(buf));
- //usleep(100000);
+ send_data(LOG_ID, 0, buf, strlen(buf));
/*************************/
/* print & send log data */
for(i = 0; i < arrayIndex; i++){
- char* logLine = format(logArray[i]);
- send_data(LOG_ID, 0, logLine, strlen(logLine) + 1);
- free(logLine);
+ int size = format_log(i, log_struct, buf);
+ send_data(LOG_ID, 0, buf, size);
}
}
+int format_log(int idx, log_t* log_struct, char* buf) {
+ int i;
+ int end = 0;
-char* format(log_t log){
- char *retString = malloc(4096*2);
-
- sprintf(retString, "%.3f\t%.4f\t" //Time and TimeSlice
-
- // current points
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
-
- //setpoints
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
-
- //corrections
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
- "%.3f\t%.3f\t%.3f\t"
+ float* row = &logArray[idx * row_size * sizeof(float)];\
- //trims
- "%.2f\t%.2f\t%.2f\t%.2f\t"
-
- //motors
- "%d\t%d\t%d\t%d\n"
-
- ,log.time_stamp, log.time_slice,
-
- // current points
- log.local_x_PID.current_point,log.local_y_PID.current_point, log.altitude_PID.current_point,
- log.currentQuadPosition.roll, log.currentQuadPosition.pitch, log.currentQuadPosition.yaw,
- log.roll_angle_filtered, log.pitch_angle_filtered,
- log.phi_dot, log.theta_dot, log.psi_dot,
-
- //setpoints
- log.local_x_PID.setpoint, log.local_y_PID.setpoint, log.altitude_PID.setpoint,
- log.angle_roll_PID.setpoint, log.angle_pitch_PID.setpoint, log.angle_yaw_PID.setpoint,
- log.ang_vel_roll_PID.setpoint, log.ang_vel_pitch_PID.setpoint, log.ang_vel_pitch_PID.setpoint,
-
- //corrections
- log.local_x_PID_values.pid_correction, log.local_y_PID_values.pid_correction, log.altitude_PID_values.pid_correction,
- log.angle_roll_PID_values.pid_correction, log.angle_pitch_PID_values.pid_correction, log.angle_yaw_PID_values.pid_correction,
- log.ang_vel_roll_PID_values.pid_correction, log.ang_vel_pitch_PID_values.pid_correction, log.ang_vel_yaw_PID_values.pid_correction,
-
- //trims
- log.trims.roll, log.trims.pitch, log.trims.yaw, log.trims.throttle,
-
- //motors
- log.motors[0], log.motors[1], log.motors[2], log.motors[3]
- );
-
-
- return retString;
+ end += sprintf(&buf[end], "%f", row[0]);
+ for (i = 1; i < row_size; i++) {
+ end += sprintf(&buf[end], ",%f", row[i]);
+ }
+ end += sprintf(&buf[end], "\n");
+ return end;
}
diff --git a/quad/sw/modular_quad_pid/src/log_data.h b/quad/sw/modular_quad_pid/src/log_data.h
index 994d043ba18abd76c73e8e95ba7e93f514c941e2..bb3622c931f7b56420175ea872420a39c7e904d7 100644
--- a/quad/sw/modular_quad_pid/src/log_data.h
+++ b/quad/sw/modular_quad_pid/src/log_data.h
@@ -7,16 +7,23 @@
#ifndef LOG_DATA_H_
#define LOG_DATA_H_
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include "PID.h"
#include "type_def.h"
-#include "uart.h"
-#include "sleep.h"
-#define LOG_STARTING_SIZE 4096 //262144 // 2^18 32768 2^15
+#define LOG_STARTING_SIZE 8192 //262144 // 2^18 32768 2^15
+// Maximum number of block outputs you can record, and maximum number of block parameters to record
+#define MAX_LOG_NUM 50
+
+void initialize_logging(log_t* log_struct, parameter_t* ps);
+
+/**
+ * Adds the given block output to the data to be logged
+ */
+void addOutputToLog(log_t* log_struct, int controller_id, int output_id);
+
+/**
+ * Adds the given block parameter to the data to be logged
+ */
+void addParamToLog(log_t* log_struct, int controller_id, int param_id);
/**
@@ -30,7 +37,7 @@
* error message
*
*/
- int log_data(log_t* log_struct);
+ int log_data(log_t* log_struct, parameter_t* ps);
/**
* Fills up an xbox hueg amount of memory with log data
@@ -40,8 +47,11 @@
/**
* Prints all the log information.
*/
- void printLogging();
+ void printLogging(log_t* log_struct, parameter_t* ps);
- char* format(log_t log);
+ /**
+ * Fills the given buffer as ASCII for the recorded index, and returns the length of the string created
+ */
+ int format_log(int idx, log_t* log_struct, char* buf);
#endif /* LOG_DATA_H_ */
diff --git a/quad/sw/modular_quad_pid/src/main.c b/quad/sw/modular_quad_pid/src/main.c
index d703e348dbdeaf14dae299732c0eff36b892767c..64d82b3a997b1619226249b20c626453398659dd 100644
--- a/quad/sw/modular_quad_pid/src/main.c
+++ b/quad/sw/modular_quad_pid/src/main.c
@@ -78,10 +78,10 @@ int main()
// Run the control algorithm
control_algorithm(&(structs.log_struct), &(structs.user_input_struct), &(structs.sensor_struct), &(structs.setpoint_struct),
- &(structs.parameter_struct), &(structs.user_defined_struct), &(structs.raw_actuator_struct), &structs);
+ &(structs.parameter_struct), &(structs.user_defined_struct), &(structs.actuator_command_struct), &structs);
// Process the commands going to the actuators
- actuator_command_processing(&(structs.log_struct), &(structs.user_input_struct), &(structs.raw_actuator_struct), &(structs.actuator_command_struct));
+ //actuator_command_processing(&(structs.log_struct), &(structs.user_input_struct), &(structs.raw_actuator_struct), &(structs.actuator_command_struct));
// send the actuator commands
send_actuator_commands(&(structs.log_struct), &(structs.actuator_command_struct));
@@ -95,7 +95,7 @@ int main()
if(structs.user_defined_struct.flight_mode == AUTO_FLIGHT_MODE)
{
// Log the data collected in this loop
- log_data(&(structs.log_struct));
+ log_data(&(structs.log_struct), &(structs.parameter_struct));
static int loop_counter = 0;
loop_counter++;
@@ -128,11 +128,9 @@ int main()
MIO7_led_off();
- printLogging();
+ printLogging(&(structs.log_struct), &(structs.parameter_struct));
flash_MIO_7_led(10, 100);
return 0;
}
-
-
diff --git a/quad/sw/modular_quad_pid/src/send_actuator_commands.c b/quad/sw/modular_quad_pid/src/send_actuator_commands.c
index 7174348923252cf7109fd1d2df8d08bcef673313..3ef6f1a87140fa572d0118c3fc73b3bdf4fb667b 100644
--- a/quad/sw/modular_quad_pid/src/send_actuator_commands.c
+++ b/quad/sw/modular_quad_pid/src/send_actuator_commands.c
@@ -6,6 +6,7 @@
*/
#include "send_actuator_commands.h"
+#include "util.h"
int send_actuator_commands(log_t* log_struct, actuator_command_t* actuator_command_struct)
{
@@ -14,11 +15,6 @@ int send_actuator_commands(log_t* log_struct, actuator_command_t* actuator_comma
for (i = 0; i < 4; i++)
pwm_write_channel(actuator_command_struct->pwms[i], i);
- log_struct->motors[0] = actuator_command_struct->pwms[0];
- log_struct->motors[1] = actuator_command_struct->pwms[1];
- log_struct->motors[2] = actuator_command_struct->pwms[2];
- log_struct->motors[3] = actuator_command_struct->pwms[3];
-
return 0;
}
diff --git a/quad/sw/modular_quad_pid/src/sensor_processing.c b/quad/sw/modular_quad_pid/src/sensor_processing.c
index 5fd682c3fae6a0bf0039932e24a20dc6eff27120..410b9219f8c905e16ab07fbfa0e41958bdd2ba53 100644
--- a/quad/sw/modular_quad_pid/src/sensor_processing.c
+++ b/quad/sw/modular_quad_pid/src/sensor_processing.c
@@ -5,6 +5,11 @@
* Author: ucart
*/
+#include <stdio.h>
+#include "log_data.h"
+#include "sensor.h"
+#include "conversion.h"
+#include "quadposition.h"
#include "sensor_processing.h"
#include "timer.h"
#include <math.h>
@@ -91,14 +96,6 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
// uart0_sendStr(dMsg);
// loop_counter = 0;
// }
-
- //logging
- log_struct->currentQuadPosition = sensor_struct->currentQuadPosition;
- log_struct->roll_angle_filtered = sensor_struct->roll_angle_filtered;
- log_struct->pitch_angle_filtered = sensor_struct->pitch_angle_filtered;
- log_struct->phi_dot = sensor_struct->phi_dot;
- log_struct->theta_dot = sensor_struct->theta_dot;
- log_struct->psi_dot = sensor_struct->psi_dot;
return 0;
}
diff --git a/quad/sw/modular_quad_pid/src/sensor_processing.h b/quad/sw/modular_quad_pid/src/sensor_processing.h
index cf3fd122b33168782cccbb7dd1edc012ca3c2107..04b5efebe48ab20bdecfcfaab11e1ffe2f63a540 100644
--- a/quad/sw/modular_quad_pid/src/sensor_processing.h
+++ b/quad/sw/modular_quad_pid/src/sensor_processing.h
@@ -7,12 +7,7 @@
#ifndef SENSOR_PROCESSING_H_
#define SENSOR_PROCESSING_H_
-
-#include <stdio.h>
-#include "log_data.h"
-#include "sensor.h"
-#include "conversion.h"
-#include "quadposition.h"
+#include "type_def.h"
/*
* Aero channel declaration
*/
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index c6f89b496581f5241b2bfa392d50f1fbda319fd5..2157f3b0319920c779c31c40e6c9db4bea68341e 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/quad/sw/modular_quad_pid/src/type_def.h
@@ -10,6 +10,7 @@
#include <stdint.h>
#include "commands.h"
+#include "computation_graph.h"
/**
* @brief
@@ -168,6 +169,8 @@ typedef struct log_t {
int packetId;
gam_t gam; // Raw and calculated gyro, accel, and mag values are all in gam_t
+
+ /*
float phi_dot, theta_dot, psi_dot; // gimbal equation values
quadPosition_t currentQuadPosition;
@@ -193,7 +196,7 @@ typedef struct log_t {
quadTrims_t trims;
int motors[4];
-
+ */
} log_t;
/**
@@ -287,7 +290,51 @@ typedef struct setpoint_t {
*
*/
typedef struct parameter_t {
- PID_t pid_controllers[MAX_CONTROLLER_ID];
+ struct computation_graph* graph;
+ // PID blocks
+ int roll_pid;
+ int pitch_pid;
+ int yaw_pid;
+ int roll_r_pid;
+ int pitch_r_pid;
+ int yaw_r_pid;
+ int x_pos_pid;
+ int y_pos_pid;
+ int alt_pid;
+ // Sensor blocks
+ int cur_pitch;
+ int cur_roll;
+ int cur_yaw;
+ int theta_dot;
+ int phi_dot;
+ int psi_dot;
+ // VRPN blocks
+ int vrpn_x;
+ int vrpn_y;
+ int vrpn_alt;
+ int vrpn_pitch, vrpn_roll;
+ // RC blocks
+ int rc_pitch;
+ int rc_roll;
+ int rc_yaw;
+ int rc_throttle;
+ // Desired positions
+ int x_set;
+ int y_set;
+ int alt_set;
+ int yaw_set;
+ // Clamps
+ int clamp_d_pwmP;
+ int clamp_d_pwmR;
+ int clamp_d_pwmY;
+ // Loop times
+ int angle_time;
+ int pos_time;
+ // Signal mixer
+ int mixer;
+ // "trim" for autonomous
+ int throttle_trim;
+ int throttle_trim_add;
} parameter_t;
/**
diff --git a/quad/sw/modular_quad_pid/src/user_input.c b/quad/sw/modular_quad_pid/src/user_input.c
index bc0c4c5b7047afa4cdecf6093e48a6b977bf9a15..b3b4331e200aad0dfc88c87684e7d28b06def909 100644
--- a/quad/sw/modular_quad_pid/src/user_input.c
+++ b/quad/sw/modular_quad_pid/src/user_input.c
@@ -7,17 +7,13 @@
#include "user_input.h"
#include "uart.h"
+#include "controllers.h"
int get_user_input(log_t* log_struct, user_input_t* user_input_struct)
{
// Read in values from RC Receiver
read_rec_all(user_input_struct->rc_commands);
- log_struct->commands.pitch = user_input_struct->rc_commands[PITCH];
- log_struct->commands.roll = user_input_struct->rc_commands[ROLL];
- log_struct->commands.throttle = user_input_struct->rc_commands[THROTTLE];
- log_struct->commands.yaw = user_input_struct->rc_commands[YAW];
-
//create setpoints for manual flight
// currently in units of radians
user_input_struct->yaw_manual_setpoint = convert_from_receiver_cmd(user_input_struct->rc_commands[YAW], YAW_MAX, YAW_CENTER, YAW_MIN, YAW_RAD_TARGET, -(YAW_RAD_TARGET));
diff --git a/quad/sw/modular_quad_pid/src/util.c b/quad/sw/modular_quad_pid/src/util.c
index c7dd33a1ca988a454de416311fca18fcfbc5fe1c..be07d38187f621dc63405b031eb5ff3131d81a5b 100644
--- a/quad/sw/modular_quad_pid/src/util.c
+++ b/quad/sw/modular_quad_pid/src/util.c
@@ -7,6 +7,18 @@
#include "util.h"
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+#include <xgpiops.h>
+#include "PID.h"
+#include "log_data.h"
+#include <sleep.h>
+#include "controllers.h"
+#include "xparameters.h"
+#include "uart.h"
+
extern int motor0_bias, motor1_bias, motor2_bias, motor3_bias;
//Global variable representing the current pulseW
int pulseW = pulse_throttle_low;
diff --git a/quad/sw/modular_quad_pid/src/util.h b/quad/sw/modular_quad_pid/src/util.h
index 285db6085f1804ebca654ec969958adf3abd9955..7c864f4d6ef165749649293280f4630805b89045 100644
--- a/quad/sw/modular_quad_pid/src/util.h
+++ b/quad/sw/modular_quad_pid/src/util.h
@@ -7,17 +7,7 @@
#ifndef _UTIL_H
#define _UTIL_H
-#include <stdlib.h>
-#include <stdio.h>
-#include <string.h>
-#include <math.h>
-#include <xgpiops.h>
-#include "PID.h"
-#include "log_data.h"
-#include <sleep.h>
-#include "controllers.h"
-#include "xparameters.h"
-#include "uart.h"
+#include "type_def.h"
#define clock_rate 100000000
diff --git a/wifi_bridge/test_scripts/serial-tcp.py b/wifi_bridge/test_scripts/serial-tcp.py
new file mode 100644
index 0000000000000000000000000000000000000000..9e81d02bdc521cadd204e472a7fb5737320975c5
--- /dev/null
+++ b/wifi_bridge/test_scripts/serial-tcp.py
@@ -0,0 +1,62 @@
+import socket
+import serial
+
+TCP_IP = "192.168.1.1"
+TCP_PORT = 8080
+
+msg_size = 1024
+message = bytes(i % 256 for i in range(msg_size))
+dropped = True
+
+ser = serial.Serial('COM6', 921600, timeout=0.01)
+ser.reset_input_buffer()
+if ser.in_waiting:
+ print("that doesn't work")
+
+while True:
+ if dropped:
+ attempts = 0
+ while attempts < 5:
+ print("Trying to connect")
+ sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ sock.settimeout(2)
+ try:
+ sock.connect((TCP_IP, TCP_PORT))
+ dropped = False
+ break
+ except:
+ attempts += 1
+ if dropped:
+ print("Failed to connect")
+ break
+ print("connected")
+
+ try:
+ print("Sending {} bytes".format(len(message)))
+ ser.write(message)
+ except Exception as e:
+ print("Failed to send all data")
+ continue
+
+ received = []
+ while len(received) < msg_size:
+ try:
+ just_received = sock.recv(msg_size - len(received))
+ except Exception as e:
+ print("Exception when receiving: ", e)
+ break
+ if len(just_received) == 0:
+ print("Your socket broke")
+ break
+ received.extend(just_received)
+
+ if len(received) != msg_size:
+ print("\tError: Received {} bytes".format(len(received)))
+ elif bytes(received) != message:
+ print("\tError: Received data does not match")
+ else:
+ print("\tYou're a winner!")
+
+ debug_msg = ser.read(4096)
+ if len(debug_msg) != 0:
+ print(debug_msg.decode())
diff --git a/wifi_bridge/test_scripts/serial-tcp_nowait.py b/wifi_bridge/test_scripts/serial-tcp_nowait.py
new file mode 100644
index 0000000000000000000000000000000000000000..d2279c3998790b5fcb8121e585c9d514633dd3ef
--- /dev/null
+++ b/wifi_bridge/test_scripts/serial-tcp_nowait.py
@@ -0,0 +1,74 @@
+import socket
+import serial
+import time
+
+TCP_IP = "192.168.1.1"
+TCP_PORT = 8080
+
+msg_size = 500
+print_interval = 100
+message = bytes(i % 256 for i in range(msg_size))
+dropped = True
+loop_time = 0.005
+
+ser = serial.Serial('COM6', 921600, timeout=0.01)
+ser.reset_input_buffer()
+
+sent_cnt = 0
+recvd_cnt = 0
+outer_start_time = time.perf_counter()
+received = []
+while True:
+ if dropped:
+ attempts = 0
+ while attempts < 5:
+ print("Trying to connect")
+ sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ sock.settimeout(2)
+ try:
+ sock.connect((TCP_IP, TCP_PORT))
+ dropped = False
+ break
+ except:
+ attempts += 1
+ if dropped:
+ print("Failed to connect")
+ break
+ print("connected")
+
+ start_time = time.perf_counter()
+ try:
+ ser.write(message)
+ sent_cnt += 1
+ except Exception as e:
+ print("Failed to send all data")
+ continue
+
+ # while len(received) < msg_size:
+ try:
+ amt_in_air = (sent_cnt - recvd_cnt) * msg_size - len(received)
+ just_received = sock.recv(1024)
+ received.extend(just_received)
+ if len(just_received) == 0:
+ print("Your socket broke")
+ break
+ except Exception as e:
+ print("Exception when receiving: ", e)
+ break
+ if len(received) >= msg_size:
+ recvd_packet = received[:msg_size]
+ received = received[msg_size:]
+ recvd_cnt += 1
+ if bytes(recvd_packet) != message:
+ print("Received data does not match")
+ if sent_cnt % print_interval == 0:
+ print("Sent {}, received {}".format(sent_cnt, recvd_cnt))
+ print("Average send time was {}".format((time.perf_counter() - outer_start_time)/print_interval))
+ outer_start_time = time.perf_counter()
+
+ while time.perf_counter() - start_time <= loop_time:
+ pass
+
+ #debug_msg = ser.read(4096)
+ #if len(debug_msg) != 0:
+ # print(debug_msg.decode())
diff --git a/wifi_bridge/test_scripts/tcp-serial.py b/wifi_bridge/test_scripts/tcp-serial.py
new file mode 100644
index 0000000000000000000000000000000000000000..8c2713096d1e56dd4f56cd509e05be132db7c338
--- /dev/null
+++ b/wifi_bridge/test_scripts/tcp-serial.py
@@ -0,0 +1,46 @@
+import socket
+import serial
+
+TCP_IP = "192.168.1.1"
+TCP_PORT = 8080
+
+msg_size = 1024*100
+message = bytes(i % 256 for i in range(msg_size))
+dropped = True
+
+ser = serial.Serial('COM6', 921600, timeout=5)
+ser.reset_input_buffer()
+if ser.in_waiting:
+ print("that doesn't work")
+
+while True:
+ if dropped:
+ attempts = 0
+ while attempts < 5:
+ print("Trying to connect")
+ sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ sock.settimeout(2)
+ try:
+ sock.connect((TCP_IP, TCP_PORT))
+ dropped = False
+ break
+ except:
+ attempts += 1
+ if dropped:
+ print("Failed to connect")
+ break
+ print("connected")
+
+ try:
+ print("Sending {} bytes".format(len(message)))
+ sock.sendall(message)
+ except Exception as e:
+ print("Failed to send all data")
+ continue
+ received = ser.read(msg_size)
+ if len(received) != msg_size:
+ print("\tError: Received {} bytes".format(len(received)))
+ elif received != message:
+ print("\tError: Received data does not match")
+ else:
+ print("\tYou're a winner!")
diff --git a/wifi_bridge/test_scripts/tcp_timer.py b/wifi_bridge/test_scripts/tcp_timer.py
new file mode 100644
index 0000000000000000000000000000000000000000..78664a980b15574af94a883fd654dfd71d85696c
--- /dev/null
+++ b/wifi_bridge/test_scripts/tcp_timer.py
@@ -0,0 +1,65 @@
+import socket
+import time
+import csv
+
+TCP_IP = "192.168.1.1"
+# UDP_IP = "127.0.0.1"
+TCP_PORT = 8080
+
+# sock.bind(('', UDP_PORT))
+
+message = bytes(range(36))
+times_full = []
+times_network = []
+times = [0.0]*100
+dropped = True
+response = bytes("initial", 'ASCII')
+addr = "initial"
+recvd_data = []
+for i in range(100):
+ if dropped:
+ sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
+ sock.settimeout(2)
+ sock.connect((TCP_IP, TCP_PORT))
+ dropped = False
+ send_msg = list(message)
+ send_msg[0] = i
+ send_msg = bytes(send_msg)
+
+ send_time = time.perf_counter()
+ times[i] = send_time
+ sock.send(send_msg)
+ try:
+ response = sock.recv(1024)
+ recvd_data.extend(response)
+ except:
+ print("timed out")
+ dropped = True
+ if len(recvd_data) >= 36:
+ end_time = time.perf_counter()
+ response = bytes(recvd_data[0:36])
+ recvd_data = recvd_data[36:]
+ msg_id = int(response[0])
+ latency = end_time - times[msg_id]
+ # serial_time = int.from_bytes(response[0:4], byteorder='big') / 1000
+ serial_time = 0
+ # times_full.append(1000 * (end_time - send_time) - 0)
+ times_full.append(1000 * latency)
+ times_network.append(1000 * (end_time - send_time) - serial_time)
+ print("received " + str(response) + " in " + str(times_full[-1]) + " from " + str(addr))
+ while time.perf_counter() - send_time < 0.01:
+ pass
+
+# with open("tcp_dist.csv", 'w', newline='') as f:
+# writer = csv.writer(f)
+# for time in times_network:
+# time = time + 2.6
+# writer.writerow([time])
+
+for time in [times_full, times_network]:
+ print("lowest: " + str(min(time)))
+ print("highest: " + str(max(time)))
+ print("median: " + str(sorted(time)[int(len(time) / 2)]))
+ print("average; " + str(sum(time) / len(time)))
+