From ba9709acca1fb0b7b618ed4c77ef841275ee8c7d Mon Sep 17 00:00:00 2001
From: David Wehr <dawehr@iastate.edu>
Date: Fri, 24 Mar 2017 02:42:53 -0500
Subject: [PATCH] Implemented get/set param, output, and source on the
quadcopter.
---
.../src/computation_graph/computation_graph.c | 10 +-
.../src/computation_graph/computation_graph.h | 17 +-
.../test/test_computation_graph.c | 31 ++-
quad/src/quad_app/callbacks.c | 198 ++++++++++++++----
quad/src/quad_app/util.c | 13 ++
quad/src/quad_app/util.h | 4 +
6 files changed, 226 insertions(+), 47 deletions(-)
diff --git a/quad/src/computation_graph/computation_graph.c b/quad/src/computation_graph/computation_graph.c
index 7434e14a1..7b21b2273 100644
--- a/quad/src/computation_graph/computation_graph.c
+++ b/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) ||
diff --git a/quad/src/computation_graph/computation_graph.h b/quad/src/computation_graph/computation_graph.h
index e5e769fd2..2cd90dc15 100644
--- a/quad/src/computation_graph/computation_graph.h
+++ b/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.
diff --git a/quad/src/computation_graph/test/test_computation_graph.c b/quad/src/computation_graph/test/test_computation_graph.c
index 1948421eb..42e913dca 100644
--- a/quad/src/computation_graph/test/test_computation_graph.c
+++ b/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();
}
diff --git a/quad/src/quad_app/callbacks.c b/quad/src/quad_app/callbacks.c
index 183ecf854..f9886913b 100644
--- a/quad/src/quad_app/callbacks.c
+++ b/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], ¶m_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
diff --git a/quad/src/quad_app/util.c b/quad/src/quad_app/util.c
index 25a8dae95..0c0a9c238 100644
--- a/quad/src/quad_app/util.c
+++ b/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
diff --git a/quad/src/quad_app/util.h b/quad/src/quad_app/util.h
index 0d06e5fe0..643498afc 100644
--- a/quad/src/quad_app/util.h
+++ b/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
--
GitLab