Fixed X/Y and roll/pitch correspondence.

 Please enter the commit message for your changes. Lines starting

quad/sw/modular_quad_pid/gen_diagram/network.dot

@@ -3,12 +3,12 @@ rankdir="LR"
 "Roll PID"[shape=record
 label="<f0>Roll PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200]"]
 "Roll" -> "Roll PID":f1 [label="Constant"]
-"RC Roll" -> "Roll PID":f2 [label="Constant"]
+"Roll Clamp" -> "Roll PID":f2 [label="Bounded"]
 "dT" -> "Roll PID":f3 [label="Constant"]
 "Pitch PID"[shape=record
 label="<f0>Pitch PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200]"]
 "Pitch" -> "Pitch PID":f1 [label="Constant"]
-"RC Pitch" -> "Pitch PID":f2 [label="Constant"]
+"Pitch Clamp" -> "Pitch PID":f2 [label="Bounded"]
 "dT" -> "Pitch PID":f3 [label="Constant"]
 "Yaw PID"[shape=record
 label="<f0>Yaw PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=2.600] |<f5> [Ki=0.000] |<f6> [Kd=0.000]"]
@@ -70,10 +70,10 @@ label="<f0>Y PWM Clamp  |<f1> --\>Bounds in |<f2> [Min=-20000.000] |<f3> [Max=20
 "Yaw Rate PID" -> "Y PWM Clamp":f1 [label="Correction"]
 "Pitch Clamp"[shape=record
 label="<f0>Pitch Clamp  |<f1> --\>Bounds in |<f2> [Min=-0.349] |<f3> [Max=0.349]"]
-"Y pos PID" -> "Pitch Clamp":f1 [label="Correction"]
+"X pos PID" -> "Pitch Clamp":f1 [label="Correction"]
 "Roll Clamp"[shape=record
 label="<f0>Roll Clamp  |<f1> --\>Bounds in |<f2> [Min=-0.349] |<f3> [Max=0.349]"]
-"X pos PID" -> "Roll Clamp":f1 [label="Correction"]
+"Y pos PID" -> "Roll Clamp":f1 [label="Correction"]
 "Pitch"[shape=record
 label="<f0>Pitch  |<f1> [Constant=0.000]"]
 "Roll"[shape=record

quad/sw/modular_quad_pid/src/callbacks.c

@@ -16,8 +16,17 @@ static size_t total_payload_received = 0;
 /**
   * Currently does nothing.
   */
-int debug(modular_structs_t *structs)
+int cb_debug(modular_structs_t *structs)
 {
+	char buf[255];
+
+	// Get the node ID, parameter ID, parameter value
+	u8 node_id = uart_buff_data_get_u8(0);
+	struct computation_graph* graph = structs->parameter_struct.graph;
+	float param_val = graph_get_output(graph, node_id, 0);
+
+	int length = snprintf(buf, sizeof buf, "%f", param_val);
+	send_data(DEBUG_ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
 	return 0;
 }
 

quad/sw/modular_quad_pid/src/control_algorithm.c

@@ -22,21 +22,16 @@ void connect_autonomous(parameter_t* ps) {
 	struct computation_graph* graph = ps->graph;
 	graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->clamp_pitch, PID_CORRECTION);
 	graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->clamp_roll, PID_CORRECTION);
-	graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
+	//graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->throttle_trim_add, ADD_SUM);
 	//graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->yaw_pid, PID_CORRECTION);
 }
 
 void connect_manual(parameter_t* ps) {
 	struct computation_graph* graph = ps->graph;
-	/*
 	graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->rc_pitch, CONST_VAL);
 	graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->rc_roll, CONST_VAL);
 	graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, CONST_VAL);
 	graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->rc_yaw, CONST_VAL);
-	*/
-	connect_autonomous(ps);
-	graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->rc_yaw, CONST_VAL);
-	graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, CONST_VAL);
 }
 
 int control_algorithm_init(parameter_t * ps)
@@ -54,7 +49,7 @@ int control_algorithm_init(parameter_t * ps)
     ps->x_pos_pid = graph_add_node_pid(graph, "X pos PID");
     ps->y_pos_pid = graph_add_node_pid(graph, "Y pos PID");
     ps->alt_pid = graph_add_node_pid(graph, "Altitude PID");
-    ps->x_set = graph_add_node_const(graph, "X Setpoint");
+    ps->x_set = graph_add_node_const(graph, "X Setpoint"); // ID 9
     ps->y_set = graph_add_node_const(graph, "Y Setpoint");
     ps->alt_set = graph_add_node_const(graph, "Alt Setpoint");
     ps->yaw_set = graph_add_node_const(graph, "Yaw Setpoint");
@@ -69,7 +64,7 @@ int control_algorithm_init(parameter_t * ps)
     ps->clamp_roll= graph_add_node_bounds(graph, "Roll Clamp");
 
     // Create blocks for sensor inputs
-    ps->cur_pitch = graph_add_node_const(graph, "Pitch");
+    ps->cur_pitch = graph_add_node_const(graph, "Pitch"); // ID 20
     ps->cur_roll = graph_add_node_const(graph, "Roll");
     ps->cur_yaw = graph_add_node_const(graph, "Yaw");
 	// Yaw angular velocity PID
@@ -120,12 +115,12 @@ int control_algorithm_init(parameter_t * ps)
 
     // X autonomous
     graph_set_source(graph, ps->x_pos_pid, PID_DT, ps->dt, CONST_VAL);
-    graph_set_source(graph, ps->clamp_roll, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
+    graph_set_source(graph, ps->clamp_pitch, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
     graph_set_source(graph, ps->x_pos_pid, PID_CUR_POINT, ps->vrpn_x, CONST_VAL);
     graph_set_source(graph, ps->x_pos_pid, PID_SETPOINT, ps->x_set, CONST_VAL);
     // Y autonomous
     graph_set_source(graph, ps->y_pos_pid, PID_DT, ps->dt, CONST_VAL);
-    graph_set_source(graph, ps->clamp_pitch, BOUNDS_IN, ps->y_pos_pid, PID_CORRECTION);
+    graph_set_source(graph, ps->clamp_roll, BOUNDS_IN, ps->y_pos_pid, PID_CORRECTION);
     graph_set_source(graph, ps->y_pos_pid, PID_CUR_POINT, ps->vrpn_y, CONST_VAL);
     graph_set_source(graph, ps->y_pos_pid, PID_SETPOINT, ps->y_set, CONST_VAL);
     // Alt autonomous
@@ -222,7 +217,7 @@ int control_algorithm_init(parameter_t * ps)
 	// to engage AUTO mode the code waits for a new packet after the flap is switched to auto
 	// before actually engaging AUTO mode
 	if(cur_fm_switch == MANUAL_FLIGHT_MODE) {
-		if (user_defined_struct->flight_mode == MANUAL_FLIGHT_MODE) {
+		if (last_fm_switch == AUTO_FLIGHT_MODE) {
 			connect_manual(parameter_struct);
 		}
 		user_defined_struct->flight_mode = MANUAL_FLIGHT_MODE;