diff --git a/quad/sw/modular_quad_pid/gen_diagram/gen_diagram b/quad/sw/modular_quad_pid/gen_diagram/gen_diagram
index aa2340340c56fb06d2f034d2a9ce4c3dc25b885f..886d998d528ab88666356c6ce04079586191f741 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 022e6781f6224e2d7dfe7a3aa28bca3d5d767580..73ea4769de1d985c7f99c005f067367a4df95df8 100644
--- a/quad/sw/modular_quad_pid/gen_diagram/network.dot
+++ b/quad/sw/modular_quad_pid/gen_diagram/network.dot
@@ -3,48 +3,48 @@ 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"]
-"Roll Clamp" -> "Roll PID":f2 [label="Bounded"]
-"dT" -> "Roll PID":f3 [label="Constant"]
+"Y pos PID" -> "Roll PID":f2 [label="Correction"]
+"Ts_angle" -> "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"]
-"Pitch Clamp" -> "Pitch PID":f2 [label="Bounded"]
-"dT" -> "Pitch PID":f3 [label="Constant"]
+"X pos PID" -> "Pitch PID":f2 [label="Correction"]
+"Ts_angle" -> "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]"]
"Yaw" -> "Yaw PID":f1 [label="Constant"]
"Yaw Setpoint" -> "Yaw PID":f2 [label="Constant"]
-"dT" -> "Yaw PID":f3 [label="Constant"]
+"Ts_angle" -> "Yaw PID":f3 [label="Constant"]
"Roll Rate PID"[shape=record
label="<f0>Roll Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=4600.000] |<f5> [Ki=0.000] |<f6> [Kd=550.000]"]
"dPhi" -> "Roll Rate PID":f1 [label="Constant"]
"Roll PID" -> "Roll Rate PID":f2 [label="Correction"]
-"dT" -> "Roll Rate PID":f3 [label="Constant"]
+"Ts_angle" -> "Roll Rate PID":f3 [label="Constant"]
"Pitch Rate PID"[shape=record
label="<f0>Pitch Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=4600.000] |<f5> [Ki=0.000] |<f6> [Kd=550.000]"]
"dTheta" -> "Pitch Rate PID":f1 [label="Constant"]
"Pitch PID" -> "Pitch Rate PID":f2 [label="Correction"]
-"dT" -> "Pitch Rate PID":f3 [label="Constant"]
+"Ts_angle" -> "Pitch Rate PID":f3 [label="Constant"]
"Yaw Rate PID"[shape=record
label="<f0>Yaw Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=435480.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000]"]
"dPsi" -> "Yaw Rate PID":f1 [label="Constant"]
"RC Yaw" -> "Yaw Rate PID":f2 [label="Constant"]
-"dT" -> "Yaw Rate PID":f3 [label="Constant"]
+"Ts_angle" -> "Yaw Rate PID":f3 [label="Constant"]
"X pos PID"[shape=record
label="<f0>X pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=-0.015] |<f5> [Ki=-0.005] |<f6> [Kd=-0.030]"]
"VRPN X" -> "X pos PID":f1 [label="Constant"]
"X Setpoint" -> "X pos PID":f2 [label="Constant"]
-"dT" -> "X pos PID":f3 [label="Constant"]
+"Ts_position" -> "X pos PID":f3 [label="Constant"]
"Y pos PID"[shape=record
label="<f0>Y pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.015] |<f5> [Ki=0.005] |<f6> [Kd=0.030]"]
"VRPN Y" -> "Y pos PID":f1 [label="Constant"]
"Y Setpoint" -> "Y pos PID":f2 [label="Constant"]
-"dT" -> "Y pos PID":f3 [label="Constant"]
+"Ts_position" -> "Y pos PID":f3 [label="Constant"]
"Altitude PID"[shape=record
label="<f0>Altitude PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=9804.000] |<f5> [Ki=817.000] |<f6> [Kd=7353.000]"]
"VRPN Alt" -> "Altitude PID":f1 [label="Constant"]
"Alt Setpoint" -> "Altitude PID":f2 [label="Constant"]
-"dT" -> "Altitude PID":f3 [label="Constant"]
+"Ts_position" -> "Altitude PID":f3 [label="Constant"]
"X Setpoint"[shape=record
label="<f0>X Setpoint |<f1> [Constant=0.000]"]
"Y Setpoint"[shape=record
@@ -59,21 +59,6 @@ label="<f0>Throttle trim |<f1> [Constant=0.000]"]
label="<f0>T trim add |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
"Altitude PID" -> "T trim add":f1 [label="Correction"]
"Throttle trim" -> "T trim add":f2 [label="Constant"]
-"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"]
-"Pitch Clamp"[shape=record
-label="<f0>Pitch Clamp |<f1> --\>Bounds in |<f2> [Min=-0.349] |<f3> [Max=0.349]"]
-"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]"]
-"Y pos PID" -> "Roll Clamp":f1 [label="Correction"]
"Pitch"[shape=record
label="<f0>Pitch |<f1> [Constant=0.000]"]
"Roll"[shape=record
@@ -103,9 +88,11 @@ 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"]
-"P PWM Clamp" -> "Signal Mixer":f2 [label="Bounded"]
-"R PWM Clamp" -> "Signal Mixer":f3 [label="Bounded"]
-"Y PWM Clamp" -> "Signal Mixer":f4 [label="Bounded"]
-"dT"[shape=record
-label="<f0>dT |<f1> [Constant=0.005]"]
+"Pitch Rate PID" -> "Signal Mixer":f2 [label="Correction"]
+"Roll Rate PID" -> "Signal Mixer":f3 [label="Correction"]
+"Yaw Rate PID" -> "Signal Mixer":f4 [label="Correction"]
+"Ts_angle"[shape=record
+label="<f0>Ts_angle |<f1> [Constant=0.005]"]
+"Ts_position"[shape=record
+label="<f0>Ts_position |<f1> [Constant=0.100]"]
}
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/gen_diagram/network.png b/quad/sw/modular_quad_pid/gen_diagram/network.png
index 1bd106753a9455c264e618a90ad12518196160f2..e8f7848eff002aad2f8db70bd08ccea743ef2f10 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 f18f39b763ea3a44a39742c397403b415e75ecb0..f85791eb100078633488b0c122736444ec0773fb 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.c
+++ b/quad/sw/modular_quad_pid/src/control_algorithm.c
@@ -20,8 +20,8 @@
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->pitch_pid, PID_SETPOINT, ps->x_pos_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->y_pos_pid, PID_CORRECTION);
//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);
}
@@ -56,13 +56,6 @@ int control_algorithm_init(parameter_t * ps)
ps->throttle_trim = graph_add_node_const(graph, "Throttle trim");
ps->throttle_trim_add = graph_add_node_add(graph, "T trim add");
- // Create blocks for bounds checking
- 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");
- ps->clamp_pitch = graph_add_node_bounds(graph, "Pitch Clamp");
- ps->clamp_roll= graph_add_node_bounds(graph, "Roll Clamp");
-
// Create blocks for sensor inputs
ps->cur_pitch = graph_add_node_const(graph, "Pitch"); // ID 20
ps->cur_roll = graph_add_node_const(graph, "Roll");
@@ -89,63 +82,57 @@ int control_algorithm_init(parameter_t * ps)
ps->mixer = graph_add_node_mixer(graph, "Signal Mixer");
- ps->dt = graph_add_node_const(graph, "dT");
+ ps->angle_time = graph_add_node_const(graph, "Ts_angle");
+ ps->pos_time = graph_add_node_const(graph, "Ts_position");
// Connect pitch PID chain
graph_set_source(graph, ps->pitch_r_pid, PID_SETPOINT, ps->pitch_pid, PID_CORRECTION);
graph_set_source(graph, ps->pitch_r_pid, PID_CUR_POINT, ps->theta_dot, CONST_VAL);
- graph_set_source(graph, ps->pitch_r_pid, PID_DT, ps->dt, CONST_VAL);
+ graph_set_source(graph, ps->pitch_r_pid, PID_DT, ps->angle_time, CONST_VAL);
//graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->rc_pitch, CONST_VAL);
graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->cur_pitch, CONST_VAL);
- graph_set_source(graph, ps->pitch_pid, PID_DT, ps->dt, CONST_VAL);
+ graph_set_source(graph, ps->pitch_pid, PID_DT, ps->angle_time, CONST_VAL);
// Connect roll PID chain
graph_set_source(graph, ps->roll_r_pid, PID_SETPOINT, ps->roll_pid, PID_CORRECTION);
graph_set_source(graph, ps->roll_r_pid, PID_CUR_POINT, ps->phi_dot, CONST_VAL);
- graph_set_source(graph, ps->roll_r_pid, PID_DT, ps->dt, CONST_VAL);
+ graph_set_source(graph, ps->roll_r_pid, PID_DT, ps->angle_time, CONST_VAL);
//graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->rc_roll, CONST_VAL);
graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->cur_roll, CONST_VAL);
- graph_set_source(graph, ps->roll_pid, PID_DT, ps->dt, CONST_VAL);
+ graph_set_source(graph, ps->roll_pid, PID_DT, ps->angle_time, CONST_VAL);
// Connect yaw PID chain
graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->rc_yaw, PID_CORRECTION);
graph_set_source(graph, ps->yaw_r_pid, PID_CUR_POINT, ps->psi_dot, CONST_VAL);
- graph_set_source(graph, ps->yaw_r_pid, PID_DT, ps->dt, CONST_VAL);
+ graph_set_source(graph, ps->yaw_r_pid, PID_DT, ps->angle_time, CONST_VAL);
// X autonomous
- graph_set_source(graph, ps->x_pos_pid, PID_DT, ps->dt, CONST_VAL);
- graph_set_source(graph, ps->clamp_pitch, BOUNDS_IN, ps->x_pos_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->x_pos_pid, PID_DT, ps->pos_time, CONST_VAL);
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_roll, BOUNDS_IN, ps->y_pos_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->y_pos_pid, PID_DT, ps->pos_time, CONST_VAL);
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
- graph_set_source(graph, ps->alt_pid, PID_DT, ps->dt, CONST_VAL);
+ graph_set_source(graph, ps->alt_pid, PID_DT, ps->pos_time, CONST_VAL);
graph_set_source(graph, ps->alt_pid, PID_CUR_POINT, ps->vrpn_alt, CONST_VAL);
graph_set_source(graph, ps->alt_pid, PID_SETPOINT, ps->alt_set, CONST_VAL);
graph_set_source(graph, ps->alt_set, CONST_VAL, ps->vrpn_alt, CONST_VAL);
graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND1, ps->alt_pid, PID_CORRECTION);
graph_set_source(graph, ps->throttle_trim_add, ADD_SUMMAND2, ps->throttle_trim, CONST_VAL);
// Yaw autonomous
- graph_set_source(graph, ps->yaw_pid, PID_DT, ps->dt, CONST_VAL);
+ graph_set_source(graph, ps->yaw_pid, PID_DT, ps->angle_time, CONST_VAL);
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->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);
+ 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);
@@ -185,30 +172,16 @@ 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 angle clamping limits
- graph_set_param_val(graph, ps->clamp_pitch, BOUNDS_MIN, -ROLL_PITCH_MAX_ANGLE);
- graph_set_param_val(graph, ps->clamp_pitch, BOUNDS_MAX, ROLL_PITCH_MAX_ANGLE);
- graph_set_param_val(graph, ps->clamp_roll, BOUNDS_MIN, -ROLL_PITCH_MAX_ANGLE);
- graph_set_param_val(graph, ps->clamp_roll, BOUNDS_MAX, ROLL_PITCH_MAX_ANGLE);
-
- // 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->dt, CONST_SET, 0.005);
+ graph_set_param_val(graph, ps->angle_time, CONST_SET, 0.005);
+ graph_set_param_val(graph, ps->pos_time, CONST_SET, 0.1);
return 0;
}
- int control_algorithm(log_t* log_struct, user_input_t * user_input_struct, sensor_t* sensor_struct, setpoint_t* setpoint_struct, parameter_t* parameter_struct, user_defined_t* user_defined_struct, actuator_command_t* actuator_struct, modular_structs_t* structs)
+ int control_algorithm(log_t* log_struct, user_input_t * user_input_struct, sensor_t* sensor_struct, setpoint_t* setpoint_struct, parameter_t* ps, user_defined_t* user_defined_struct, actuator_command_t* actuator_struct, modular_structs_t* structs)
{
- struct computation_graph* graph = parameter_struct->graph;
+ struct computation_graph* graph = ps->graph;
// use the 'flap' switch as the flight mode selector
int cur_fm_switch = read_flap(user_input_struct->rc_commands[FLAP]);
static int last_fm_switch = MANUAL_FLIGHT_MODE;
@@ -218,7 +191,7 @@ int control_algorithm_init(parameter_t * ps)
// before actually engaging AUTO mode
if(cur_fm_switch == MANUAL_FLIGHT_MODE) {
if (last_fm_switch == AUTO_FLIGHT_MODE) {
- connect_manual(parameter_struct);
+ connect_manual(ps);
}
user_defined_struct->flight_mode = MANUAL_FLIGHT_MODE;
}
@@ -228,7 +201,7 @@ int control_algorithm_init(parameter_t * ps)
{
user_defined_struct->engaging_auto = 1;
- graph_set_param_val(graph, parameter_struct->throttle_trim, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
+ graph_set_param_val(graph, ps->throttle_trim, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
log_struct->trims.roll = sensor_struct->trims.roll;
log_struct->trims.pitch = sensor_struct->trims.pitch;
@@ -245,17 +218,17 @@ int control_algorithm_init(parameter_t * ps)
// also reset the previous error and accumulated error from the position PIDs
if((cur_fm_switch == AUTO_FLIGHT_MODE) && (user_defined_struct->engaging_auto == 2))
{
- graph_set_param_val(graph, parameter_struct->x_set, CONST_SET, sensor_struct->currentQuadPosition.x_pos);
- graph_set_param_val(graph, parameter_struct->y_set, CONST_SET, sensor_struct->currentQuadPosition.y_pos);
- graph_set_param_val(graph, parameter_struct->alt_set, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
- graph_set_param_val(graph, parameter_struct->yaw_set, CONST_SET, sensor_struct->currentQuadPosition.yaw);
+ graph_set_param_val(graph, ps->x_set, CONST_SET, sensor_struct->currentQuadPosition.x_pos);
+ graph_set_param_val(graph, ps->y_set, CONST_SET, sensor_struct->currentQuadPosition.y_pos);
+ graph_set_param_val(graph, ps->alt_set, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
+ graph_set_param_val(graph, ps->yaw_set, CONST_SET, sensor_struct->currentQuadPosition.yaw);
// reset the flag that engages auto mode
user_defined_struct->engaging_auto = 0;
// finally engage the AUTO_FLIGHT_MODE
// this ensures that we've gotten a new update packet right after the switch was set to auto mode
user_defined_struct->flight_mode = AUTO_FLIGHT_MODE;
- connect_autonomous(parameter_struct);
+ connect_autonomous(ps);
}
//PIDS///////////////////////////////////////////////////////////////////////
@@ -268,49 +241,49 @@ int control_algorithm_init(parameter_t * ps)
if(user_input_struct->locationFresh)
{
// VRPN values
- graph_set_param_val(graph, parameter_struct->vrpn_x, CONST_SET, sensor_struct->currentQuadPosition.x_pos);
- graph_set_param_val(graph, parameter_struct->vrpn_y, CONST_SET, sensor_struct->currentQuadPosition.y_pos);
- graph_set_param_val(graph, parameter_struct->vrpn_alt, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
- graph_set_param_val(graph, parameter_struct->cur_yaw, CONST_SET, sensor_struct->currentQuadPosition.yaw);
+ graph_set_param_val(graph, ps->vrpn_x, CONST_SET, sensor_struct->currentQuadPosition.x_pos);
+ graph_set_param_val(graph, ps->vrpn_y, CONST_SET, sensor_struct->currentQuadPosition.y_pos);
+ graph_set_param_val(graph, ps->vrpn_alt, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
+ graph_set_param_val(graph, ps->cur_yaw, CONST_SET, sensor_struct->currentQuadPosition.yaw);
}
// Sensor values
- graph_set_param_val(graph, parameter_struct->cur_pitch, CONST_SET, sensor_struct->pitch_angle_filtered);
- graph_set_param_val(graph, parameter_struct->cur_roll, CONST_SET, sensor_struct->roll_angle_filtered);
- graph_set_param_val(graph, parameter_struct->theta_dot, CONST_SET, sensor_struct->theta_dot);
- graph_set_param_val(graph, parameter_struct->phi_dot, CONST_SET, sensor_struct->phi_dot);
- graph_set_param_val(graph, parameter_struct->psi_dot, CONST_SET, sensor_struct->psi_dot);
+ graph_set_param_val(graph, ps->cur_pitch, CONST_SET, sensor_struct->pitch_angle_filtered);
+ graph_set_param_val(graph, ps->cur_roll, CONST_SET, sensor_struct->roll_angle_filtered);
+ graph_set_param_val(graph, ps->theta_dot, CONST_SET, sensor_struct->theta_dot);
+ graph_set_param_val(graph, ps->phi_dot, CONST_SET, sensor_struct->phi_dot);
+ graph_set_param_val(graph, ps->psi_dot, CONST_SET, sensor_struct->psi_dot);
// RC values
- graph_set_param_val(graph, parameter_struct->rc_pitch, CONST_SET, user_input_struct->pitch_angle_manual_setpoint);
- graph_set_param_val(graph, parameter_struct->rc_roll, CONST_SET, user_input_struct->roll_angle_manual_setpoint);
- graph_set_param_val(graph, parameter_struct->rc_yaw, CONST_SET, user_input_struct->yaw_manual_setpoint);
- graph_set_param_val(graph, parameter_struct->rc_throttle, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
+ graph_set_param_val(graph, ps->rc_pitch, CONST_SET, user_input_struct->pitch_angle_manual_setpoint);
+ graph_set_param_val(graph, ps->rc_roll, CONST_SET, user_input_struct->roll_angle_manual_setpoint);
+ graph_set_param_val(graph, ps->rc_yaw, CONST_SET, user_input_struct->yaw_manual_setpoint);
+ graph_set_param_val(graph, ps->rc_throttle, CONST_SET, user_input_struct->rc_commands[THROTTLE]);
- int outputs[1] = {parameter_struct->mixer};
+ int outputs[1] = {ps->mixer};
graph_compute_nodes(graph, outputs, 1);
// here for now so in case any flight command is not PID controlled, it will default to rc_command value:
//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))
- //{
+ if((user_input_struct->rc_commands[THROTTLE] >
+ 118000) || (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE))
+ {
//THROTTLE
- actuator_struct->pwms[0] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM0);
+ actuator_struct->pwms[0] = graph_get_output(graph, ps->mixer, MIXER_PWM0);
- actuator_struct->pwms[1] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM1);
+ actuator_struct->pwms[1] = graph_get_output(graph, ps->mixer, MIXER_PWM1);
- actuator_struct->pwms[2] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM2);
+ actuator_struct->pwms[2] = graph_get_output(graph, ps->mixer, MIXER_PWM2);
- actuator_struct->pwms[3] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM3);
- //}
- /*else
+ actuator_struct->pwms[3] = graph_get_output(graph, ps->mixer, MIXER_PWM3);
+ }
+ else
{
actuator_struct->pwms[0] = user_input_struct->rc_commands[THROTTLE];
actuator_struct->pwms[1] = user_input_struct->rc_commands[THROTTLE];
actuator_struct->pwms[2] = user_input_struct->rc_commands[THROTTLE];
actuator_struct->pwms[3] = user_input_struct->rc_commands[THROTTLE];
- }*/
+ }
//logging
// here we are not actually duplicating the logging from the PID computation
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index efc37f2f8849c26397c74674dcd00bf3dfefaa46..ba125fa40c917a2fe453fc838f7fd3fdac60dee6 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/quad/sw/modular_quad_pid/src/type_def.h
@@ -321,16 +321,11 @@ typedef struct parameter_t {
int y_set;
int alt_set;
int yaw_set;
- // Loop time
- int dt;
+ // Loop times
+ int angle_time;
+ int pos_time;
// Signal mixer
int mixer;
- // Clamping blocks
- int clamp_pitch;
- int clamp_roll;
- int clamp_d_pwmR; // Clamp the change in PWM values for roll
- int clamp_d_pwmP; // ... pitch
- int clamp_d_pwmY; // ... yaw
// "trim" for autonomous
int throttle_trim;
int throttle_trim_add;