diff --git a/quad/src/quad_app/callbacks.c b/quad/src/quad_app/callbacks.c
index f55a8ecb6d75e666171a657d00c488815cf23a45..8acc3cc7c77cf97b3e3d4365d8de546baa382779 100644
--- a/quad/src/quad_app/callbacks.c
+++ b/quad/src/quad_app/callbacks.c
@@ -4,6 +4,7 @@
#include "type_def.h"
#include "computation_graph.h"
#include "util.h"
+#include "graph_blocks.h"
/*
* Static variables used to keep track of packet counts
@@ -19,14 +20,14 @@ static size_t total_payload_received = 0;
*/
int cb_debug(struct modular_structs *structs, struct metadata *meta, unsigned char *data, unsigned short length)
{
- char buf[255];
+ u8 buf[255];
// Get the node ID, parameter ID, parameter value
u8 node_id = data[0];
struct computation_graph* graph = structs->parameter_struct.graph;
float param_val = graph_get_output(graph, node_id, 0);
- int len = snprintf(buf, sizeof buf, "%f", param_val);
+ int len = snprintf((char*)buf, sizeof buf, "%f", param_val);
send_data(&structs->hardware_struct.uart, DEBUG_ID, 0, buf, len >= sizeof(buf) ? 255 : length + 1);
return 0;
}
@@ -48,7 +49,7 @@ int cb_getpacketlogs(struct modular_structs* structs, struct metadata *meta, u8
char buf[255];
// Message logging number of messages received and size of payload received
- int len = snprintf(buf, sizeof buf, "%d,%lu", n_msg_received, total_payload_received);
+ int len = snprintf((char*)buf, sizeof buf, "%d,%lu", n_msg_received, total_payload_received);
send_data(&structs->hardware_struct.uart, LOG_ID, 0, buf, len >= sizeof(buf) ? 255 : len + 1);
return 0;
@@ -189,7 +190,7 @@ int cb_getparam(struct modular_structs *structs, struct metadata *meta, unsigned
float param_val = graph_get_param_val(graph, ids.id, ids.sub_id);
// Format the response data
- char resp_data[8];
+ u8 resp_data[8];
// Controller ID
pack_short(ids.id, resp_data);
// Parameter ID
@@ -271,6 +272,7 @@ int cb_getsource(struct modular_structs *structs, struct metadata *meta, unsigne
pack_short(source.controller_output, resp_data + 6);
send_data(&structs->hardware_struct.uart, RESPSOURCE_ID, msg_id, resp_data, sizeof(resp_data));
+ return 0;
}
/**
@@ -289,7 +291,7 @@ int cb_getoutput(struct modular_structs *structs, struct metadata *meta, unsigne
float output_val = graph_get_output(graph, ids.id, ids.sub_id);
// Format the response data
- char resp_data[8];
+ u8 resp_data[8];
// Controller ID
pack_short(ids.id, resp_data);
// Output ID
@@ -321,30 +323,36 @@ int cb_getnodes(struct modular_structs *structs, struct metadata *meta, unsigned
if (graph->n_nodes >= 150) {
static char* error_msg = "Over 150 nodes. Not responding to cb_getnodes for fear of buffer overflow.";
send_data(&structs->hardware_struct.uart, DEBUG_ID, 0,
- error_msg, sizeof(error_msg));
+ (u8*)error_msg, sizeof(error_msg));
return -1;
}
// Number of bytes in node ID being sent. Currently short (16 bits)
- size_t id_len = 2;
+ const size_t id_len = 2;
+
+ u8 resp_buf[4096];
+ size_t offset = 0;
+ // Send the number of nodes there are in the graph
+ pack_short(graph->n_nodes, resp_buf + offset);
+ offset += id_len;
- char resp_buf[4096];
+ // Send all the node data
int i;
// Currently ID is always index in array.
// computation_graph provides no method of accessing ID, since it is implicit
for (i = 0; i < graph->n_nodes; i++) {
- pack_short(i, resp_buf + (id_len * i));
+ pack_short(i, resp_buf + offset);
+ offset += id_len;
}
// Construct type IDs
- size_t offset = id_len * graph->n_nodes;
for (i = 0; i < graph->n_nodes; i++) {
int type_id = graph->nodes[i].type->type_id;
- pack_short(type_id, resp_buf + offset + (id_len * i));
+ pack_short(type_id, resp_buf + offset);
+ offset += id_len;
}
// Construct list of node names
- offset += id_len * graph->n_nodes;
for (i = 0; i < graph->n_nodes; i++) {
size_t remaining_size = sizeof(resp_buf) - offset;
const char* name = graph->nodes[i].name;
@@ -385,27 +393,16 @@ int cb_getnodes(struct modular_structs *structs, struct metadata *meta, unsigned
int cb_addnode(struct modular_structs *structs, struct metadata *meta, unsigned char *data, unsigned short length) {
// Check if the data length is large enough
if (length < 2) {return -1;}
-
- // Size of name
- size_t name_len = length - 2;
struct computation_graph* graph = structs->parameter_struct.graph;
- char resp_buf[2];
-
- int16_t node_type = build_short(data);
- if (node_type < 0 && node_type >= MAX_BLOCK_TYPES) {
- pack_short(-1, resp_buf);
- send_data(&structs->hardware_struct.uart, RESPADDNODE_ID, meta->msg_id, resp_buf, 2);
- return -1;
- }
+ // Get the data for the new node
+ int block_type = build_short(data);
+ char* name = (char*) &data[2];
+ int new_node_id = graph_add_defined_block(graph, block_type, name);
- char* name = strndup(data+2, name_len);
- int new_block_id = graph_add_defined_block(graph, node_type, name);
- free(name);
- // Format the response data
- // New Block Id
- pack_short(new_block_id, resp_buf);
+ // Respond with the result of graph_add_defined_block, which will be -1 if failure
+ u8 resp_buf[2];
+ pack_short(new_node_id, resp_buf);
- // Send the response
- send_data(&structs->hardware_struct.uart, RESPADDNODE_ID, meta->msg_id, resp_buf, 2);
+ send_data(&structs->hardware_struct.uart, RESPADDNODE_ID, meta->msg_id, resp_buf, sizeof(resp_buf));
return 0;
}
diff --git a/quad/src/quad_app/communication.c b/quad/src/quad_app/communication.c
index 32b1e3da08d080c3ea43f8ccbea34dbcbdb13c0d..aa7561a20a2b389a0a21e39cfc585192ce8a61cc 100644
--- a/quad/src/quad_app/communication.c
+++ b/quad/src/quad_app/communication.c
@@ -102,7 +102,7 @@ void process_received(modular_structs_t *structs) {
}
}
-int send_data(struct UARTDriver *uart, u16 type_id, u16 msg_id, char* data, size_t size) {
+int send_data(struct UARTDriver *uart, u16 type_id, u16 msg_id, u8* data, size_t size) {
//----------------------------------------------------------------------------------------------
// index|| 0 | 1 | 2 | 3 & 4 | 5 & 6 | 7+ | end |
//---------------------------------------------------------------------------------------------|
@@ -134,7 +134,7 @@ int send_data(struct UARTDriver *uart, u16 type_id, u16 msg_id, char* data, size
}
for (i = 0; i < size; i++) {
packet_checksum ^= data[i];
- uart->write(uart, (unsigned char) data[i]);
+ uart->write(uart, data[i]);
}
uart->write(uart, packet_checksum);
diff --git a/quad/src/quad_app/communication.h b/quad/src/quad_app/communication.h
index 8655aad007e1406774d1a70cf2ed130faac4f426..6441ef11000c0d644b0f1df2b3b8024f94d080d8 100644
--- a/quad/src/quad_app/communication.h
+++ b/quad/src/quad_app/communication.h
@@ -10,6 +10,6 @@
int initUartComms();
void process_received(modular_structs_t *structs);
-int send_data(struct UARTDriver *uart, u16 type_id, u16 msg_id, char* data, size_t size);
+int send_data(struct UARTDriver *uart, u16 type_id, u16 msg_id, u8* data, size_t size);
#endif