Merge branch 'groundStation-dev-comp_graph-commands' of...

Merge branch 'groundStation-dev-comp_graph-commands' of https://git.ece.iastate.edu/danc/MicroCART_17-18 into groundStation-dev-comp_graph-commands

.gitmodules

 [submodule "groundStation/src/vrpn"]
 	path = groundStation/src/vrpn
 	url = https://github.com/vrpn/vrpn
+	ignore = dirty

controls/model/.gitignore

+slprj/
+test_model_grt_rtw/
+test_model_sfun.mexw64
\ No newline at end of file

groundStation/src/backend/backend.c

@@ -341,28 +341,48 @@ void printVrpnData(struct ucart_vrpn_TrackerData * td) {
 
 int connectToZybo() {
 	int sock;
-	int status = 0;
+	int status = -1;
 
 	if (getenv(NOQUAD_ENV)) {
 		return 0;
 	}
 
-	/* Use bluetooth by default */
-	if (!getenv(QUAD_WIFI_ENV)) {
-		printf("Using BT Settings\n");
-		struct sockaddr_rc addr;
+	/* Use wifi by default */
+	if (getenv(QUAD_COMM_ENV)) {
+		/* Determine if we are using bluetooth or local */
+		if(strcmp(getenv(QUAD_COMM_ENV), "local") == 0) {
+			printf("Using Local Socket Settings\n");
+			
+			struct sockaddr_un remote;
+			char str[100];
+
+			if ((sock = socket(AF_UNIX, SOCK_STREAM, 0)) == -1) {
+				perror("socket");
+				exit(1);
+			}
 
-		// allocate a socket	
-		sock = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
+			remote.sun_family = AF_UNIX;
+			char * sock_env = getenv(QUAD_LOCAL_SOCKET);
+			strcpy(remote.sun_path, sock_env ? sock_env : QUAD_DEFAULT_LOCAL_SOCKET);
+			printf("Attempting to connect to local socket at '%s'. please be patiend.\n", remote.sun_path);
 
-		//set the connection params ie. who to connect to	
-		addr.rc_family = AF_BLUETOOTH;
-		addr.rc_channel = (uint8_t) QUAD_BT_CHANNEL;
-		str2ba( QUAD_BT_ADDR, &addr.rc_bdaddr );
-		
-		printf("Attempting to connect to zybo. Please be patient...\n");
-		// blocking call to connect to socket sock ie. zybo board
-		status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
+			status = connect(sock, (struct sockaddr *)&remote, sizeof(remote));
+		} else if (strcmp(getenv(QUAD_COMM_ENV), "bluetooth") == 0) {
+			printf("Using BT Settings\n");
+			struct sockaddr_rc addr;
+
+			// allocate a socket	
+			sock = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
+
+			//set the connection params ie. who to connect to	
+			addr.rc_family = AF_BLUETOOTH;
+			addr.rc_channel = (uint8_t) QUAD_BT_CHANNEL;
+			str2ba( QUAD_BT_ADDR, &addr.rc_bdaddr );
+			
+			printf("Attempting to connect to zybo. Please be patient...\n");
+			// blocking call to connect to socket sock ie. zybo board
+			status = connect(sock, (struct sockaddr *)&addr, sizeof(addr));
+		}
 	} else {
 		printf("Using WIFI settings\n");
 		struct sockaddr_in addr;

groundStation/src/backend/config.h

@@ -11,7 +11,10 @@
 // If you are planning on using any of these env vars and you have
 // 	exported them with normal user rights. You will need to run the 
 // 	backend with sudo elevation and with the --preserve-env flag or -E
-#define QUAD_WIFI_ENV "UCART_USE_WIFI"
+#define QUAD_COMM_ENV "UCART_COMM_CHANNEL"
+
+#define QUAD_DEFAULT_LOCAL_SOCKET "./virtquad.socket"
+#define QUAD_LOCAL_SOCKET "UCART_LOCAL_SOCKET"
 #define QUAD_IP_ENV "UCART_QUAD_IP"
 #define QUAD_IP_DEFAULT "192.168.1.1"
 #define QUAD_PORT_ENV "UCART_QUAD_PORT"

quad/Makefile

@@ -14,6 +14,7 @@ libs:
 	$(MAKE) -C src/test
 	$(MAKE) -C src/queue
 	$(MAKE) -C src/computation_graph
+	$(MAKE) -C src/graph_blocks
 	$(MAKE) -C src/commands
 	$(MAKE) -C src/quad_app
 
@@ -38,6 +39,7 @@ deep-clean:
 	$(MAKE) -C src/test clean
 	$(MAKE) -C src/queue clean
 	$(MAKE) -C src/computation_graph clean
+	$(MAKE) -C src/graph_blocks clean
 	$(MAKE) -C src/commands clean
 	$(MAKE) -C src/quad_app clean
 	bash scripts/clean_xsdk_workspace.sh

quad/src/commands/commands.c

@@ -143,14 +143,14 @@ struct MessageType MessageTypes[MAX_TYPE_ID] =
 		// Function pointer
 		&cb_log
 	},
-	// RESPONSE
+	// LOG_END
 	{
 		// Command text
-		"response",
+		"logend",
 		// Type of the command data
 		stringType,
 		// Function pointer
-		&cb_response
+		&cb_logend
 	},
 	// SETPARAM
 	{
@@ -259,15 +259,6 @@ struct MessageType MessageTypes[MAX_TYPE_ID] =
 		floatType,
 		// Function pointer
 		&cb_respaddnode
-	},
-	// LOG_END
-	{
-		// Command text
-		"logend",
-		// Type of the command data
-		stringType,
-		// Function pointer
-		&cb_logend
 	}
 };
 

quad/src/computation_graph/Makefile

 TOP=../..
 
 NAME = computation_graph
-REQLIBS = -ltest -lm
+REQLIBS = -ltest -lm -lgraph_blocks
 
 include $(TOP)/library.mk

quad/src/computation_graph/computation_graph.h

@@ -25,14 +25,16 @@ struct computation_graph {
 
 // 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;
+    const char* const* input_names; // Array of strings corresponding to the inputs
+    const char* const* output_names; // Array of strings corresponding to the outputs
+    const char* const* param_names; // Array of strings corresponding to the parameters
+    int n_inputs; // Number of inputs
+    int n_outputs; // Number of outputs
+    int n_params; // Number of parameters
+    execute_node_t execute; // Function describing how to produce outputs
+    reset_node_t reset; // Reset this node. Called upon (re)connection
+    size_t state_size; // Size of the state struct for this type
+    int type_id; // A unique ID for this node type
 };
 
 // Holds a tuple for defining the source of a node. Includes the node ID and its output ID

quad/src/computation_graph/test/test_computation_graph.c

@@ -2,11 +2,7 @@
 
 
 #include "computation_graph.h"
-#include "node_add.h"
-#include "node_mult.h"
-#include "node_constant.h"
-#include "node_gain.h"
-#include "node_accumulator.h"
+#include "graph_blocks.h"
 
 #define GRAPH_TEST_EPS 0.00001
 
@@ -19,10 +15,10 @@ static int nequal(double val1, double val2) {
 
 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");
+    int block = graph_add_defined_block(graph, BLOCK_ADD, "Add");
+    int cblock3 = graph_add_defined_block(graph, BLOCK_CONSTANT, "3");
     graph_set_param_val(graph, cblock3, CONST_SET, 3);
-    int cblock4 = graph_add_node_const(graph, "4");
+    int cblock4 = graph_add_defined_block(graph, BLOCK_CONSTANT, "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);
@@ -35,8 +31,8 @@ int graph_test_one_add() {
 
 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");
+    int gain1 = graph_add_defined_block(graph, BLOCK_GAIN, "gain1");
+    int gain2 = graph_add_defined_block(graph, BLOCK_GAIN, "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};
@@ -47,8 +43,8 @@ int graph_test_circular_runs() {
 
 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");
+    int acum1 = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator1");
+    int acum2 = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "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
@@ -57,8 +53,8 @@ int graph_test_circular_resets() {
 // 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");
+    int cblock = graph_add_defined_block(graph, BLOCK_CONSTANT, "const");
+    int acum_b = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator");
     graph_set_source(graph, acum_b, ACCUM_IN, cblock, CONST_VAL);
 
     int to_compute_for[1] = {acum_b};
@@ -76,7 +72,7 @@ int graph_test_accumulator() {
     }
 
     // Test reset on source set
-    int gain_b = graph_add_node_gain(graph, "Gain");
+    int gain_b = graph_add_defined_block(graph, BLOCK_GAIN, "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;
@@ -93,9 +89,9 @@ int graph_test_accumulator() {
 // 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");
+    int acum_b = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator");
+    int add_block = graph_add_defined_block(graph, BLOCK_ADD, "Add");
+    int cblock = graph_add_defined_block(graph, BLOCK_CONSTANT, "const");
     graph_set_param_val(graph, cblock, CONST_SET, 2);
 
 
@@ -112,10 +108,10 @@ int graph_test_single_run() {
 // 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");
+    int cblock = graph_add_defined_block(graph, BLOCK_CONSTANT, "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");
+    int acum_b = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator");
+    int gain1 = graph_add_defined_block(graph, BLOCK_GAIN, "gain1");
     graph_set_param_val(graph, gain1, GAIN_GAIN, 1);
 
     graph_set_source(graph, gain1, GAIN_INPUT, acum_b, ACCUMULATED);
@@ -124,7 +120,7 @@ int graph_test_reset_rules() {
     graph_compute_nodes(graph, to_compute_for, 1);
     // state of acum_b is now 5
 
-    int gain2 = graph_add_node_gain(graph, "gain2");
+    int gain2 = graph_add_defined_block(graph, BLOCK_GAIN, "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);
@@ -138,7 +134,7 @@ int graph_test_reset_rules() {
 
 int graph_test_self_loop() {
     struct computation_graph *graph = create_graph();
-    int gain1 = graph_add_node_gain(graph, "gain1");
+    int gain1 = graph_add_defined_block(graph, BLOCK_GAIN, "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);
@@ -147,8 +143,8 @@ int graph_test_self_loop() {
 
 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");
+    int cblock = graph_add_defined_block(graph, BLOCK_CONSTANT, "const");
+    int acum_b = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator");
     graph_set_source(graph, acum_b, ACCUM_IN, cblock, CONST_VAL);
 
     graph_set_param_val(graph, cblock, CONST_SET, 3);
@@ -167,11 +163,11 @@ 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");
+    int cblock1 = graph_add_defined_block(graph, BLOCK_CONSTANT, "const1");
+    int cblock2 = graph_add_defined_block(graph, BLOCK_CONSTANT, "const2");
+    int accum_b1 = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator1");
+    int accum_b2 = graph_add_defined_block(graph, BLOCK_ACCUMULATE, "accumulator2");
+    int add_b = graph_add_defined_block(graph, BLOCK_ADD, "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);
@@ -199,10 +195,10 @@ 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");
+    int d_block = graph_add_defined_block(graph, BLOCK_CONSTANT, "const1");
+    int gain_block = graph_add_defined_block(graph, BLOCK_GAIN, "gain");
+    int gain2_block = graph_add_defined_block(graph, BLOCK_GAIN, "gain2");
+    int const_b = graph_add_defined_block(graph, BLOCK_CONSTANT, "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);
@@ -219,8 +215,8 @@ int graph_test_update_disconnected() {
 
 int graph_test_get_source() {
     struct computation_graph *graph = create_graph();
-    int add_block = graph_add_node_add(graph, "Add");
-    int cblock3 = graph_add_node_const(graph, "3");
+    int add_block = graph_add_defined_block(graph, BLOCK_ADD, "Add");
+    int cblock3 = graph_add_defined_block(graph, BLOCK_CONSTANT, "3");
     graph_set_source(graph, add_block, ADD_SUMMAND1, cblock3, CONST_VAL);
 
     struct node_src source = graph_get_source(graph, add_block, ADD_SUMMAND1);
@@ -234,7 +230,7 @@ int graph_test_get_source() {
 
 int graph_test_get_source_null() {
     struct computation_graph *graph = create_graph();
-    int add_block = graph_add_node_add(graph, "Add");
+    int add_block = graph_add_defined_block(graph, BLOCK_ADD, "Add");
 
     struct node_src source = graph_get_source(graph, 123, ADD_SUMMAND1);
     if (source.controller_id != -1) {

quad/src/graph_blocks/.gitignore

+obj/

quad/src/graph_blocks/Makefile

+TOP=../..
+
+NAME = graph_blocks
+REQLIBS = -lcomputation_graph
+
+include $(TOP)/library.mk

quad/src/graph_blocks/graph_blocks.c

+#include "graph_blocks.h"
+#include <stdlib.h>
+
+// See graph_blocks.h
+const struct graph_node_type* blockDefs[MAX_BLOCK_TYPES] = 
+{
+    &node_const_type,
+    &node_add_type,
+    &node_mult_type,
+    &node_gain_type,
+    &node_accum_type,
+    &node_bounds_type,
+    &node_mixer_type,
+    &node_pid_type
+};
+
+
+int graph_add_defined_block(struct computation_graph* graph, int type_id, const char* name) {
+    // Verify block type is valid
+    if (type_id >= MAX_BLOCK_TYPES) {
+        return -1;
+    }
+    const struct graph_node_type *block_type = blockDefs[type_id];
+    void* state = NULL;
+    // Allocate the state struct for this node, if necessary
+    if (block_type->state_size) {
+        state = malloc(block_type->state_size);
+        if (!state) {return -1;} // Check for malloc failure
+    }
+
+    // Use the computation graph implementation's add node function
+    return graph_add_node(graph, name, block_type, state);
+}
\ No newline at end of file

quad/src/graph_blocks/graph_blocks.h

+#ifndef __GRAPH_BLOCKS_H__
+#define __GRAPH_BLOCKS_H__
+
+#include "computation_graph.h"
+#include "node_constant.h"
+#include "node_add.h"
+#include "node_mult.h"
+#include "node_gain.h"
+#include "node_accumulator.h"
+#include "node_bounds.h"
+#include "node_mixer.h"
+#include "node_pid.h"
+
+/*
+ * ---------- How-To ------------
+ * To add a new block type, put the implementation (.c and .h) files
+ * in the same directory as this file, and include the header file above
+ * Add a new entry to this enum right before MAX_BLOCK_TYPES
+ * 
+ * In graph_blocks.c, add a new entry at the end of the array with
+ * your graph_node_type struct.
+ */
+
+
+
+/*
+ * Enumerates all the types of different block types.
+ * Must match blockDefs
+ */
+enum BlockTypes {
+    BLOCK_CONSTANT,      // 00
+    BLOCK_ADD,           // 01
+    BLOCK_MULT,          // 02
+    BLOCK_GAIN,          // 03
+    BLOCK_ACCUMULATE,    // 04
+    BLOCK_BOUNDS,        // 05
+    BLOCK_MIXER,         // 07
+    BLOCK_PID,           // 08
+    //                        <-- Insert new block type here
+    MAX_BLOCK_TYPES
+};
+
+/*
+ * Array corresponding to the different block type structs
+ * Mustm match the nums in BlockTypes
+ */
+extern const struct graph_node_type* blockDefs[MAX_BLOCK_TYPES];
+
+
+/*
+ * Creates a new node and adds it to the graph with the given type ID and name
+ * Returns the id of the new node upon success, -1 upon failure
+ */
+int graph_add_defined_block(struct computation_graph* graph, int type_id, const char* name);
+#endif // __GRAPH_BLOCKS_H__

quad/src/computation_graph/node_accumulator.c → quad/src/graph_blocks/node_accumulator.c

@@ -27,7 +27,9 @@ const struct graph_node_type node_accum_type = {
         .n_outputs = 1,
         .n_params = 0,
         .execute = accum_nodes,
-        .reset = reset
+        .reset = reset,
+        .state_size = sizeof(struct accum_state),
+        .type_id = BLOCK_ACCUMULATE
 };
 
 int graph_add_node_accum(struct computation_graph *graph, const char* name) {

quad/src/computation_graph/node_accumulator.h → quad/src/graph_blocks/node_accumulator.h

 #ifndef __NODE_ACCUMULATOR_H__
 #define __NODE_ACCUMULATOR_H__
 #include "computation_graph.h"
+#include "graph_blocks.h"
 
 int graph_add_node_accum(struct computation_graph *graph, const char* name);
 

quad/src/computation_graph/node_add.c → quad/src/graph_blocks/node_add.c

@@ -17,7 +17,9 @@ const struct graph_node_type node_add_type = {
         .n_outputs = 1,
         .n_params = 0,
         .execute = add_nodes,
-        .reset = reset
+        .reset = reset,
+        .state_size = 0,
+        .type_id = BLOCK_ADD
 };
 
 int graph_add_node_add(struct computation_graph *graph, const char* name) {

quad/src/computation_graph/node_add.h → quad/src/graph_blocks/node_add.h

 #ifndef __NODE_ADD_H__
 #define __NODE_ADD_H__
 #include "computation_graph.h"
+#include "graph_blocks.h"
 
 int graph_add_node_add(struct computation_graph *graph, const char* name);
 

quad/src/quad_app/node_bounds.c → quad/src/graph_blocks/node_bounds.c

@@ -25,7 +25,9 @@ const struct graph_node_type node_bounds_type = {
         .n_outputs = 1,
         .n_params = 2,
         .execute = bounds_computation,
-        .reset = reset_bounds
+        .reset = reset_bounds,
+        .state_size = 0,
+        .type_id = BLOCK_BOUNDS
 };
 
 int graph_add_node_bounds(struct computation_graph *graph, const char* name) {

quad/src/quad_app/node_bounds.h → quad/src/graph_blocks/node_bounds.h

 #ifndef __NODE_BOUNDS_H__
 #define __NODE_BOUNDS_H__
 #include "computation_graph.h"
+#include "graph_blocks.h"
 
 int graph_add_node_bounds(struct computation_graph *graph, const char* name);