diff --git a/quad/src/commands/commands.c b/quad/src/commands/commands.c
index 14ae7c58c9e3bac69bfc1d4a04c8239cabc0d908..8dddcbf3c26d53b8f306fa8024e4e02d2c75526b 100644
--- a/quad/src/commands/commands.c
+++ b/quad/src/commands/commands.c
@@ -213,7 +213,7 @@ struct MessageType MessageTypes[MAX_TYPE_ID] =
 		// Type of the command data
 		floatType,
 		// Function pointer
-		&cb_respoutput
+		&cb_getoutput
 	},
 	// RESPOUTPUT
 	{
diff --git a/quad/src/graph_blocks/graph_blocks.h b/quad/src/graph_blocks/graph_blocks.h
index cc822b242bbbc2a80a064ce2390a846cef7bc8d5..12382fda407c98694a7c799170feba12e824ddb6 100644
--- a/quad/src/graph_blocks/graph_blocks.h
+++ b/quad/src/graph_blocks/graph_blocks.h
@@ -34,8 +34,8 @@ enum BlockTypes {
     BLOCK_GAIN,          // 03
     BLOCK_ACCUMULATE,    // 04
     BLOCK_BOUNDS,        // 05
-    BLOCK_MIXER,         // 07
-    BLOCK_PID,           // 08
+    BLOCK_MIXER,         // 06
+    BLOCK_PID,           // 07
     //                        <-- Insert new block type here
     MAX_BLOCK_TYPES
 };
diff --git a/quad/src/quad_app/callbacks.c b/quad/src/quad_app/callbacks.c
index 62f89be50c3abb9a686e6d0b80b601b432caba97..6ddd546a344df78d637707399cf600472040df53 100644
--- a/quad/src/quad_app/callbacks.c
+++ b/quad/src/quad_app/callbacks.c
@@ -258,17 +258,16 @@ int cb_getsource(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 le
 	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);
+	struct node_ids ids = get_node_ids(data);
 
 	u8 resp_data[8];
-	pack_short(node_id, resp_data);
-	pack_short(input_id, resp_data + 2);
+	pack_short(ids.id, resp_data);
+	pack_short(ids.sub_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);
+	struct node_src source = graph_get_source(graph, ids.id, ids.sub_id);
+	pack_short(source.controller_id, resp_data + 4);
+	pack_short(source.controller_output, resp_data + 6);
 
 	send_data(&structs->hardware_struct.uart, RESPSOURCE_ID, msg_id, resp_data, sizeof(resp_data));
 }
@@ -280,7 +279,7 @@ int cb_getsource(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 le
 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;}
+	if (length != 4) {return -1;}
 	u16 msg_id = meta->msg_id;
 
 	// Get the controller ID, parameter ID
@@ -388,4 +387,4 @@ int cb_addnode(modular_structs_t* structs, metadata_t *meta,  u8 *data, u16 leng
 	size_t name_len = length - 2;
 
 	return 0;
-}
\ No newline at end of file
+}
diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 178cfc8472d4e22b252176691160e18808389e15..424f098cbe02be1fb3e7707b6383fa0814c99957 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -56,7 +56,7 @@ int control_algorithm_init(parameter_t * ps)
     ps->throttle_trim_add = graph_add_defined_block(graph, BLOCK_ADD, "T trim add");
 
     // Create blocks for sensor inputs
-    ps->cur_pitch = graph_add_defined_block(graph, BLOCK_CONSTANT, "Pitch"); // ID 20
+    ps->cur_pitch = graph_add_defined_block(graph, BLOCK_CONSTANT, "Pitch"); // ID 15
     ps->cur_roll = graph_add_defined_block(graph, BLOCK_CONSTANT, "Roll");
     ps->cur_yaw = graph_add_defined_block(graph, BLOCK_CONSTANT, "Yaw");
 	// Yaw angular velocity PID
diff --git a/quad/src/quad_app/util.c b/quad/src/quad_app/util.c
index 0c0a9c23803f0a0c92c02286cc52085648d1e68f..c021fef37cc1daeb6adafe20a75b25e1ec99e558 100644
--- a/quad/src/quad_app/util.c
+++ b/quad/src/quad_app/util.c
@@ -95,14 +95,14 @@ float build_float(u8 *buff) {
 }
 
 int16_t build_short(u8* buff) {
-	return buff[1] << 8 | buff[0];
+	return (buff[0] & 0xFF) | ((buff[1] & 0xFF) << 8);
 }
 
 void pack_short(int16_t val, u8* buff) {
-	buff[0] = val | 0x0F;
-	buff[1] = (val >> 8) | 0x0F;
+	buff[0] = val & 0xFF;
+	buff[1] = (val >> 8) & 0xFF;
 }
 
 void pack_float(float val, u8* buff) {
-	memcpy(&buff, &val, sizeof(val));
-}
\ No newline at end of file
+	memcpy(buff, &val, sizeof(val));
+}