Added PWM clamping back in.

quad/sw/modular_quad_pid/gen_diagram/network.dot

@@ -71,6 +71,15 @@ label="<f0>dTheta  |<f1> [Constant=0.000]"]
 label="<f0>dPhi  |<f1> [Constant=0.000]"]
 "dPsi"[shape=record
 label="<f0>dPsi  |<f1> [Constant=0.000]"]
+"P PWM Clamp"[shape=record
+label="<f0>P PWM Clamp  |<f1> --\>Bounds in |<f2> [Min=-20000.000] |<f3> [Max=20000.000]"]
+"Pitch Rate PID" -> "P PWM Clamp":f1 [label="Correction"]
+"R PWM Clamp"[shape=record
+label="<f0>R PWM Clamp  |<f1> --\>Bounds in |<f2> [Min=-20000.000] |<f3> [Max=20000.000]"]
+"Roll Rate PID" -> "R PWM Clamp":f1 [label="Correction"]
+"Y PWM Clamp"[shape=record
+label="<f0>Y PWM Clamp  |<f1> --\>Bounds in |<f2> [Min=-20000.000] |<f3> [Max=20000.000]"]
+"Yaw Rate PID" -> "Y PWM Clamp":f1 [label="Correction"]
 "VRPN X"[shape=record
 label="<f0>VRPN X  |<f1> [Constant=0.000]"]
 "VRPN Y"[shape=record
@@ -88,9 +97,9 @@ label="<f0>RC Throttle  |<f1> [Constant=0.000]"]
 "Signal Mixer"[shape=record
 label="<f0>Signal Mixer  |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4> --\>Yaw"]
 "RC Throttle" -> "Signal Mixer":f1 [label="Constant"]
-"Pitch Rate PID" -> "Signal Mixer":f2 [label="Correction"]
-"Roll Rate PID" -> "Signal Mixer":f3 [label="Correction"]
-"Yaw Rate PID" -> "Signal Mixer":f4 [label="Correction"]
+"P PWM Clamp" -> "Signal Mixer":f2 [label="Bounded"]
+"R PWM Clamp" -> "Signal Mixer":f3 [label="Bounded"]
+"Y PWM Clamp" -> "Signal Mixer":f4 [label="Bounded"]
 "Ts_angle"[shape=record
 label="<f0>Ts_angle  |<f1> [Constant=0.005]"]
 "Ts_position"[shape=record

quad/sw/modular_quad_pid/src/control_algorithm.c

@@ -68,6 +68,9 @@ int control_algorithm_init(parameter_t * ps)
     ps->theta_dot = graph_add_node_const(graph, "dTheta");
     ps->phi_dot = graph_add_node_const(graph, "dPhi");
     ps->psi_dot = graph_add_node_const(graph, "dPsi");
+    ps->clamp_d_pwmP = graph_add_node_bounds(graph, "P PWM Clamp");
+    ps->clamp_d_pwmR = graph_add_node_bounds(graph, "R PWM Clamp");
+    ps->clamp_d_pwmY = graph_add_node_bounds(graph, "Y PWM Clamp");
 
     // Create blocks for VRPN data
     ps->vrpn_x = graph_add_node_const(graph, "VRPN X");
@@ -127,15 +130,16 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_source(graph, ps->yaw_pid, PID_SETPOINT, ps->yaw_set, CONST_VAL);
     graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->cur_yaw, CONST_VAL);
 
+    // Connect PWM clamping blocks
+    graph_set_source(graph, ps->clamp_d_pwmP, BOUNDS_IN, ps->pitch_r_pid, PID_CORRECTION);
+    graph_set_source(graph, ps->clamp_d_pwmR, BOUNDS_IN, ps->roll_r_pid, PID_CORRECTION);
+    graph_set_source(graph, ps->clamp_d_pwmY, BOUNDS_IN, ps->yaw_r_pid, PID_CORRECTION);
 
     // Connect signal mixer
     //graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, CONST_VAL);
-    graph_set_source(graph, ps->mixer, MIXER_PITCH, ps->pitch_r_pid, BOUNDS_OUT);
-    graph_set_source(graph, ps->mixer, MIXER_ROLL, ps->roll_r_pid, BOUNDS_OUT);
-    graph_set_source(graph, ps->mixer, MIXER_YAW, ps->yaw_r_pid, BOUNDS_OUT);
-
-    // Set initial mode
-    connect_manual(ps);
+    graph_set_source(graph, ps->mixer, MIXER_PITCH, ps->clamp_d_pwmP, BOUNDS_OUT);
+    graph_set_source(graph, ps->mixer, MIXER_ROLL, ps->clamp_d_pwmR, BOUNDS_OUT);
+    graph_set_source(graph, ps->mixer, MIXER_YAW, ps->clamp_d_pwmY, BOUNDS_OUT);
 
     // Set pitch PID constants
     graph_set_param_val(graph, ps->pitch_pid, PID_KP, PITCH_ANGLE_KP);
@@ -172,10 +176,21 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_param_val(graph, ps->alt_pid, PID_KI, ALT_ZPOS_KI);
     graph_set_param_val(graph, ps->alt_pid, PID_KD, ALT_ZPOS_KD);
 
+    // Set PWM difference clamping limits
+    graph_set_param_val(graph, ps->clamp_d_pwmP, BOUNDS_MIN, -20000);
+    graph_set_param_val(graph, ps->clamp_d_pwmP, BOUNDS_MAX, 20000);
+    graph_set_param_val(graph, ps->clamp_d_pwmR, BOUNDS_MIN, -20000);
+    graph_set_param_val(graph, ps->clamp_d_pwmR, BOUNDS_MAX, 20000);
+    graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MIN, -20000);
+    graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MAX, 20000);
+
     // TODO: Change this to use LOOP_TIME
     graph_set_param_val(graph, ps->angle_time, CONST_SET, 0.005);
     graph_set_param_val(graph, ps->pos_time, CONST_SET, 0.1);
 
+    // Set initial mode
+    connect_manual(ps);
+
 	return 0;
  }
 
@@ -265,8 +280,8 @@ int control_algorithm_init(parameter_t * ps)
 	//memcpy(raw_actuator_struct->controller_corrected_motor_commands, user_input_struct->rc_commands, sizeof(int) * 6);
 
 	// don't use the PID corrections if the throttle is less than about 10% of its range
-	if((user_input_struct->rc_commands[THROTTLE] >
-	118000) || (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE))
+    // NOTE: This needs to be changed when throttle is autonomous!!!
+	if((user_input_struct->rc_commands[THROTTLE] > 118000))
 	{
 			//THROTTLE
 			actuator_struct->pwms[0] = graph_get_output(graph, ps->mixer, MIXER_PWM0);

quad/sw/modular_quad_pid/src/type_def.h

@@ -321,6 +321,10 @@ typedef struct parameter_t {
 	int y_set;
 	int alt_set;
 	int yaw_set;
+	// Clamps
+	int clamp_d_pwmP;
+	int clamp_d_pwmR;
+	int clamp_d_pwmY;
 	// Loop times
 	int angle_time;
 	int pos_time;