This is stable code!!! For our demo.

quad/src/gen_diagram/network.dot

@@ -3,12 +3,12 @@ rankdir="LR"
 "Roll PID"[shape=record
 label="<f0>Roll PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=35.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.880]"]
 "Roll" -> "Roll PID":f1 [label="Constant"]
-"RC Roll" -> "Roll PID":f2 [label="Constant"]
+"Yaw Correction" -> "Roll PID":f2 [label="Rotated Y"]
 "Ts_IMU" -> "Roll PID":f3 [label="Constant"]
 "Pitch PID"[shape=record
 label="<f0>Pitch PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=35.000] |<f5> [Ki=0.000] |<f6> [Kd=1.000] |<f7> [alpha=0.880]"]
 "Pitch trim add" -> "Pitch PID":f1 [label="Sum"]
-"RC Pitch" -> "Pitch PID":f2 [label="Constant"]
+"Yaw Correction" -> "Pitch PID":f2 [label="Rotated X"]
 "Ts_IMU" -> "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] |<f7> [alpha=0.000]"]
@@ -28,7 +28,7 @@ label="<f0>Pitch Rate PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |
 "Yaw Rate PID"[shape=record
 label="<f0>Yaw Rate PID  |<f1> --\>Cur point |<f2> --\>Setpoint |<f3> --\>dt |<f4> [Kp=0.297] |<f5> [Ki=0.000] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
 "Gyro Z" -> "Yaw Rate PID":f1 [label="Constant"]
-"RC Yaw" -> "Yaw Rate PID":f2 [label="Constant"]
+"Yaw Rate Clamp" -> "Yaw Rate PID":f2 [label="Bounded"]
 "Ts_IMU" -> "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.550] |<f5> [Ki=0.007] |<f6> [Kd=0.000] |<f7> [alpha=0.000]"]
@@ -77,8 +77,6 @@ label="<f0>Flow Vel X Filt  |<f1> [Constant=0.000]"]
 label="<f0>Flow Vel Y Filt  |<f1> [Constant=0.000]"]
 "Flow Quality"[shape=record
 label="<f0>Flow Quality  |<f1> [Constant=0.000]"]
-"Flow Distance"[shape=record
-label="<f0>Flow Distance  |<f1> [Constant=0.000]"]
 "Pitch trim"[shape=record
 label="<f0>Pitch trim  |<f1> [Constant=0.045]"]
 "Pitch trim add"[shape=record
@@ -101,7 +99,7 @@ label="<f0>R PWM Clamp  |<f1> --\>Bounds in |<f2> [Min=-20000.000] |<f3> [Max=20
 label="<f0>Y PWM Clamp  |<f1> --\>Bounds in |<f2> [Min=-20000.000] |<f3> [Max=20000.000]"]
 "Yaw Rate PID" -> "Y PWM Clamp":f1 [label="Correction"]
 "Yaw Rate Clamp"[shape=record
-label="<f0>Yaw Rate Clamp  |<f1> --\>Bounds in |<f2> [Min=-1.000] |<f3> [Max=1.000]"]
+label="<f0>Yaw Rate Clamp  |<f1> --\>Bounds in |<f2> [Min=-1.500] |<f3> [Max=1.500]"]
 "Yaw PID" -> "Yaw Rate Clamp":f1 [label="Correction"]
 "VRPN X"[shape=record
 label="<f0>VRPN X  |<f1> [Constant=0.000]"]
@@ -199,7 +197,7 @@ label="<f0>PSI Sum  |<f1> --\>Summand 1 |<f2> --\>Summand 2"]
 label="<f0>Mag Yaw  |<f1> [Constant=0.000]"]
 "Signal Mixer"[shape=record
 label="<f0>Signal Mixer  |<f1> --\>Throttle |<f2> --\>Pitch |<f3> --\>Roll |<f4> --\>Yaw"]
-"RC Throttle" -> "Signal Mixer":f1 [label="Constant"]
+"T trim add" -> "Signal Mixer":f1 [label="Sum"]
 "P PWM Clamp" -> "Signal Mixer":f2 [label="Bounded"]
 "R PWM Clamp" -> "Signal Mixer":f3 [label="Bounded"]
 "Y PWM Clamp" -> "Signal Mixer":f4 [label="Bounded"]

quad/src/graph_blocks/node_yaw_rot.c

@@ -19,7 +19,6 @@ static void rotate_yaw(void *state, const double* params, const double *inputs,
 
 	outputs[ROT_OUT_Y] =
 			inputs[ROT_CUR_X] * sin(inputs[ROT_YAW]) + inputs[ROT_CUR_Y] * cos(inputs[ROT_YAW]);
-
 }
 static void reset(void *state) {}
 

quad/src/quad_app/control_algorithm.c

@@ -12,6 +12,7 @@
 #include "PID.h"
 #include "util.h"
 #include "timer.h"
+#include "user_input.h"
 
 //#define USE_LIDAR
 //#define USE_OF
@@ -298,7 +299,6 @@ int control_algorithm_init(parameter_t * ps)
     graph_set_source(graph, ps->of_trimmed_y, ADD_SUMMAND1, ps->of_integ_y, INTEGRATED);
     graph_set_source(graph, ps->of_trimmed_y, ADD_SUMMAND2, ps->of_trim_y, 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);
@@ -377,7 +377,7 @@ int control_algorithm_init(parameter_t * ps)
     // Set initial mode
     connect_manual(ps);
 
-	return 0;
+    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* ps, user_defined_t* user_defined_struct, actuator_command_t* actuator_struct, modular_structs_t* structs)
@@ -420,29 +420,77 @@ int control_algorithm_init(parameter_t * ps)
 	// 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))
 	{
+
+		// 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;
+		connect_autonomous(ps);
+
+		// Reset optical flow trims. Do this after connecting autonomous
+		graph_set_param_val(graph, ps->of_trim_x, CONST_SET, 0);
+		graph_set_param_val(graph, ps->of_trim_y, CONST_SET, 0);
+		graph_set_param_val(graph, ps->psi_offset, CONST_SET, -graph_get_output(graph, ps->psi, CONST_VAL));
+
+
 #ifdef USE_OF
 		graph_set_param_val(graph, ps->x_set, CONST_SET, graph_get_output(graph, ps->of_trimmed_x, ADD_SUM));
 		graph_set_param_val(graph, ps->y_set, CONST_SET, graph_get_output(graph, ps->of_trimmed_y, ADD_SUM));
 		//graph_set_param_val(graph, ps->alt_set, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
-		graph_set_param_val(graph, ps->yaw_set, CONST_SET, sensor_struct->currentQuadPosition.yaw);
 #else
 		graph_set_param_val(graph, ps->x_set, CONST_SET, sensor_struct->currentQuadPosition.x_pos);
 		graph_set_param_val(graph, ps->y_set, CONST_SET, sensor_struct->currentQuadPosition.y_pos);
 		graph_set_param_val(graph, ps->alt_set, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
+#endif
+
+#ifdef USE_GYRO_YAW
+		// Calculate current yaw angle
+		int nodes[1] = {ps->psi_sum};
+		graph_compute_nodes(graph, nodes, 1);
+		graph_set_param_val(graph, ps->yaw_set, CONST_SET, graph_get_output(graph, ps->psi_sum, CONST_VAL));
+#else
 		graph_set_param_val(graph, ps->yaw_set, CONST_SET, sensor_struct->currentQuadPosition.yaw);
 #endif
 
-		// 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;
-		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 - 1;
 	}
 
-	//PIDS///////////////////////////////////////////////////////////////////////
+	if (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE) {
+		//---------- Optical flow trim values from RC controller ------------//
+		float cur_x_trim = graph_get_param_val(graph, ps->of_trim_x, CONST_SET);
+		float cur_y_trim = graph_get_param_val(graph, ps->of_trim_y, CONST_SET);
+		float cur_yaw_trim = graph_get_param_val(graph, ps->psi_offset, CONST_SET);
+		// Max step size will move at 0.5 m/s
+		float max_step_size = 0.5 / 200.0; // 200 Hz update rate
+		// Only add if above 10% of range
+		if (fabs(user_input_struct->pitch_angle_manual_setpoint) > PITCH_RAD_TARGET * 0.1) {
+			// Scale to +/- 1
+			float normalized_stick = (user_input_struct->pitch_angle_manual_setpoint / PITCH_RAD_TARGET);
+			// Remove dead-zone
+			normalized_stick = normalized_stick < 0 ? normalized_stick + 0.1 : normalized_stick - 0.1;
+			normalized_stick /= 0.9;
+			float amt_to_shift = normalized_stick * max_step_size;
+			graph_set_param_val(graph, ps->of_trim_x, CONST_SET, cur_x_trim + amt_to_shift);
+		}
+		if (fabs(user_input_struct->roll_angle_manual_setpoint) > ROLL_RAD_TARGET * 0.1) {
+			float normalized_stick = (user_input_struct->roll_angle_manual_setpoint / ROLL_RAD_TARGET);
+			normalized_stick = normalized_stick < 0 ? normalized_stick + 0.1 : normalized_stick - 0.1;
+			normalized_stick /= 0.9;
+			float amt_to_shift = normalized_stick * max_step_size;
+			graph_set_param_val(graph, ps->of_trim_y, CONST_SET, cur_y_trim - amt_to_shift);
+		}
+		// Rotate max of 0.5 rad/s
+		float max_step_rot = 0.5 / 200.0;
+		if (fabs(user_input_struct->yaw_manual_setpoint) > YAW_RAD_TARGET * 0.1) {
+			float normalized_stick = (user_input_struct->yaw_manual_setpoint / YAW_RAD_TARGET);
+			normalized_stick = normalized_stick < 0 ? normalized_stick + 0.1 : normalized_stick - 0.1;
+			normalized_stick /= 0.9;
+			float amt_to_rot = normalized_stick * max_step_rot;
+			graph_set_param_val(graph, ps->psi_offset, CONST_SET, cur_yaw_trim - amt_to_rot);
+		}
+	}
 
 	/* 					Position loop
 	 * Reads current position, and outputs

quad/src/quad_app/log_data.c

@@ -15,6 +15,7 @@
 #include "communication.h"
 #include "computation_graph.h"
 #include "graph_blocks.h"
+#include "timer.h"
 
 // Current index of the log array
 static int arrayIndex = 0;
@@ -254,6 +255,9 @@ void printLogging(struct CommDriver *comm, log_t* log_struct, parameter_t* ps, r
 	for(i = 0; i < arrayIndex; i++){
 		format_log(i, log_struct, &buf);
 		send_data(comm->uart, LOG_ID, 0, (u8*)buf.str, buf.size);
+		// This is a stupid hack because if the axi timer is not reset in awhile, it always returns 0, and the timer_end_loop() call hangs forever after a long log
+		// Not 100% certain this works
+		timer_axi_reset();
 	}
 
 	// Empty message of type LOG_END to indicate end of log

quad/src/quad_app/timer.c

@@ -12,6 +12,10 @@ void timer_init_globals(struct TimerDriver *given_global_timer, struct TimerDriv
   axi_timer = given_axi_timer;
 }
 
+void timer_axi_reset() {
+	axi_timer->restart(axi_timer);
+}
+
 int timer_start_loop()
 {
   //timing code

quad/src/quad_app/timer.h

@@ -38,5 +38,8 @@ float get_last_loop_time();
 
 u64 timer_get_count();
 
+// Resets the axi timer
+void timer_axi_reset();
+
 void timer_init_globals(struct TimerDriver *global_timer, struct TimerDriver *axi_timer);
 #endif /* TIMER_H_ */

quad/src/quad_app/type_def.h

@@ -65,6 +65,7 @@ typedef struct {
 	double alt_pos;
 
 	double yaw;
+	double yawOffset;
 	double roll;
 	double pitch;
 } quadPosition_t;
@@ -409,6 +410,8 @@ typedef struct parameter_t {
 	int throttle_trim_add;
 	int pitch_trim;
 	int pitch_trim_add;
+	int yaw_trim;
+	int yaw_trim_add;
 	// Velocity nodes
 	int x_vel_pid;
 	int y_vel_pid;