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

quad/src/computation_graph/computation_graph.c

@@ -81,6 +81,14 @@ int graph_set_source(struct computation_graph *graph,
     return 0;
 }
 
+struct node_src graph_get_source(struct computation_graph *graph, int node_id, int input_id) {
+    if (node_id >= graph->n_nodes || node_id < 0 ||
+        input_id >= graph->nodes[node_id].type->n_inputs || input_id < 0) {
+        return (struct node_src) {.controller_id = -1, .controller_output = -1};
+    }
+    return graph->nodes[node_id].input_srcs[input_id];
+}
+
 int graph_add_node(struct computation_graph *graph,
                    const char* name,
                    const struct graph_node_type *type,
@@ -104,7 +112,7 @@ int graph_add_node(struct computation_graph *graph,
     new_node->updated = 1;
     new_node->output_values = malloc(type->n_outputs * sizeof(double));
     new_node->param_values = calloc(type->n_params, sizeof(double));
-    new_node->input_srcs = malloc(type->n_inputs * sizeof(struct input_type));
+    new_node->input_srcs = malloc(type->n_inputs * sizeof(struct node_src));
     // Check that malloc succeeded in every case which memory was requested
     if ((type->n_outputs && !new_node->output_values) ||
         (type->n_params && !new_node->param_values) ||

quad/src/computation_graph/computation_graph.h

@@ -35,6 +35,12 @@ struct graph_node_type {
     reset_node_t reset;
 };
 
+// Holds a tuple for defining the source of a node. Includes the node ID and its output ID
+struct node_src { 
+    int controller_id;
+    int controller_output;
+};
+
 // Declares an instance of a node
 struct graph_node {
     const char *name; // Name of this node instance
@@ -44,10 +50,7 @@ struct graph_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;
+    struct node_src *input_srcs; // Array of tuples indicating the source for each input to this node
     int updated; // 1 if this node has had an input or parameter change
 };
 
@@ -68,6 +71,12 @@ struct computation_graph *create_graph();
  */
 int graph_set_source(struct computation_graph *graph, int dest_node_id, int dest_input, int src_node_id, int src_output);
 
+/*
+ * Returns the source node/output pair of a node's input. The mirror of graph_set_source.
+ * Returns a node ID of -1 if the requested node or input does not exist
+ */
+struct node_src graph_get_source(struct computation_graph *graph, int node_id, int input_id);
+
 /*
  * Creates a new node with the given data, and adds it to the graph.
  * Returns a negative integer upon failure.

quad/src/computation_graph/test/test_computation_graph.c

@@ -217,8 +217,35 @@ int graph_test_update_disconnected() {
     return nequal(set_val, 2*1.2345);
 }
 
+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");
+    graph_set_source(graph, add_block, ADD_SUMMAND1, cblock3, CONST_VAL);
+
+    struct node_src source = graph_get_source(graph, add_block, ADD_SUMMAND1);
+    if (source.controller_id != cblock3 || source.controller_output != CONST_VAL) {
+        return 1;
+    }
+    else {
+        return 0;
+    }
+}
+
+int graph_test_get_source_null() {
+    struct computation_graph *graph = create_graph();
+    int add_block = graph_add_node_add(graph, "Add");
+
+    struct node_src source = graph_get_source(graph, 123, ADD_SUMMAND1);
+    if (source.controller_id != -1) {
+        return 1;
+    }
+    else {
+        return 0;
+    }
+}
+
 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");
@@ -229,5 +256,7 @@ int main() {
     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(graph_test_get_source, "Tests that the get_source call works normally");
+    test(graph_test_get_source_null, "Tests that the get_source call returns ID -1 when invalid ID is passed");
     return test_summary();
 }

quad/src/quad_app/callbacks.c

@@ -99,41 +99,55 @@ int cb_beginupdate(modular_structs_t *structs, metadata_t *meta, u8 *data, u16 l
 	return 0;
 }
 
+/*
+ * -----------------------------------------------
+ * Callbacks for control network modification/info
+ * -----------------------------------------------
+*/
+
+// Struct for holding a node ID and either a parameter, output, or input index
+struct node_ids {
+	int16_t id;
+	int16_t sub_id;
+};
 
-/* Callbacks for configuration */
+/*
+ * Given a data array, returns a node_ids struct retrieved from the array.
+ * Assumes the given array is at least 4 bytes to hold the data.
+*/
+struct node_ids get_node_ids(u8 *data) {
+	return (struct node_ids) {
+		.id = build_short(data),
+		.sub_id = build_short(data + 2)
+	};
+}
 
 /**
-  * Handles a command to set a controller parameter on the quad.
+  * Handles a command to set a node parameter on the quad.
   *
   * NOTE:
   * Expects the uart buff to have data in the following format:
   * |--------------------------------------------------------|
-  * |  data index ||      0      |      1      |    2 - 5    |
+  * |  data index ||    0 - 1    |    2 - 3    |    4 - 7    |
   * |--------------------------------------------------------|
-  * |   parameter ||  control ID | ctrl parmID |  param val  |
+  * |   parameter ||   node ID   | node parmID |  param val  |
   * |--------------------------------------------------------|
-  * |       bytes ||      1      |      1      |      4      |
+  * |       bytes ||      2      |      2      |      4      |
   * |--------------------------------------------------------|
   * 
   * Does not send anything in response.
   */
 int cb_setparam(modular_structs_t *structs, metadata_t *meta, u8 *data, u16 length)
 {
-	// Get some of the meta data
-	u16 data_len = length;
 	// Check if the data length is correct
-	if (data_len != 6)
-	{
-		return -1;
-	}
+	if (length != 8) {return -1;}
 	struct computation_graph* graph = structs->parameter_struct.graph;
 
 	// Get the node ID, parameter ID, parameter value
-	u8 node_id = data[0];
-	u8 param_id = data[1];
-	float param_val = build_float(data + 2);
+	struct node_ids ids = get_node_ids(data);
+	float param_val = build_float(data + 4);
 	// Set the value for that parameter on that node
-	graph_set_param_val(graph, node_id, param_id, param_val);
+	graph_set_param_val(graph, ids.id, ids.sub_id, param_val);
 
 	return 0;
 }
@@ -143,54 +157,156 @@ int cb_setparam(modular_structs_t *structs, metadata_t *meta, u8 *data, u16 leng
   *
   * NOTE:
   * Expects the uart buff to have data in the following format:
-  * |------------------------------------------|
-  * |  data index ||      0      |      1      |
-  * |------------------------------------------|
-  * |   parameter ||  control ID | ctrl parmID |
-  * |------------------------------------------|
-  * |       bytes ||      1      |      1      |
-  * |------------------------------------------|
+  * |-------------------------------------------|
+  * |  data index ||    0 - 1     |    2 - 3    |
+  * |-------------------------------------------|
+  * |   parameter ||    node ID   | node parmID |
+  * |-------------------------------------------|
+  * |       bytes ||       2      |      2      |
+  * |-------------------------------------------|
   *
-  * Sends a response of type RESPONSECONTROL_ID.
+  * Sends a response of type RESPPARAM_ID.
   * The response will have a message ID equal to the one originally received.
   * The data of the response will be in the following format:
   * |--------------------------------------------------------|
-  * |  data index ||      0      |      1      |    2 - 5    |
+  * |  data index ||    0 - 1    |    2 - 3    |    4 - 7    |
   * |--------------------------------------------------------|
-  * |   parameter ||  control ID | ctrl parmID |  param val  |
+  * |   parameter ||   node ID   | node parmID |  param val  |
   * |--------------------------------------------------------|
-  * |       bytes ||      1      |      1      |      4      |
+  * |       bytes ||      2      |      2      |      4      |
   * |--------------------------------------------------------|
   */
 int cb_getparam(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 length)
 {
-	// Get some of the meta data
-	u16 data_len = length;
-	u16 msg_id = meta->msg_id;
 	// Check if the data length is correct
-	if (data_len != 2)
-	{
-		return -1;
-	}
+	if (length != 8) {return -1;}
+	u16 msg_id = meta->msg_id;
 
 	// Get the controller ID, parameter ID
-	u8 node_id = data[0];
-	u8 param_id = data[1];
+	struct node_ids ids = get_node_ids(data);
 	struct computation_graph* graph = structs->parameter_struct.graph;
-	float param_val = graph_get_param_val(graph, node_id, param_id);
+	float param_val = graph_get_param_val(graph, ids.id, ids.sub_id);
 
 	// Format the response data
-	char resp_data[6];
+	char resp_data[8];
 	// Controller ID
-	resp_data[0] = node_id;
+	pack_short(ids.id, resp_data);
 	// Parameter ID
-	resp_data[1] = param_id;
+	pack_short(ids.sub_id, resp_data + 2);
 	// Parameter value (4 byte float)
-	// TODO set a strict byte ordering for communication between the ground station and the quad
-	memcpy(&resp_data[2], &param_val, sizeof(param_val));
+	pack_float(param_val, resp_data + 4);
 
 	// Send the response
 	send_data(&structs->hardware_struct.uart, RESPPARAM_ID, msg_id, resp_data, sizeof(resp_data));
 
 	return 0;
 }
+
+/**
+  * Handles a command to set a node's input source
+  *
+  * NOTE:
+  * Expects the uart buff to have data in the following format:
+  * |---------------------------------------------------------------------------|
+  * |  data index ||     0 - 1    |     2 - 3     |    4 - 5    |     6 - 7     |
+  * |---------------------------------------------------------------------------|
+  * |   parameter || dest node ID | dest input ID | src node ID | src output ID |
+  * |---------------------------------------------------------------------------|
+  * |       bytes ||       2      |       2       |      2      |       2       |
+  * |---------------------------------------------------------------------------|
+  * 
+  * Does not send anything in response.
+  */
+int cb_setsource(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 length) {
+	if (length != 8) {return -1;}
+	int16_t dest_node = build_short(data);
+	int16_t dest_input = build_short(data + 2);
+	int16_t src_node = build_short(data + 4);
+	int16_t src_input = build_short(data + 6);
+
+	struct computation_graph* graph = structs->parameter_struct.graph;
+	graph_set_source(graph, dest_node, dest_input, src_node, src_input);
+
+	return 0;
+}
+
+/**
+  * Handles a command to get the source of a node's input
+  *
+  * NOTE:
+  * Expects the uart buff to have data in the following format:
+  * |---------------------------------------------|
+  * |  data index ||    0 - 1     |     2 - 3     |
+  * |---------------------------------------------|
+  * |   parameter ||    node ID   | node input ID |
+  * |---------------------------------------------|
+  * |       bytes ||       2      |       2       |
+  * |---------------------------------------------|
+  *
+  * Sends a response of type RESPSOURCE_ID.
+  * The response will have a message ID equal to the one originally received.
+  * The data of the response will be in the following format:
+  * |---------------------------------------------------------------------------|
+  * |  data index ||     0 - 1    |     2 - 3     |    4 - 5    |     6 - 7     |
+  * |---------------------------------------------------------------------------|
+  * |   parameter || dest node ID | dest input ID | src node ID | src output ID |
+  * |---------------------------------------------------------------------------|
+  * |       bytes ||       2      |       2       |      2      |       2       |
+  * |---------------------------------------------------------------------------|
+  */
+int cb_getsource(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 length) {
+	if (length != 4) {return -1;}
+	u16 msg_id = meta->msg_id;
+	// Get requested IDs
+	int16_t node_id = build_short(data);
+	int16_t input_id = build_short(data + 2);
+
+	u8 resp_data[8];
+	pack_short(node_id, resp_data);
+	pack_short(input_id, resp_data + 2);
+
+	struct computation_graph* graph = structs->parameter_struct.graph;
+	struct node_src source = graph_get_source(graph, node_id, input_id);
+	pack_short(source.controller_id, data + 4);
+	pack_short(source.controller_output, data + 6);
+
+	send_data(&structs->hardware_struct.uart, RESPSOURCE_ID, msg_id, resp_data, sizeof(resp_data));
+}
+
+/**
+  * Handles a command to get a node output value from the quad.
+  * Packet structure is the same as getparam
+  */
+int cb_getoutput(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 length)
+{
+	// Check if the data length is correct
+	if (length != 8) {return -1;}
+	u16 msg_id = meta->msg_id;
+
+	// Get the controller ID, parameter ID
+	struct node_ids ids = get_node_ids(data);
+	struct computation_graph* graph = structs->parameter_struct.graph;
+	float output_val = graph_get_output(graph, ids.id, ids.sub_id);
+
+	// Format the response data
+	char resp_data[8];
+	// Controller ID
+	pack_short(ids.id, resp_data);
+	// Output ID
+	pack_short(ids.sub_id, resp_data + 2);
+	// Output value (4 byte float)
+	pack_float(output_val, resp_data + 4);
+
+	// Send the response
+	send_data(&structs->hardware_struct.uart, RESPOUTPUT_ID, msg_id, resp_data, sizeof(resp_data));
+
+	return 0;
+}
+
+int cb_getnodes(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 length) {
+
+}
+
+int cb_addnode(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 length) {
+
+}
\ No newline at end of file

quad/src/quad_app/util.c

@@ -93,3 +93,16 @@ float build_float(u8 *buff) {
     | buff[0];
   return x.f;
 }
+
+int16_t build_short(u8* buff) {
+	return buff[1] << 8 | buff[0];
+}
+
+void pack_short(int16_t val, u8* buff) {
+	buff[0] = val | 0x0F;
+	buff[1] = (val >> 8) | 0x0F;
+}
+
+void pack_float(float val, u8* buff) {
+	memcpy(&buff, &val, sizeof(val));
+}
\ No newline at end of file

quad/src/quad_app/util.h

@@ -18,5 +18,9 @@ void kill_motors(struct PWMOutputDriver *pwm_outputs);
 
 int build_int(u8 *buff);
 float build_float(u8 *buff);
+int16_t build_short(u8* buff);
+
+void pack_short(int16_t val, u8* buff);
+void pack_float(float val, u8* buff);
 
 #endif //_UTIL_H