diff --git a/quad/sw/modular_quad_pid/gen_diagram/gen_diagram b/quad/sw/modular_quad_pid/gen_diagram/gen_diagram
index 5ecd8da3b67c09fcecc9879100ab1bc41dd31b42..c39dfb12558e81c2f9a4d6d6932afa7e726249ba 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 2d0ae44ff6860531b5914db9576a75986c681ad9..8ebddda8b107ed93a0ca7dfd9d35f1b9ee11b33e 100644
--- a/quad/sw/modular_quad_pid/gen_diagram/network.dot
+++ b/quad/sw/modular_quad_pid/gen_diagram/network.dot
@@ -12,6 +12,9 @@ label="<f0>Pitch PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4>
"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]"]
+"Yaw" -> "Yaw PID":f1 [label="Constant"]
+"Yaw Setpoint" -> "Yaw PID":f2 [label="Constant"]
+"dT" -> "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"]
@@ -28,11 +31,34 @@ label="<f0>Yaw Rate PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f
"RC Yaw" -> "Yaw Rate PID":f2 [label="Constant"]
"dT" -> "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.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000]"]
+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"]
"Y pos PID"[shape=record
-label="<f0>Y pos PID |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000]"]
-"Altitude"[shape=record
-label="<f0>Altitude |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000]"]
+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"]
+"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"]
+"X Setpoint"[shape=record
+label="<f0>X Setpoint |<f1> [Constant=0.000]"]
+"Y Setpoint"[shape=record
+label="<f0>Y Setpoint |<f1> [Constant=0.000]"]
+"Alt Setpoint"[shape=record
+label="<f0>Alt Setpoint |<f1> [Constant=0.000]"]
+"Yaw Setpoint"[shape=record
+label="<f0>Yaw Setpoint |<f1> [Constant=0.000]"]
+"Throttle trim"[shape=record
+label="<f0>Throttle trim |<f1> [Constant=0.000]"]
+"T trim add"[shape=record
+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"]
@@ -60,6 +86,12 @@ label="<f0>dTheta |<f1> [Constant=0.000]"]
label="<f0>dPhi |<f1> [Constant=0.000]"]
"dPsi"[shape=record
label="<f0>dPsi |<f1> [Constant=0.000]"]
+"VRPN X"[shape=record
+label="<f0>VRPN X |<f1> [Constant=0.000]"]
+"VRPN Y"[shape=record
+label="<f0>VRPN Y |<f1> [Constant=0.000]"]
+"VRPN Alt"[shape=record
+label="<f0>VRPN Alt |<f1> [Constant=0.000]"]
"RC Pitch"[shape=record
label="<f0>RC Pitch |<f1> [Constant=0.000]"]
"RC Roll"[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 f68b5ba73ade931574e1f9ebfd61edb93bd5c293..1bd106753a9455c264e618a90ad12518196160f2 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 3623d4d6f92a3c0c2e67af047d9f919fff9e4165..ba6a8733b2589b7fe044188b505e1279d750798f 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.c
+++ b/quad/sw/modular_quad_pid/src/control_algorithm.c
@@ -12,13 +12,30 @@
#include "graph_blocks/node_bounds.h"
#include "graph_blocks/node_constant.h"
#include "graph_blocks/node_mixer.h"
+#include "graph_blocks/node_add.h"
#include "PID.h"
#include "util.h"
#define ROLL_PITCH_MAX_ANGLE 0.3490 // 20 degrees
- int control_algorithm_init(parameter_t * ps)
- {
+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->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);
+}
+
+int control_algorithm_init(parameter_t * ps)
+{
struct computation_graph* graph = create_graph();
ps->graph = graph;
@@ -31,7 +48,13 @@
ps->yaw_r_pid = graph_add_node_pid(graph, "Yaw Rate PID");
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");
+ ps->alt_pid = graph_add_node_pid(graph, "Altitude PID");
+ ps->x_set = graph_add_node_const(graph, "X Setpoint");
+ 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");
+ 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");
@@ -53,6 +76,11 @@
ps->phi_dot = graph_add_node_const(graph, "dPhi");
ps->psi_dot = graph_add_node_const(graph, "dPsi");
+ // Create blocks for VRPN data
+ ps->vrpn_x = graph_add_node_const(graph, "VRPN X");
+ ps->vrpn_y = graph_add_node_const(graph, "VRPN Y");
+ ps->vrpn_alt = graph_add_node_const(graph, "VRPN Alt");
+
// Create blocks for RC controller
ps->rc_pitch = graph_add_node_const(graph, "RC Pitch");
ps->rc_roll = graph_add_node_const(graph, "RC Roll");
@@ -67,7 +95,7 @@
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_pid, PID_SETPOINT, ps->rc_pitch, 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);
@@ -75,7 +103,7 @@
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_pid, PID_SETPOINT, ps->rc_roll, 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);
@@ -83,15 +111,30 @@
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_pid, PID_SETPOINT, ps->rc_yaw, CONST_VAL);
- graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->yaw, CONST_VAL);
- graph_set_source(graph, ps->yaw_pid, PID_DT, ps->dt, CONST_VAL);
- */
- // Connect angle clamping blocks
- graph_set_source(graph, ps->clamp_pitch, BOUNDS_IN, ps->y_pos_pid, PID_CORRECTION);
+
+ // 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->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->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_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_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);
@@ -99,11 +142,14 @@
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_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);
+ // Set initial mode
+ connect_manual(ps);
+
// Set pitch PID constants
graph_set_param_val(graph, ps->pitch_pid, PID_KP, PITCH_ANGLE_KP);
graph_set_param_val(graph, ps->pitch_pid, PID_KI, PITCH_ANGLE_KI);
@@ -128,6 +174,18 @@
graph_set_param_val(graph, ps->yaw_r_pid, PID_KI, YAW_ANGULAR_VELOCITY_KI);
graph_set_param_val(graph, ps->yaw_r_pid, PID_KD, YAW_ANGULAR_VELOCITY_KD);
+ // Set X/Y/Alt constants
+ graph_set_param_val(graph, ps->x_pos_pid, PID_KP, XPOS_KP);
+ graph_set_param_val(graph, ps->x_pos_pid, PID_KI, XPOS_KI);
+ graph_set_param_val(graph, ps->x_pos_pid, PID_KD, XPOS_KD);
+ graph_set_param_val(graph, ps->y_pos_pid, PID_KP, YPOS_KP);
+ graph_set_param_val(graph, ps->y_pos_pid, PID_KI, YPOS_KI);
+ graph_set_param_val(graph, ps->y_pos_pid, PID_KD, YPOS_KD);
+ graph_set_param_val(graph, ps->alt_pid, PID_KP, ALT_ZPOS_KP);
+ 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);
@@ -161,23 +219,12 @@
if(cur_fm_switch == MANUAL_FLIGHT_MODE)
user_defined_struct->flight_mode = MANUAL_FLIGHT_MODE;
- static float roll_trim = 0.0;
- static float pitch_trim = 0.0;
-
// flap switch was just toggled to auto flight mode
if((last_fm_switch != cur_fm_switch) && (cur_fm_switch == AUTO_FLIGHT_MODE))
{
user_defined_struct->engaging_auto = 1;
- // Read in trimmed values because it should read trim values right when the pilot flips the flight mode switch
- pitch_trim = user_input_struct->pitch_angle_manual_setpoint; //rc_commands[PITCH] - PITCH_CENTER;
- roll_trim = user_input_struct->roll_angle_manual_setpoint; //rc_commands[ROLL] - ROLL_CENTER;
- //sensor_struct->trimmedRCValues.yaw = yaw_manual_setpoint; //rc_commands[YAW] - YAW_CENTER;
-
-// sensor_struct->trims.roll = raw_actuator_struct->controller_corrected_motor_commands[ROLL];
-// sensor_struct->trims.pitch = raw_actuator_struct->controller_corrected_motor_commands[PITCH];
-// sensor_struct->trims.yaw = raw_actuator_struct->controller_corrected_motor_commands[YAW];
- sensor_struct->trims.throttle = user_input_struct->rc_commands[THROTTLE];
+ graph_set_param_val(graph, parameter_struct->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;
@@ -194,26 +241,13 @@
// 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))
{
- // TODO: Change this to reconnect PIDs
- /*
- // zero out the accumulated error so the I terms don't cause wild things to happen
- parameter_struct->pid_controllers[ALT_ID].acc_error = 0.0;
- parameter_struct->pid_controllers[LOCAL_X_ID].acc_error = 0.0;
- parameter_struct->pid_controllers[LOCAL_Y_ID].acc_error = 0.0;
-
- // make previous error equal to the current so the D term doesn't spike
- parameter_struct->pid_controllers[ALT_ID].prev_error = 0.0;
- parameter_struct->pid_controllers[LOCAL_X_ID].prev_error = 0.0;
- parameter_struct->pid_controllers[LOCAL_Y_ID].prev_error = 0.0;
-
- setpoint_struct->desiredQuadPosition.alt_pos = sensor_struct->currentQuadPosition.alt_pos;
- setpoint_struct->desiredQuadPosition.x_pos = sensor_struct->currentQuadPosition.x_pos;
- setpoint_struct->desiredQuadPosition.y_pos = sensor_struct->currentQuadPosition.y_pos;
- setpoint_struct->desiredQuadPosition.yaw = 0.0;//currentQuadPosition.yaw;
+ 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);
// 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;
@@ -226,61 +260,26 @@
* a pitch or roll for the angle loop PIDs
*/
-// static int counter_between_packets = 0;
-
if(user_input_struct->locationFresh)
{
- // TODO: Change this to populate VRPN block
- /*
- parameter_struct->pid_controllers[LOCAL_Y_ID].current_point = sensor_struct->currentQuadPosition.y_pos;
- parameter_struct->pid_controllers[LOCAL_Y_ID].setpoint = setpoint_struct->desiredQuadPosition.y_pos;
-
- parameter_struct->pid_controllers[LOCAL_X_ID].current_point = sensor_struct->currentQuadPosition.x_pos;
- parameter_struct->pid_controllers[LOCAL_X_ID].setpoint = setpoint_struct->desiredQuadPosition.x_pos;
-
- parameter_struct->pid_controllers[ALT_ID].current_point = sensor_struct->currentQuadPosition.alt_pos;
- parameter_struct->pid_controllers[ALT_ID].setpoint = setpoint_struct->desiredQuadPosition.alt_pos;
- */
- //logging and PID computation
- //log_struct->local_y_PID_values = pid_computation(&(parameter_struct->pid_controllers[LOCAL_Y_ID]));
- //log_struct->local_x_PID_values = pid_computation(&(parameter_struct->pid_controllers[LOCAL_X_ID]));
- //log_struct->altitude_PID_values = pid_computation(&(parameter_struct->pid_controllers[ALT_ID]));
-
- // yaw angular position PID calculation
- graph_set_param_val(graph, parameter_struct->cur_yaw, CONST_SET, sensor_struct->currentQuadPosition.yaw);
- // TODO: Set yaw setpoint
- //parameter_struct->pid_controllers[YAW_ID].current_point = sensor_struct->currentQuadPosition.yaw;// in radians
- //parameter_struct->pid_controllers[YAW_ID].setpoint = setpoint_struct->desiredQuadPosition.yaw; // constant setpoint
-
- //logging and PID computation
- //log_struct->angle_yaw_PID_values = pid_computation(&(parameter_struct->pid_controllers[YAW_ID]));
-
+ // 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);
}
-
+ // 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);
-
+ // 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]);
-
- /*parameter_struct->pid_controllers[PITCH_ID].setpoint =
- (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
- (parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction) + pitch_trim : user_input_struct->pitch_angle_manual_setpoint;
-
- parameter_struct->pid_controllers[ROLL_ID].setpoint =
- (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
- (parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction) + roll_trim : user_input_struct->roll_angle_manual_setpoint;
- */
- /*
- int outputs[3] = {parameter_struct->pitch_r_pid,
- parameter_struct->roll_r_pid,
- parameter_struct->yaw_r_pid};*/
int outputs[1] = {parameter_struct->mixer};
graph_compute_nodes(graph, outputs, 1);
@@ -291,39 +290,6 @@
if((user_input_struct->rc_commands[THROTTLE] >
118000) || (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE))
{
-
- if(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)
- {
- /*
- //THROTTLE
- raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] =
- ((int)(parameter_struct->pid_controllers[ALT_ID].pid_correction)) + sensor_struct->trims.throttle;
-
- //ROLL
- raw_actuator_struct->controller_corrected_motor_commands[ROLL] =
- parameter_struct->pid_controllers[ROLL_RATE_ID].pid_correction; // + sensor_struct->trims.roll;
-
- //PITCH
- raw_actuator_struct->controller_corrected_motor_commands[PITCH] =
- parameter_struct->pid_controllers[PITCH_RATE_ID].pid_correction; // + sensor_struct->trims.pitch;
-
- //YAW
- raw_actuator_struct->controller_corrected_motor_commands[YAW] =
- parameter_struct->pid_controllers[YAW_RATE_ID].pid_correction;// + sensor_struct->trims.yaw;
- */
-
-// static int slow_down = 0;
-// slow_down++;
-// if(slow_down % 50 == 0)
-// printf("X: %.3f\tY: %.3f\tZ: %.3f\tX_s: %.3f\tX_c: %.3f\tY_s: %.3f\tY_c: %.3f\tZ_s: %.3f\tZ_c: %.3f\t\n",
-// parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction,
-// parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction,
-// parameter_struct->pid_controllers[ALT_ID].pid_correction,
-// parameter_struct->pid_controllers[LOCAL_X_ID].setpoint, parameter_struct->pid_controllers[LOCAL_X_ID].current_point,
-// parameter_struct->pid_controllers[LOCAL_Y_ID].setpoint, parameter_struct->pid_controllers[LOCAL_Y_ID].current_point,
-// parameter_struct->pid_controllers[ALT_ID].setpoint, parameter_struct->pid_controllers[ALT_ID].current_point);
- }
- else{
//THROTTLE
actuator_struct->pwms[0] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM0);
@@ -332,32 +298,6 @@
actuator_struct->pwms[2] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM2);
actuator_struct->pwms[3] = graph_get_output(graph, parameter_struct->mixer, MIXER_PWM3);
- }
-
- /*
- //BOUNDS CHECKING
- if(raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] < 0)
- raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] = 0;
-
- //BOUNDS CHECKING
- if(raw_actuator_struct->controller_corrected_motor_commands[ROLL] > 20000)
- raw_actuator_struct->controller_corrected_motor_commands[ROLL] = 20000;
-
- if(raw_actuator_struct->controller_corrected_motor_commands[ROLL] < -20000)
- raw_actuator_struct->controller_corrected_motor_commands[ROLL] = -20000;
-
- if(raw_actuator_struct->controller_corrected_motor_commands[PITCH] > 20000)
- raw_actuator_struct->controller_corrected_motor_commands[PITCH] = 20000;
-
- if(raw_actuator_struct->controller_corrected_motor_commands[PITCH] < -20000)
- raw_actuator_struct->controller_corrected_motor_commands[PITCH] = -20000;
-
- if(raw_actuator_struct->controller_corrected_motor_commands[YAW] > 20000)
- raw_actuator_struct->controller_corrected_motor_commands[YAW] = 20000;
-
- if(raw_actuator_struct->controller_corrected_motor_commands[YAW] < -20000)
- raw_actuator_struct->controller_corrected_motor_commands[YAW] = -20000;
- */
}
else
{
@@ -365,10 +305,6 @@
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];
- /*
- raw_actuator_struct->controller_corrected_motor_commands[ROLL] = 0;
- raw_actuator_struct->controller_corrected_motor_commands[PITCH] = 0;
- raw_actuator_struct->controller_corrected_motor_commands[YAW] = 0;*/
}
//logging
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_add.c b/quad/sw/modular_quad_pid/src/graph_blocks/node_add.c
new file mode 120000
index 0000000000000000000000000000000000000000..d7025e0c28ea1028022b77e23924a7734ee55e00
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_add.c
@@ -0,0 +1 @@
+../../../../computation_graph/src/graph_blocks/node_add.c
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_add.h b/quad/sw/modular_quad_pid/src/graph_blocks/node_add.h
new file mode 120000
index 0000000000000000000000000000000000000000..4f4b0ea11eb74e5c45426122afb754f13b098266
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_add.h
@@ -0,0 +1 @@
+../../../../computation_graph/src/graph_blocks/node_add.h
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index cdc34ce167adaa61f410570fda6edb13f866293a..efc37f2f8849c26397c74674dcd00bf3dfefaa46 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/quad/sw/modular_quad_pid/src/type_def.h
@@ -307,11 +307,20 @@ typedef struct parameter_t {
int theta_dot;
int phi_dot;
int psi_dot;
+ // VRPN blocks
+ int vrpn_x;
+ int vrpn_y;
+ int vrpn_alt;
// RC blocks
int rc_pitch;
int rc_roll;
int rc_yaw;
int rc_throttle;
+ // Desired positions
+ int x_set;
+ int y_set;
+ int alt_set;
+ int yaw_set;
// Loop time
int dt;
// Signal mixer
@@ -322,6 +331,9 @@ typedef struct parameter_t {
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;
} parameter_t;
/**