Made sampling time in control algorithm dynamic.

quad/sw/modular_quad_pid/gen_diagram/Makefile

@@ -2,7 +2,7 @@
 QUAD_BLOCKS = ../src/graph_blocks
 
 gen_diagram: generate.c ../src/control_algorithm.c ../src/computation_graph.c
-	gcc -o gen_diagram -I. -I../src/ generate.c ../src/computation_graph.c ../src/control_algorithm.c ../src/graph_blocks/*.c -Dread_flap=freadflap
+	gcc -o gen_diagram -I. -I../src/ generate.c ../src/computation_graph.c ../src/control_algorithm.c ../src/graph_blocks/*.c -Dread_flap=freadflap -Dget_last_loop_time=fgettime
 
 .PHONY: clean
 clean:

quad/sw/modular_quad_pid/gen_diagram/create_png.sh

+#/bin/bash
+
+dot -Tpng network.dot -o network.png
+

quad/sw/modular_quad_pid/gen_diagram/generate.c

@@ -13,6 +13,7 @@ parameter_t ps;
 
 //int control_algorithm_init(parameter_t * ps);
 int freadflap(int i) {return i;}
+float fgettime() {return 0.0;}
 
 int main() {
     control_algorithm_init(&ps);

quad/sw/modular_quad_pid/gen_diagram/network.dot

@@ -72,13 +72,13 @@ label="<f0>dPhi  |<f1> [Constant=0.000]"]
 "dPsi"[shape=record
 label="<f0>dPsi  |<f1> [Constant=0.000]"]
 "P PWM Clamp"[shape=record
-label="<f0>P PWM Clamp  |<f1> --\>Bounds in |<f2> [Min=-20000.000] |<f3> [Max=20000.000]"]
+label="<f0>P PWM Clamp  |<f1> --\>Bounds in |<f2> [Min=-30000.000] |<f3> [Max=30000.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]"]
+label="<f0>R PWM Clamp  |<f1> --\>Bounds in |<f2> [Min=-30000.000] |<f3> [Max=30000.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]"]
+label="<f0>Y PWM Clamp  |<f1> --\>Bounds in |<f2> [Min=-30000.000] |<f3> [Max=30000.000]"]
 "Yaw Rate PID" -> "Y PWM Clamp":f1 [label="Correction"]
 "VRPN X"[shape=record
 label="<f0>VRPN X  |<f1> [Constant=0.000]"]

quad/sw/modular_quad_pid/src/control_algorithm.c

@@ -15,9 +15,11 @@
 #include "graph_blocks/node_add.h"
 #include "PID.h"
 #include "util.h"
+#include "timer.h"
 
 #define ROLL_PITCH_MAX_ANGLE 1.5708 // 90 degrees
 #define PWM_DIFF_BOUNDS 30000
+#define VRPN_REFRESH_TIME 0.01f // 10ms
 
 void connect_autonomous(parameter_t* ps) {
 	struct computation_graph* graph = ps->graph;
@@ -25,10 +27,6 @@ void connect_autonomous(parameter_t* ps) {
 	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);
-	// TODO: Change this to use VRPN time differences
-	// NOTE: This is being set here because if we set it in the initialization, it is
-	//       never marked as "updated", and therefore, doesn't get computed. Yeah, I know, that's bad...
-    graph_set_param_val(graph, ps->pos_time, CONST_SET, 0.1);
 }
 
 void connect_manual(parameter_t* ps) {
@@ -37,8 +35,6 @@ void connect_manual(parameter_t* ps) {
 	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);
-    // TODO: Change this to use LOOP_TIME
-    graph_set_param_val(graph, ps->angle_time, CONST_SET, 0.005);
 }
 
 int control_algorithm_init(parameter_t * ps)
@@ -186,12 +182,12 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_param_val(graph, ps->alt_pid, PID_KD, ALT_ZPOS_KD);
 
     // 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);
+    graph_set_param_val(graph, ps->clamp_d_pwmP, BOUNDS_MIN, -PWM_DIFF_BOUNDS);
+    graph_set_param_val(graph, ps->clamp_d_pwmP, BOUNDS_MAX, PWM_DIFF_BOUNDS);
+    graph_set_param_val(graph, ps->clamp_d_pwmR, BOUNDS_MIN, -PWM_DIFF_BOUNDS);
+    graph_set_param_val(graph, ps->clamp_d_pwmR, BOUNDS_MAX, PWM_DIFF_BOUNDS);
+    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);
 
     // Set initial mode
     connect_manual(ps);
@@ -228,6 +224,8 @@ int control_algorithm_init(parameter_t * ps)
 			user_defined_struct->engaging_auto = 2;
 
 
+	// The last packet ID from VRPN.
+	static int last_vrpn_id = 0;
 	// if the flap switch was toggled to AUTO_FLIGHT_MODE and we've received a new packet
 	// then record the current position as the desired position
 	// also reset the previous error and accumulated error from the position PIDs
@@ -244,6 +242,8 @@ int control_algorithm_init(parameter_t * ps)
 		// 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(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;
 	}
 
 	//PIDS///////////////////////////////////////////////////////////////////////
@@ -262,8 +262,16 @@ int control_algorithm_init(parameter_t * ps)
 	    graph_set_param_val(graph, ps->vrpn_pitch, CONST_SET, sensor_struct->currentQuadPosition.pitch);
 	    graph_set_param_val(graph, ps->vrpn_roll, CONST_SET, sensor_struct->currentQuadPosition.roll);
 	    graph_set_param_val(graph, ps->cur_yaw, CONST_SET, sensor_struct->currentQuadPosition.yaw);
+
+	    // Use the difference in IDs to compute the sampling time for the position PIDs
+	    int current_vrpn_id = sensor_struct->currentQuadPosition.packetId;
+	    graph_set_param_val(graph, ps->pos_time, CONST_SET, VRPN_REFRESH_TIME * (current_vrpn_id - last_vrpn_id));
+	    last_vrpn_id = current_vrpn_id;
 	}
 
+	// Loop time
+    graph_set_param_val(graph, ps->angle_time, CONST_SET, get_last_loop_time());
+
 	// Sensor values
     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);

quad/sw/modular_quad_pid/src/log_data.c

@@ -62,6 +62,9 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
 	addOutputToLog(log_struct, ps->pitch_r_pid, PID_CORRECTION);
 	addOutputToLog(log_struct, ps->roll_r_pid, PID_CORRECTION);
 	addOutputToLog(log_struct, ps->yaw_r_pid, PID_CORRECTION);
+	addOutputToLog(log_struct, ps->cur_pitch, CONST_VAL);
+	addOutputToLog(log_struct, ps->cur_roll, CONST_VAL);
+	addOutputToLog(log_struct, ps->cur_yaw, CONST_VAL);
 	addOutputToLog(log_struct, ps->vrpn_x, CONST_VAL);
 	addOutputToLog(log_struct, ps->vrpn_y, CONST_VAL);
 	addOutputToLog(log_struct, ps->vrpn_alt, CONST_VAL);

quad/sw/modular_quad_pid/src/sensor_processing.c

@@ -80,10 +80,10 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
 			+ (raw_sensor_struct->gam.gyro_zVel_r*cos_phi*sec_theta);
 
 	// Complementary Filter Calculations
-	sensor_struct->pitch_angle_filtered = 0.98 * (sensor_struct->pitch_angle_filtered + sensor_struct->theta_dot * LOOP_TIME)
+	sensor_struct->pitch_angle_filtered = 0.98 * (sensor_struct->pitch_angle_filtered + sensor_struct->theta_dot * get_last_loop_time())
 			+ 0.02 * raw_sensor_struct->gam.accel_pitch;
 
-	sensor_struct->roll_angle_filtered = 0.98 * (sensor_struct->roll_angle_filtered + sensor_struct->phi_dot* LOOP_TIME)
+	sensor_struct->roll_angle_filtered = 0.98 * (sensor_struct->roll_angle_filtered + sensor_struct->phi_dot* get_last_loop_time())
 			+ 0.02 * raw_sensor_struct->gam.accel_roll;
 
 //	static int loop_counter = 0;

quad/sw/modular_quad_pid/src/timer.c

@@ -8,11 +8,14 @@
 
 
 #include "timer.h"
+#include "xtime_l.h"
+#include <xtmrctr.h>
 
-XTime before = 0, after = 0;
-XTmrCtr axi_timer;
-float LOOP_TIME;
-float time_stamp = 0;
+static XTime before = 0;
+static XTime after = 0;
+static XTmrCtr axi_timer;
+static float LOOP_TIME;
+static float time_stamp = 0;
 
 int timer_init()
 {
@@ -65,6 +68,10 @@ int timer_end_loop(log_t *log_struct)
 	return 0;
 }
 
-u32 timer_get_count() {
+float get_last_loop_time() {
+	return LOOP_TIME;
+}
+
+uint32_t timer_get_count() {
 	return XTmrCtr_GetValue(&axi_timer, 0);
 }

quad/sw/modular_quad_pid/src/timer.h

@@ -9,14 +9,6 @@
 #define TIMER_H_
 
 #include "log_data.h"
-#include "xtime_l.h"
-#include <xtmrctr.h>
-
-extern XTime before;
-extern XTime after;
-extern XTmrCtr axi_timer;
-extern float LOOP_TIME;
-extern float time_stamp;
 
 // desired loop time is not guaranteed (its possible that the loop may take longer than desired)
 #define DESIRED_USEC_PER_LOOP 5000 // gives 5ms loops
@@ -53,6 +45,9 @@ int timer_start_loop();
  */
 int timer_end_loop(log_t *log_struct);
 
-u32 timer_get_count();
+// Returns the number of seconds the last loop took
+float get_last_loop_time();
+
+uint32_t timer_get_count();
 
 #endif /* TIMER_H_ */