diff --git a/quad/src/quad_app/callbacks.c b/quad/src/quad_app/callbacks.c
index 6ddd546a344df78d637707399cf600472040df53..361e03f5e07f2b460b7748f2a9c08dd94306d36c 100644
--- a/quad/src/quad_app/callbacks.c
+++ b/quad/src/quad_app/callbacks.c
@@ -3,6 +3,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
@@ -18,14 +19,14 @@ static size_t total_payload_received = 0;
*/
int cb_debug(modular_structs_t *structs, metadata_t *meta, u8 *data, u16 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;
}
@@ -44,10 +45,10 @@ int cb_packetlog(modular_structs_t* structs, metadata_t *meta, u8 *data, u16 len
* with the packet log data.
*/
int cb_getpacketlogs(modular_structs_t* structs, metadata_t *meta, u8 *data, u16 length) {
- char buf[255];
+ u8 buf[255];
// Message logging number of messages received and size of payload received
- int len = snprintf(buf, sizeof buf, "%d,%d", n_msg_received, total_payload_received);
+ int len = snprintf((char*)buf, sizeof buf, "%d,%d", n_msg_received, total_payload_received);
send_data(&structs->hardware_struct.uart, LOG_ID, 0, buf, len >= sizeof(buf) ? 255 : len + 1);
return 0;
@@ -188,7 +189,7 @@ int cb_getparam(modular_structs_t* structs, metadata_t *meta, u8 *data, u16 len
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
@@ -270,6 +271,7 @@ int cb_getsource(modular_structs_t* structs, metadata_t *meta, u8 *data, u16 le
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;
}
/**
@@ -288,7 +290,7 @@ int cb_getoutput(modular_structs_t* structs, metadata_t *meta, u8 *data, u16 le
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
@@ -320,30 +322,36 @@ int cb_getnodes(modular_structs_t* structs, metadata_t *meta, u8 *data, u16 len
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;
@@ -383,8 +391,17 @@ int cb_getnodes(modular_structs_t* structs, metadata_t *meta, u8 *data, u16 len
*/
int cb_addnode(modular_structs_t* structs, metadata_t *meta, u8 *data, u16 length) {
if (length < 2) {return -1;}
- // Size of name
- size_t name_len = length - 2;
+ struct computation_graph* graph = structs->parameter_struct.graph;
+ // 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);
+
+ // 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_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 c0d37659e29c0b8145b00e15f9a3302e2ef343ae..d6da551aa67032e4c6816dd94b27a4cbd2cd5d05 100644
--- a/quad/src/quad_app/communication.c
+++ b/quad/src/quad_app/communication.c
@@ -103,7 +103,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 |
//---------------------------------------------------------------------------------------------|
@@ -135,7 +135,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