diff --git a/quad/computation_graph/src/computation_graph.c b/quad/computation_graph/src/computation_graph.c
index 5fcfd4c5c76ad7c221da927c410f45184c5c0578..7434e14a112117b8dbe22a3ecd300b04c6c90413 100644
--- a/quad/computation_graph/src/computation_graph.c
+++ b/quad/computation_graph/src/computation_graph.c
@@ -103,7 +103,7 @@ int graph_add_node(struct computation_graph *graph,
new_node->n_children = 0;
new_node->updated = 1;
new_node->output_values = malloc(type->n_outputs * sizeof(double));
- new_node->param_values = malloc(type->n_params * sizeof(double));
+ new_node->param_values = calloc(type->n_params, sizeof(double));
new_node->input_srcs = malloc(type->n_inputs * sizeof(struct input_type));
// Check that malloc succeeded in every case which memory was requested
if ((type->n_outputs && !new_node->output_values) ||
diff --git a/quad/sw/modular_quad_pid/src/PID.h b/quad/sw/modular_quad_pid/src/PID.h
index a7701ab2a8a8701343d4e65d5688759d70aba2b9..db0251195595434ac179da7eb0f97d3ac5f9d1c5 100644
--- a/quad/sw/modular_quad_pid/src/PID.h
+++ b/quad/sw/modular_quad_pid/src/PID.h
@@ -49,6 +49,7 @@
#define YPOS_KP 0.015f
#define YPOS_KI 0.005f
#define YPOS_KD 0.03f
+#define YPOS_ALPHA 0.8923855
//Pitch constants
@@ -74,6 +75,7 @@
#define XPOS_KP -0.015f
#define XPOS_KI -0.005f
#define XPOS_KD -0.03f
+#define XPOS_ALPHA 0.8923855
//Throttle constants
diff --git a/quad/sw/modular_quad_pid/src/control_algorithm.c b/quad/sw/modular_quad_pid/src/control_algorithm.c
index 0d3d6f4e4269005355da5403df80c4757689d1f6..158d086b169e070468e2608e560a1de8d28d77e3 100644
--- a/quad/sw/modular_quad_pid/src/control_algorithm.c
+++ b/quad/sw/modular_quad_pid/src/control_algorithm.c
@@ -174,9 +174,11 @@ int control_algorithm_init(parameter_t * ps)
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->x_pos_pid, PID_ALPHA, XPOS_ALPHA);
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->y_pos_pid, PID_ALPHA, YPOS_ALPHA);
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);
@@ -189,6 +191,10 @@ int control_algorithm_init(parameter_t * ps)
graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MIN, -PWM_DIFF_BOUNDS);
graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MAX, PWM_DIFF_BOUNDS);
+
+ // Initial value for sampling periods
+ graph_set_param_val(graph, ps->pos_time, CONST_SET, 0.04);
+ graph_set_param_val(graph, ps->angle_time, CONST_SET, 0.005);
// Set initial mode
connect_manual(ps);
@@ -243,7 +249,7 @@ int control_algorithm_init(parameter_t * ps)
user_defined_struct->flight_mode = AUTO_FLIGHT_MODE;
connect_autonomous(ps);
// Reset this when autonomous is engaged, so there is not a large difference at the start of autonomous
- last_vrpn_id = sensor_struct->currentQuadPosition.packetId;
+ last_vrpn_id = sensor_struct->currentQuadPosition.packetId - 1;
}
//PIDS///////////////////////////////////////////////////////////////////////
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
index acf77b5b4051c446067dfaea395a05cf89d1f916..7222094577d6c4ba1f41c93f65e5bdbe00c4dc75 100644
--- a/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.c
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.c
@@ -8,6 +8,7 @@ static double FLT_EPSILON = 0.0001;
struct pid_node_state {
double prev_error; // Previous error
double acc_error; // Accumulated error
+ double last_filtered; // Last output from the filtered derivative
};
// The generic PID diagram. This function takes in pid inputs (CUR_POINT and SETOINT) and calculates the output "pid_correction"
@@ -49,13 +50,20 @@ static void pid_computation(void *state, const double* params, const double *inp
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_state->acc_error * inputs[PID_DT];
- D = params[PID_KD] * (change_in_error / inputs[PID_DT]);
-
+ // Low-pass filter on derivative
+ double change_in_error = error - pid_state->prev_error;
+ double term1 = params[PID_ALPHA] * pid_state->last_filtered;
+ double derivative = change_in_error / inputs[PID_DT];
+ if (inputs[PID_DT] == 0) { // Divide by zero check
+ derivative = 0;
+ }
+ double term2 = params[PID_KD] * (1.0f - params[PID_ALPHA]) * derivative;
+ D = term1 + term2;
+ pid_state->last_filtered = D; // Store filtered value for next filter iteration
pid_state->prev_error = error; // Store the current error into the state
outputs[PID_CORRECTION] = P + I + D; // Store the computed correction
@@ -67,19 +75,20 @@ static void reset_pid(void *state) {
struct pid_node_state* pid_state = (struct pid_node_state*)state;
pid_state->acc_error = 0;
pid_state->prev_error = 0;
+ pid_state->last_filtered = 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"};
+static const char* const param_names[4] = {"Kp", "Ki", "Kd", "alpha"};
const struct graph_node_type node_pid_type = {
.input_names = in_names,
.output_names = out_names,
.param_names = param_names,
.n_inputs = 3,
.n_outputs = 1,
- .n_params = 3,
+ .n_params = 4,
.execute = pid_computation,
.reset = reset_pid
};
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
index 75ded4af02edbc2cacbc68035f5a0970d65e38dc..ee841adde3e19f7ef86930bdb94da4e8eee76488 100644
--- a/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.h
+++ b/quad/sw/modular_quad_pid/src/graph_blocks/node_pid.h
@@ -19,7 +19,8 @@ enum graph_node_pid_outputs {
enum graph_node_pid_params {
PID_KP, // Proportional constant
PID_KI, // Integral constant
- PID_KD // Derivative constant
+ PID_KD, // Derivative constant
+ PID_ALPHA // alpha = (1 - N*T_s); High values mean more filtering
};
#endif // __NODE_PID_H__