diff --git a/quad/sw/modular_quad_pid/src/computation_graph.c b/quad/sw/modular_quad_pid/src/computation_graph.c
new file mode 120000
index 0000000000000000000000000000000000000000..b4ff9ed9c10b3956ed2f586c7e3fab989e5f550d
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/computation_graph.c
@@ -0,0 +1 @@
+computation_graph/src/computation_graph.c
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/computation_graph.h b/quad/sw/modular_quad_pid/src/computation_graph.h
new file mode 120000
index 0000000000000000000000000000000000000000..b8b749d0f0e89c67392f7d4d610cddf7e44bb374
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/computation_graph.h
@@ -0,0 +1 @@
+computation_graph/src/computation_graph.h
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/control_algorithm.c b/quad/sw/modular_quad_pid/src/control_algorithm.c
index 63ba756137739578a39f2609048a3728027266c7..757cc8a9b628a10fd5a547bdb9ed6730766f28a7 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.c
+++ b/quad/sw/modular_quad_pid/src/control_algorithm.c
@@ -6,58 +6,111 @@
*/
// This implemented modular quadrotor software implements a PID control algorithm
-
+
#include "control_algorithm.h"
#include "communication.h"
+#include "graph_blocks/node_pid.h"
+#include "graph_blocks/node_bounds.h"
+#include "graph_blocks/node_constant.h"
#define ROLL_PITCH_MAX_ANGLE 0.3490 // 20 degrees
- int control_algorithm_init(parameter_t * parameter_struct)
+ int control_algorithm_init(parameter_t * ps)
{
- // HUMAN Piloted (RC) PID DEFINITIONS //////
- // RC PIDs for roll (2 loops: angle --> angular velocity)
- parameter_struct->pid_controllers[ROLL_ID].dt = 0.005; parameter_struct->pid_controllers[ROLL_RATE_ID].dt = 0.005; // 5 ms calculation period
-
- // RC PIDs for pitch (2 loops: angle --> angular velocity)
- parameter_struct->pid_controllers[PITCH_ID].dt = 0.005; parameter_struct->pid_controllers[PITCH_RATE_ID].dt = 0.005; // 5 ms calculation period
-
- // initialize Yaw PID_t and PID constants
- // RC PID for yaw (1 loop angular velocity)
- parameter_struct->pid_controllers[YAW_RATE_ID].dt = 0.005; // 5 ms calculation period
-
- // AUTOMATIC Pilot (Position) PID DEFINITIONS //////
- // Local X PID using a translation from X camera system data to quad local X position (3 loops: local y position --> angle --> angular velocity)
- parameter_struct->pid_controllers[LOCAL_X_ID].dt = 0.100;
-
- // Local Y PID using a translation from Y camera system data to quad local Y position(3 loops: local x position --> angle --> angular velocity)
- parameter_struct->pid_controllers[LOCAL_Y_ID].dt = 0.100;
-
- // CAM PIDs for yaw (2 loops angle --> angular velocity)
- parameter_struct->pid_controllers[YAW_ID].dt = 0.100;
-
- // CAM PID for altitude (1 loop altitude)
- parameter_struct->pid_controllers[ALT_ID].dt = 0.100;
-
- // PID coeffiecients (Position)
- setPIDCoeff(&(parameter_struct->pid_controllers[LOCAL_Y_ID]), YPOS_KP, YPOS_KI, YPOS_KD);
- setPIDCoeff(&(parameter_struct->pid_controllers[LOCAL_X_ID]), XPOS_KP, XPOS_KI, XPOS_KD);
- setPIDCoeff(&(parameter_struct->pid_controllers[ALT_ID]), ALT_ZPOS_KP, ALT_ZPOS_KI, ALT_ZPOS_KD);
-
- // PID coefficients (Angle)
- setPIDCoeff(&(parameter_struct->pid_controllers[PITCH_ID]), PITCH_ANGLE_KP, PITCH_ANGLE_KI, PITCH_ANGLE_KD);
- setPIDCoeff(&(parameter_struct->pid_controllers[ROLL_ID]), ROLL_ANGLE_KP, ROLL_ANGLE_KI, ROLL_ANGLE_KD);
- setPIDCoeff(&(parameter_struct->pid_controllers[YAW_ID]), YAW_ANGLE_KP, YAW_ANGLE_KI, YAW_ANGLE_KD);
-
- // PID coefficients (Angular Velocity)
- setPIDCoeff(&(parameter_struct->pid_controllers[PITCH_RATE_ID]), PITCH_ANGULAR_VELOCITY_KP, PITCH_ANGULAR_VELOCITY_KI, PITCH_ANGULAR_VELOCITY_KD);
- setPIDCoeff(&(parameter_struct->pid_controllers[ROLL_RATE_ID]), ROLL_ANGULAR_VELOCITY_KP, ROLL_ANGULAR_VELOCITY_KI, ROLL_ANGULAR_VELOCITY_KD);
- setPIDCoeff(&(parameter_struct->pid_controllers[YAW_RATE_ID]), YAW_ANGULAR_VELOCITY_KP, YAW_ANGULAR_VELOCITY_KI, YAW_ANGULAR_VELOCITY_KD);
+ struct computation_graph* graph = create_graph();
+ ps->graph = graph;
+
+ // Create all the PID blocks
+ ps->roll_pid = graph_add_node_pid("Roll PID");
+ ps->pitch_pid = graph_add_node_pid("Pitch PID");
+ ps->yaw_pid = graph_add_node_pid("Yaw PID");
+ ps->roll_r_pid = graph_add_node_pid("Roll Rate PID");
+ ps->pitch_r_pid = graph_add_node_pid("Pitch Rate PID");
+ ps->yaw_r_pid = graph_add_node_pid("Yaw Rate PID");
+ ps->x_pos_pid = graph_add_node_pid("X pos PID PID");
+ ps->y_pos_pid = graph_add_node_pid("Y pos PID");
+ ps->alt_pid = graph_add_node_pid("Altitude");
+
+ // Create blocks for sensor inputs
+ ps->cur_pitch = graph_add_node_const("Pitch");
+ ps->cur_roll = graph_add_node_const("Roll");
+ ps->cur_yaw = graph_add_node_const("Yaw");
+ // Yaw angular velocity PID
+ // theta_dot is the angular velocity about the y-axis
+ // phi_dot is the angular velocity about the x-axis
+ // psi_dot is the angular velocity about the z-axis
+ // These are calculated from using the gimbal equations
+ ps->theta = graph_add_node_const("Theta");
+ ps->phi = graph_add_node_const("Phi");
+ ps->psi = graph_add_node_const("Psi");
+
+ // Create blocks for RC controller
+ ps->rc_pitch = graph_add_node_const("RC Pitch");
+ ps->rc_roll = graph_add_node_const("RC Roll");
+ ps->rc_yaw = graph_add_node_const("RC Yaw");
+ ps->rc_throttle = graph_add_node_const("RC Throttle");
+
+ ps->dt = graph_add_node_const("dT");
+
+ // 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, 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_CUR_POINT, ps->pitch, CONST_VAL);
+ graph_set_source(graph, ps->pitch_pid, PID_DT, ps->dt, 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, 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_CUR_POINT, ps->roll, CONST_VAL);
+ graph_set_source(graph, ps->roll_pid, PID_DT, ps->dt, 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, 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);
+ */
+
+ // 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);
+ graph_set_param_val(graph, ps->pitch_pid, PID_KD, PITCH_ANGLE_KD);
+ graph_set_param_val(graph, ps->pitch_r_pid, PID_KP, PITCH_ANGULAR_VELOCITY_KP);
+ graph_set_param_val(graph, ps->pitch_r_pid, PID_KI, PITCH_ANGULAR_VELOCITY_KI);
+ graph_set_param_val(graph, ps->pitch_r_pid, PID_KD, PITCH_ANGULAR_VELOCITY_KD);
+
+ // Set roll PID constants
+ graph_set_param_val(graph, ps->roll_pid, PID_KP, ROLL_ANGLE_KP);
+ graph_set_param_val(graph, ps->roll_pid, PID_KI, ROLL_ANGLE_KI);
+ graph_set_param_val(graph, ps->roll_pid, PID_KD, ROLL_ANGLE_KD);
+ graph_set_param_val(graph, ps->roll_r_pid, PID_KP, ROLL_ANGULAR_VELOCITY_KP);
+ graph_set_param_val(graph, ps->roll_r_pid, PID_KI, ROLL_ANGULAR_VELOCITY_KI);
+ graph_set_param_val(graph, ps->roll_r_pid, PID_KD, ROLL_ANGULAR_VELOCITY_KD);
+
+ // Set yaw PID constants
+ graph_set_param_val(graph, ps->yaw_pid, PID_KP, YAW_ANGLE_KP);
+ graph_set_param_val(graph, ps->yaw_pid, PID_KI, YAW_ANGLE_KI);
+ graph_set_param_val(graph, ps->yaw_pid, PID_KD, YAW_ANGLE_KD);
+ graph_set_param_val(graph, ps->yaw_r_pid, PID_KP, YAW_ANGULAR_VELOCITY_KP);
+ 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);
+
+ // TODO: Change this to use LOOP_TIME
+ graph_set_param_val(graph, ps->dt, CONST_SET, 0.005);
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, raw_actuator_t* raw_actuator_struct, modular_structs_t* structs)
{
+ struct computation_graph* graph = parameter_struct->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;
@@ -101,6 +154,8 @@
// 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;
@@ -118,7 +173,7 @@
// 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;
@@ -135,6 +190,8 @@
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;
@@ -143,90 +200,46 @@
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]));
+ //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
- 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
+ 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]));
+ //log_struct->angle_yaw_PID_values = pid_computation(&(parameter_struct->pid_controllers[YAW_ID]));
}
+ 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->cur_theta, CONST_SET, sensor_struct->theta_dot);
+ graph_set_param_val(graph, parameter_struct->cur_phi, CONST_SET, sensor_struct->phi_dot);
+ graph_set_param_val(graph, parameter_struct->cur_psi, CONST_SET, sensor_struct->psi_dot);
- /* Angle loop
- * Calculates current orientation, and outputs
- * a pitch, roll, or yaw velocity for the angular velocity loop PIDs
- */
+ 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);
- //angle boundaries
- if(parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction > ROLL_PITCH_MAX_ANGLE)
- {
- parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction = ROLL_PITCH_MAX_ANGLE;
- }
- if(parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction < -ROLL_PITCH_MAX_ANGLE)
- {
- parameter_struct->pid_controllers[LOCAL_X_ID].pid_correction = -ROLL_PITCH_MAX_ANGLE;
- }
- if(parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction > ROLL_PITCH_MAX_ANGLE)
- {
- parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction = ROLL_PITCH_MAX_ANGLE;
- }
- if(parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction < -ROLL_PITCH_MAX_ANGLE)
- {
- parameter_struct->pid_controllers[LOCAL_Y_ID].pid_correction = -ROLL_PITCH_MAX_ANGLE;
- }
- parameter_struct->pid_controllers[PITCH_ID].current_point = sensor_struct->pitch_angle_filtered;
- 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[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].current_point = sensor_struct->roll_angle_filtered;
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;
-
-
- //logging and PID computation
- log_struct->angle_pitch_PID_values = pid_computation(&(parameter_struct->pid_controllers[PITCH_ID]));
- log_struct->angle_roll_PID_values = pid_computation(&(parameter_struct->pid_controllers[ROLL_ID]));
-
-
- /* Angular Velocity Loop
- * Takes the desired angular velocity from the angle loop,
- * and calculates a PID correction with the current angular velocity
- */
-
- // theta_dot is the angular velocity about the y-axis
- // it is calculated from using the gimbal equations
- parameter_struct->pid_controllers[PITCH_RATE_ID].current_point = sensor_struct->theta_dot;
- parameter_struct->pid_controllers[PITCH_RATE_ID].setpoint = parameter_struct->pid_controllers[PITCH_ID].pid_correction;
-
- // phi_dot is the angular velocity about the x-axis
- // it is calculated from using the gimbal equations
- parameter_struct->pid_controllers[ROLL_RATE_ID].current_point = sensor_struct->phi_dot;
- parameter_struct->pid_controllers[ROLL_RATE_ID].setpoint = parameter_struct->pid_controllers[ROLL_ID].pid_correction;
-
- // Yaw angular velocity PID
- // psi_dot is the angular velocity about the z-axis
- // it is calculated from using the gimbal equations
- parameter_struct->pid_controllers[YAW_RATE_ID].current_point = sensor_struct->psi_dot;
- parameter_struct->pid_controllers[YAW_RATE_ID].setpoint = /*(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
- parameter_struct->pid_controllers[YAW_ID].pid_correction :*/ user_input_struct->yaw_manual_setpoint; // no trim added because the controller already works well
-
- //logging and PID computation
- log_struct->ang_vel_pitch_PID_values = pid_computation(&(parameter_struct->pid_controllers[PITCH_RATE_ID]));
- log_struct->ang_vel_roll_PID_values = pid_computation(&(parameter_struct->pid_controllers[ROLL_RATE_ID]));
- log_struct->ang_vel_yaw_PID_values = pid_computation(&(parameter_struct->pid_controllers[YAW_RATE_ID]));
-
- //END PIDs///////////////////////////////////////////////////////////////////////
+ (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;
+ */
+ graph_compute_node(graph, parameter_struct->pitch_r_pid);
+ graph_compute_node(graph, parameter_struct->roll_r_pid);
+ graph_compute_node(graph, parameter_struct->yaw_r_pid);
// 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);
@@ -238,6 +251,7 @@
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;
@@ -253,6 +267,7 @@
//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++;
@@ -268,17 +283,18 @@
else{
//ROLL
raw_actuator_struct->controller_corrected_motor_commands[ROLL] =
- parameter_struct->pid_controllers[ROLL_RATE_ID].pid_correction;
+ graph_get_output(graph, parameter_struct->roll_r_pid, PID_CORRECTION);
//PITCH
raw_actuator_struct->controller_corrected_motor_commands[PITCH] =
- parameter_struct->pid_controllers[PITCH_RATE_ID].pid_correction;
+ graph_get_output(graph, parameter_struct->pitch_r_pid, PID_CORRECTION);
//YAW
raw_actuator_struct->controller_corrected_motor_commands[YAW] =
- parameter_struct->pid_controllers[YAW_RATE_ID].pid_correction;
+ graph_get_output(graph, parameter_struct->yaw_r_pid, PID_CORRECTION);
}
+ // TODO: Make these blocks
//BOUNDS CHECKING
if(raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] < 0)
raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] = 0;
@@ -314,6 +330,7 @@
// here we are not actually duplicating the logging from the PID computation
// the PID computation logs PID_values struct where this logs the PID struct
// they contain different sets of data
+ /*
log_struct->local_y_PID = parameter_struct->pid_controllers[LOCAL_Y_ID];
log_struct->local_x_PID = parameter_struct->pid_controllers[LOCAL_X_ID];
log_struct->altitude_PID = parameter_struct->pid_controllers[ALT_ID];
@@ -325,7 +342,7 @@
log_struct->ang_vel_roll_PID = parameter_struct->pid_controllers[ROLL_RATE_ID];
log_struct->ang_vel_pitch_PID = parameter_struct->pid_controllers[PITCH_RATE_ID];
log_struct->ang_vel_yaw_PID = parameter_struct->pid_controllers[YAW_RATE_ID];
-
+ */
last_fm_switch = cur_fm_switch;
// Make location stale now
@@ -333,7 +350,7 @@
return 0;
}
-
+
void setPIDCoeff(PID_t* p, float pValue, float iValue, float dValue) {
p->Kp = pValue;
@@ -341,4 +358,3 @@
p->Kd = dValue;
}
-
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_bounds.c b/quad/sw/modular_quad_pid/src/graph_blocks/node_bounds.c
new file mode 100644
index 0000000000000000000000000000000000000000..aae4a67957a3d76c89d44480962e7b426b94f235
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_bounds.c
@@ -0,0 +1,33 @@
+#include "node_bounds.h"
+#include <stdlib.h>
+
+static void bounds_computation(void *state, const double* params, const double *inputs, double *outputs) {
+ double to_be_bounded = inputs[BOUNDS_IN];
+ if (to_be_bounded < params[BOUNDS_MIN]) {
+ to_be_bounded = params[BOUNDS_MIN];
+ }
+ if (to_be_bounded > params[BOUNDS_MAX]) {
+ to_be_bounded = params[BOUNDS_MAX];
+ }
+ outputs[BOUNDS_OUT] = to_be_bounded;
+}
+
+static void reset_bounds(void *state) {}
+
+static const char* const in_names[1] = {"Bounds in"};
+static const char* const out_names[1] = {"Bounded"};
+static const char* const param_names[2] = {"Min", "Max"};
+const struct graph_node_type node_bounds_type = {
+ .input_names = in_names,
+ .output_names = out_names,
+ .param_names = param_names,
+ .n_inputs = 1,
+ .n_outputs = 1,
+ .n_params = 2,
+ .execute = bounds_computation,
+ .reset = reset_bounds
+};
+
+int graph_add_node_bounds(struct computation_graph *graph, const char* name) {
+ return graph_add_node(graph, name, &node_bounds_type, NULL);
+}
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_bounds.h b/quad/sw/modular_quad_pid/src/graph_blocks/node_bounds.h
new file mode 100644
index 0000000000000000000000000000000000000000..15415488f46e61d425931d9b363c9fc6479d750a
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_bounds.h
@@ -0,0 +1,22 @@
+#ifndef __NODE_BOUNDS_H__
+#define __NODE_BOUNDS_H__
+#include "computation_graph.h"
+
+int graph_add_node_bounds(struct computation_graph *graph, const char* name);
+
+extern const struct graph_node_type node_bounds_type;
+
+enum graph_node_bounds_inputs {
+ BOUNDS_IN, // Input to be bounded
+};
+
+enum graph_node_bounds_outputs {
+ BOUNDS_OUT // Boundde output
+};
+
+enum graph_node_bounds_params {
+ BOUNDS_MIN,
+ BOUNDS_MAX
+};
+
+#endif // __NODE_BOUNDS_H__
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c
new file mode 120000
index 0000000000000000000000000000000000000000..0e47b26d1b909f68d792a4d8326da82893ffb76a
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.c
@@ -0,0 +1 @@
+computation_graph/src/node_constant.c
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h
new file mode 120000
index 0000000000000000000000000000000000000000..a231d9af0f982561c18f45fe9838f5e2862a7610
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_constant.h
@@ -0,0 +1 @@
+computation_graph/src/node_constant.h
\ No newline at end of file
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.c b/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.c
new file mode 100644
index 0000000000000000000000000000000000000000..745cf0dad11b68a7611885db034d76aeb0bccf66
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.c
@@ -0,0 +1,89 @@
+#include "node_pid.h"
+#include <stdlib.h>
+
+struct pid_node_state {
+ double prev_error; // Previous error
+ double acc_error; // Accumulated error
+};
+
+// The generic PID diagram. This function takes in pid inputs (CUR_POINT and SETOINT) and calculates the output "pid_correction"
+// part based on those parameters.
+//
+// + --- error ------------------ P + --- ----------------------------
+// setpoint ---> / sum \ --------->| Kp * error |--------------->/ sum \ -------->| output: "pid_correction" |
+// \ / | ------------------ \ / ----------------------------
+// --- | --- ||
+// - ^ | + ^ ^ + ||
+// | | ------------------------------- | | ------- \/------------
+// | |----->| Ki * accumulated error * dt |----+ | | |
+// | | ------------------------------- I | | SYSTEM |
+// | | | | |
+// | | | --------||------------
+// | | | ||
+// | | ---------------------------------- | ||
+// | |----->| Kd * (error - last error) / dt |----+ ||
+// | ---------------------------------- D ||
+// | ||
+// | -----------\/-----------
+// |____________________________________________________________| Sensor measurements: |
+// | "current point" |
+// ------------------------
+//
+static void pid_computation(void *state, const double* params, const double *inputs, double *outputs) {
+ struct pid_node_state* pid_state = (struct pid_state*)state;
+
+ double P = 0.0, I = 0.0, D = 0.0;
+
+ // calculate the current error
+ double error = inputs[PID_SETPOINT] - inputs[PID_CUR_POINT];
+
+ // Accumulate the error (if Ki is less than epsilon, rougly 0,
+ // then reset the accumulated error for safety)
+ if (fabs(pid->Ki) <= FLT_EPSILON) {
+ pid_state->acc_error = 0;
+ } else {
+ pid_state->acc_error += error;
+ }
+
+ double change_in_error = error - pid_state->prev_error;
+
+ // Compute each term's contribution
+ P = params[PID_KP] * error;
+ I = params[PID_KI] * pid->acc_error * inputs[PID_DT];
+ D = params[PID_KD] * (change_in_error / inputs[PID_DT]);
+
+ pid_state->prev_error = error; // Store the current error into the state
+
+ output[PID_CORRECTION] = P + I + D; // Store the computed correction
+}
+
+// This function sets the accumulated error and previous error to 0
+// to prevent previous errors from affecting output after a reset
+static void reset_pid(void *state) {
+ struct pid_node_state* pid_state = (struct pid_state*)state;
+ pid_state->acc_error = 0;
+ pid_state->prev_error = 0;
+}
+
+
+static const char* const in_names[3] = {"Cur point", "Setpoint", "dt"};
+static const char* const out_names[1] = {"Correction"};
+static const char* const param_names[3] = {"Kp", "Ki", "Kd"};
+const struct graph_node_type node_accum_type = {
+ .input_names = in_names,
+ .output_names = out_names,
+ .param_names = param_names,
+ .n_inputs = 3,
+ .n_outputs = 1,
+ .n_params = 3,
+ .execute = pid_computation,
+ .reset = reset_pid
+};
+
+int graph_add_node_pid(struct computation_graph *graph, const char* name) {
+ struct pid_node_state* node_state = malloc(sizeof(struct pid_node_state));
+ if (sizeof(struct accum_state) && !node_state) {
+ return -1; // malloc failed
+ }
+ return graph_add_node(graph, name, &node_accum_type, node_state);
+}
diff --git a/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.h b/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.h
new file mode 100644
index 0000000000000000000000000000000000000000..ce87339b7f39adc410263b48005539f8fac47da2
--- /dev/null
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.h
@@ -0,0 +1,25 @@
+#ifndef __NODE_PID_H__
+#define __NODE_PID_H__
+#include "computation_graph.h"
+
+int graph_add_node_pid(struct computation_graph *graph, const char* name);
+
+extern const struct graph_node_type node_pid_type;
+
+enum graph_node_pid_inputs {
+ PID_CUR_POINT, // Current value of the system
+ PID_SETPOINT, // Desired value of the system
+ PID_DT // sample period
+};
+
+enum graph_node_pid_outputs {
+ PID_CORRECTION // Correction factor computed by the PID
+};
+
+enum graph_node_pid_params {
+ PID_KP, // Proportional constant
+ PID_KI, // Integral constant
+ PID_KD // Derivative constant
+};
+
+#endif // __NODE_PID_H__
diff --git a/quad/sw/modular_quad_pid/src/main.c b/quad/sw/modular_quad_pid/src/main.c
index d703e348dbdeaf14dae299732c0eff36b892767c..8ef38990d98c1a5415c9be48a5b0c2d8a829b6ec 100644
--- a/quad/sw/modular_quad_pid/src/main.c
+++ b/quad/sw/modular_quad_pid/src/main.c
@@ -134,5 +134,3 @@ int main()
return 0;
}
-
-
diff --git a/quad/sw/modular_quad_pid/src/type_def.h b/quad/sw/modular_quad_pid/src/type_def.h
index c6f89b496581f5241b2bfa392d50f1fbda319fd5..acc5815903d4b63695013cab39c3bc84382700b9 100644
--- a/quad/sw/modular_quad_pid/src/type_def.h
+++ b/quad/sw/modular_quad_pid/src/type_def.h
@@ -1,4 +1,24 @@
-/*
+int roll_pid =
+int pitch_pid =
+int yaw_pid = g
+int roll_r_pid
+int pitch_r_pid
+int yaw_r_pid =
+int x_pos_pid =
+int y_pos_pid =
+int alt_pid = g
+// Create block
+int cur_pitch =
+int cur_roll =
+int cur_yaw = g
+int theta = gra
+int phi = graph
+int psi = graph
+// Create block
+int rc_pitch =
+int rc_roll = g
+int rc_yaw = gr
+int rc_throttle/*
* struct_def.h
*
* Created on: Mar 2, 2016
@@ -10,6 +30,7 @@
#include <stdint.h>
#include "commands.h"
+#include "computation_graph.h"
/**
* @brief
@@ -288,6 +309,31 @@ typedef struct setpoint_t {
*/
typedef struct parameter_t {
PID_t pid_controllers[MAX_CONTROLLER_ID];
+ struct computation_graph* graph;
+ // PID blocks
+ int roll_pid;
+ int pitch_pid;
+ int yaw_pid;
+ int roll_r_pid;
+ int pitch_r_pid;
+ int yaw_r_pid;
+ int x_pos_pid;
+ int y_pos_pid;
+ int alt_pid;
+ // Sensor blocks
+ int cur_pitch;
+ int cur_roll;
+ int cur_yaw;
+ int theta;
+ int phi;
+ int psi;
+ // RC blocks
+ int rc_pitch;
+ int rc_roll;
+ int rc_yaw;
+ int rc_throttle;
+ // Loop time
+ int dt;
} parameter_t;
/**