Added option to filter derivative term on controllers.

PID nodes have a second parameter, called alpha that is (1 - N*T_s), where N is equivalent to Simulink N.
If alpha == 0, no filtering (default).
Also fixed issue where sampling time for autonomous was 0 for the first iteration.

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) ||

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

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///////////////////////////////////////////////////////////////////////

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
 };

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__