Switched to Euler rates for angle rate control. Switched to using VRPN for pitch and roll.

quad/sw/modular_quad_pid/gen_diagram/create_png.sh

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

quad/sw/modular_quad_pid/gen_diagram/network.dot

@@ -2,49 +2,49 @@ digraph G {
 rankdir="LR"
 "Roll PID"[shape=record
 label="<f0>Roll PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200]"]
-"Roll" -> "Roll PID":f1 [label="Constant"]
+"VRPN Roll" -> "Roll PID":f1 [label="Constant"]
 "RC Roll" -> "Roll PID":f2 [label="Constant"]
-"Ts_angle" -> "Roll PID":f3 [label="Constant"]
+"Ts_VRPN" -> "Roll PID":f3 [label="Constant"]
 "Pitch PID"[shape=record
 label="<f0>Pitch PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=15.000] |<f5> [Ki=0.000] |<f6> [Kd=0.200]"]
-"Pitch" -> "Pitch PID":f1 [label="Constant"]
+"VRPN Pitch" -> "Pitch PID":f1 [label="Constant"]
 "RC Pitch" -> "Pitch PID":f2 [label="Constant"]
-"Ts_angle" -> "Pitch PID":f3 [label="Constant"]
+"Ts_VRPN" -> "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"]
-"Ts_angle" -> "Yaw PID":f3 [label="Constant"]
+"Ts_IMU" -> "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"]
 "Roll PID" -> "Roll Rate PID":f2 [label="Correction"]
-"Ts_angle" -> "Roll Rate PID":f3 [label="Constant"]
+"Ts_IMU" -> "Roll Rate PID":f3 [label="Constant"]
 "Pitch Rate PID"[shape=record
 label="<f0>Pitch Rate PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=4600.000] |<f5> [Ki=0.000] |<f6> [Kd=550.000]"]
 "dTheta" -> "Pitch Rate PID":f1 [label="Constant"]
 "Pitch PID" -> "Pitch Rate PID":f2 [label="Correction"]
-"Ts_angle" -> "Pitch Rate PID":f3 [label="Constant"]
+"Ts_IMU" -> "Pitch Rate PID":f3 [label="Constant"]
 "Yaw Rate PID"[shape=record
 label="<f0>Yaw Rate PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=435480.000] |<f5> [Ki=0.000] |<f6> [Kd=0.000]"]
 "dPsi" -> "Yaw Rate PID":f1 [label="Constant"]
 "RC Yaw" -> "Yaw Rate PID":f2 [label="Constant"]
-"Ts_angle" -> "Yaw Rate PID":f3 [label="Constant"]
+"Ts_VRPN" -> "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.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"]
-"Ts_position" -> "X pos PID":f3 [label="Constant"]
+"Ts_VRPN" -> "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.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"]
-"Ts_position" -> "Y pos PID":f3 [label="Constant"]
+"Ts_VRPN" -> "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"]
-"Ts_position" -> "Altitude PID":f3 [label="Constant"]
+"Ts_VRPN" -> "Altitude PID":f3 [label="Constant"]
 "X Setpoint"[shape=record
 label="<f0>X Setpoint  |<f1> [Constant=0.000]"]
 "Y Setpoint"[shape=record
@@ -104,8 +104,8 @@ label="<f0>Signal Mixer  |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4>
 "P PWM Clamp" -> "Signal Mixer":f2 [label="Bounded"]
 "R PWM Clamp" -> "Signal Mixer":f3 [label="Bounded"]
 "Y PWM Clamp" -> "Signal Mixer":f4 [label="Bounded"]
-"Ts_angle"[shape=record
-label="<f0>Ts_angle  |<f1> [Constant=0.005]"]
-"Ts_position"[shape=record
-label="<f0>Ts_position  |<f1> [Constant=0.100]"]
+"Ts_IMU"[shape=record
+label="<f0>Ts_IMU  |<f1> [Constant=0.000]"]
+"Ts_VRPN"[shape=record
+label="<f0>Ts_VRPN  |<f1> [Constant=0.000]"]
 }
\ No newline at end of file

quad/sw/modular_quad_pid/src/control_algorithm.c

@@ -90,29 +90,29 @@ int control_algorithm_init(parameter_t * ps)
 
     ps->mixer = graph_add_node_mixer(graph, "Signal Mixer");
 
-    ps->angle_time = graph_add_node_const(graph, "Ts_angle");
-    ps->pos_time = graph_add_node_const(graph, "Ts_position");
+    ps->angle_time = graph_add_node_const(graph, "Ts_IMU");
+    ps->pos_time = graph_add_node_const(graph, "Ts_VRPN");
 
     // 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_dot, CONST_VAL);
     graph_set_source(graph, ps->pitch_r_pid, PID_DT, ps->angle_time, 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->angle_time, 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_CUR_POINT, ps->vrpn_pitch, CONST_VAL);
+    graph_set_source(graph, ps->pitch_pid, PID_DT, ps->pos_time, 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_dot, CONST_VAL);
     graph_set_source(graph, ps->roll_r_pid, PID_DT, ps->angle_time, 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->angle_time, 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_CUR_POINT, ps->vrpn_roll, CONST_VAL);
+    graph_set_source(graph, ps->roll_pid, PID_DT, ps->pos_time, 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_dot, CONST_VAL);
-    graph_set_source(graph, ps->yaw_r_pid, PID_DT, ps->angle_time, CONST_VAL);
+    graph_set_source(graph, ps->yaw_r_pid, PID_DT, ps->pos_time, CONST_VAL);
 
 
     // X autonomous
@@ -275,9 +275,13 @@ int control_algorithm_init(parameter_t * ps)
 	// 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);
-    graph_set_param_val(graph, ps->theta_dot, CONST_SET, sensor_struct->theta_dot);
-    graph_set_param_val(graph, ps->phi_dot, CONST_SET, sensor_struct->phi_dot);
-    graph_set_param_val(graph, ps->psi_dot, CONST_SET, sensor_struct->psi_dot);
+    //graph_set_param_val(graph, ps->theta_dot, CONST_SET, sensor_struct->theta_dot);
+    //graph_set_param_val(graph, ps->phi_dot, CONST_SET, sensor_struct->phi_dot);
+    //graph_set_param_val(graph, ps->psi_dot, CONST_SET, sensor_struct->psi_dot);
+    graph_set_param_val(graph, ps->theta_dot, CONST_SET, sensor_struct->gyr_y);
+    graph_set_param_val(graph, ps->phi_dot, CONST_SET, sensor_struct->gyr_x);
+    graph_set_param_val(graph, ps->psi_dot, CONST_SET, sensor_struct->gyr_z);
+
     // RC values
     graph_set_param_val(graph, ps->rc_pitch, CONST_SET, user_input_struct->pitch_angle_manual_setpoint);
     graph_set_param_val(graph, ps->rc_roll, CONST_SET, user_input_struct->roll_angle_manual_setpoint);

quad/sw/modular_quad_pid/src/sensor_processing.c

@@ -79,6 +79,11 @@ int sensor_processing(log_t* log_struct, user_input_t *user_input_struct, raw_se
 	sensor_struct->psi_dot = (raw_sensor_struct->gam.gyro_yVel_q*sin_phi*sec_theta)
 			+ (raw_sensor_struct->gam.gyro_zVel_r*cos_phi*sec_theta);
 
+	// Copy in raw gyroscope values
+	sensor_struct->gyr_x = raw_sensor_struct->gam.gyro_xVel_p;
+	sensor_struct->gyr_y = raw_sensor_struct->gam.gyro_yVel_q;
+	sensor_struct->gyr_z = raw_sensor_struct->gam.gyro_zVel_r;
+
 	// Complementary Filter Calculations
 	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;

quad/sw/modular_quad_pid/src/type_def.h

@@ -254,13 +254,13 @@ typedef struct sensor {
 	int acc_z_t;	// time of accelerometer z data
 
 
-	int gyr_x;		// gyroscope x data
+	float gyr_x;		// gyroscope x data
 	int gyr_x_t;	// time of gyroscope x data
 
-	int gyr_y;		// gyroscope y data
+	float gyr_y;		// gyroscope y data
 	int gyr_y_t;	// time of gyroscope y data
 
-	int gyr_z;		// gyroscope z data
+	float gyr_z;		// gyroscope z data
 	int gyr_z_t;	// time of gyroscope z data
 
 	int ldr_z;		//lidar z data (altitude)