diff --git a/quad/src/quad_app/PID.h b/quad/src/quad_app/PID.h
index 4c5e4b81f0273a9a62aa175b6c0f350914fb67a2..5a83225b0b9813e4d311bf61c8f1eae89c7f24a0 100644
--- a/quad/src/quad_app/PID.h
+++ b/quad/src/quad_app/PID.h
@@ -21,7 +21,7 @@
 //#define YAW_ANGLE_KD 0.0f
 
 // when using units of radians
-#define YAW_ANGULAR_VELOCITY_KP 190.0f * 2292.0f//200.0f * 2292.0f
+#define YAW_ANGULAR_VELOCITY_KP 29700//200.0f * 2292.0f
 #define YAW_ANGULAR_VELOCITY_KI 0.0f
 #define YAW_ANGULAR_VELOCITY_KD 0.0f
 #define YAW_ANGLE_KP 2.6f
diff --git a/quad/src/quad_app/iic_utils.c b/quad/src/quad_app/iic_utils.c
index 90cdfb6872963b2c3ef5c7437a8abb9435e0c581..a0061e6b00432768feded1b15495c850fad5a914 100644
--- a/quad/src/quad_app/iic_utils.c
+++ b/quad/src/quad_app/iic_utils.c
@@ -30,7 +30,7 @@ int iic0_mpu9150_start(){
 	// Set clock reference to Z Gyro
 	iic0_mpu9150_write(0x6B, 0x03);
 	// Configure Digital Low/High Pass filter
-	iic0_mpu9150_write(0x1A,0x06); // Level 6 low pass on gyroscope
+	iic0_mpu9150_write(0x1A,0x05); // Level 5 low pass on gyroscope
 
 	// Configure Gyro to 2000dps, Accel. to +/-8G
 	iic0_mpu9150_write(0x1B, 0x18);
diff --git a/quad/src/quad_app/log_data.c b/quad/src/quad_app/log_data.c
index 9448fc10d5d47ec8e32c31e364983f5a94a8c873..3e67c14aa36862cae1ad885955e944a86056007b 100644
--- a/quad/src/quad_app/log_data.c
+++ b/quad/src/quad_app/log_data.c
@@ -112,6 +112,9 @@ void initialize_logging(log_t* log_struct, parameter_t* ps) {
 	addOutputToLog(log_struct, ps->vrpn_alt, CONST_VAL, m);
 	addOutputToLog(log_struct, ps->vrpn_pitch, CONST_VAL, rad);
 	addOutputToLog(log_struct, ps->vrpn_roll, CONST_VAL, rad);
+	addOutputToLog(log_struct, ps->x_set, CONST_VAL, m);
+	addOutputToLog(log_struct, ps->y_set, CONST_VAL, m);
+	addOutputToLog(log_struct, ps->alt_set, CONST_VAL, m);
 	addOutputToLog(log_struct, ps->mixer, MIXER_PWM0, pwm_val);
 	addOutputToLog(log_struct, ps->mixer, MIXER_PWM1, pwm_val);
 	addOutputToLog(log_struct, ps->mixer, MIXER_PWM2, pwm_val);
diff --git a/quad/src/quad_app/log_data.h b/quad/src/quad_app/log_data.h
index 34f1658d02f316918547bc0eedd760bcbf121264..becc2c2e188a819e1e9735cc625e93dd514f52c7 100644
--- a/quad/src/quad_app/log_data.h
+++ b/quad/src/quad_app/log_data.h
@@ -9,7 +9,7 @@
 #define LOG_DATA_H_
 #include "type_def.h"
 
-#define LOG_STARTING_SIZE 8192 //262144 // 2^18      32768  2^15
+#define LOG_STARTING_SIZE 60000 //262144 // 2^18      32768  2^15
 // Maximum number of block outputs you can record, and maximum number of block parameters to record
 #define MAX_LOG_NUM 50
 
diff --git a/quad/src/quad_app/quad_app.c b/quad/src/quad_app/quad_app.c
index 1e301732814d8cf7faba2984ca192c99387afb00..01eabe9017f733685ae8f9737398099e657a0bf0 100644
--- a/quad/src/quad_app/quad_app.c
+++ b/quad/src/quad_app/quad_app.c
@@ -75,15 +75,12 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
 		// update the GUI
 		update_GUI(&(structs.log_struct));
 
-		// Processing of loop timer at the end of the control loop
-		timer_end_loop(&(structs.log_struct));
-
 		if (!this_kill_condition) {
+			// Log the data collected in this loop
+			log_data(&(structs.log_struct), &(structs.parameter_struct));
+
 			if(structs.user_defined_struct.flight_mode == AUTO_FLIGHT_MODE)
 			{
-				// Log the data collected in this loop
-				log_data(&(structs.log_struct), &(structs.parameter_struct));
-
 				static int loop_counter = 0;
 				loop_counter++;
 
@@ -113,6 +110,9 @@ int quad_main(int (*setup_hardware)(hardware_t *hardware_struct))
 		}
 
 		last_kill_condition = this_kill_condition;
+
+		// Processing of loop timer at the end of the control loop
+		timer_end_loop(&(structs.log_struct));
 	}
 
 	kill_motors(&(structs.hardware_struct.pwm_outputs));