diff --git a/quad/sw/modular_quad_pid/gen_diagram/gen_diagram b/quad/sw/modular_quad_pid/gen_diagram/gen_diagram
index 886d998d528ab88666356c6ce04079586191f741..8174dab245654da3b79d9ab54c835b93766a8b02 100755
Binary files a/quad/sw/modular_quad_pid/gen_diagram/gen_diagram and b/quad/sw/modular_quad_pid/gen_diagram/gen_diagram differ
diff --git a/quad/sw/modular_quad_pid/gen_diagram/network.dot b/quad/sw/modular_quad_pid/gen_diagram/network.dot
index 73ea4769de1d985c7f99c005f067367a4df95df8..00e5b058e18e15345720278726ff5faae6351bee 100644
--- a/quad/sw/modular_quad_pid/gen_diagram/network.dot
+++ b/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
diff --git a/quad/sw/modular_quad_pid/gen_diagram/network.png b/quad/sw/modular_quad_pid/gen_diagram/network.png
index e8f7848eff002aad2f8db70bd08ccea743ef2f10..f1203a6c4dc72f46b67c1f76e06ae6a4c93eb2d4 100644
Binary files a/quad/sw/modular_quad_pid/gen_diagram/network.png and b/quad/sw/modular_quad_pid/gen_diagram/network.png differ
diff --git a/quad/sw/modular_quad_pid/src/control_algorithm.c b/quad/sw/modular_quad_pid/src/control_algorithm.c
index f85791eb100078633488b0c122736444ec0773fb..632cde85131d83bd2a5c811c8ce3124b02abe351 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.c
+++ b/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);
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index ba125fa40c917a2fe453fc838f7fd3fdac60dee6..cf893938b9cf34327f543ab2b607bf352c6e89a5 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/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;