Fixed bugs with quad callbacks for graph commands.

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
 	{

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
 };

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
+}

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

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));
+}