diff --git a/quad/sw/modular_quad_pid/src/log_data.c b/quad/sw/modular_quad_pid/src/log_data.c
index 1dd5dc9e888b5ecce863f1870d6d31a94c3cbc82..1c7cfd6d7ce46610601fe78740f02364d4b93589 100644
--- a/quad/sw/modular_quad_pid/src/log_data.c
+++ b/quad/sw/modular_quad_pid/src/log_data.c
@@ -124,6 +124,10 @@ int log_data(log_t* log_struct, parameter_t* ps)
  */
 
 void printLogging(log_t* log_struct, parameter_t* ps){
+	if (arrayIndex == 0) {
+		// Don't send any logs if nothing was logged
+		return;
+	}
 	int i;//, j;
 	char buf[2048] = {};
 	buf[0] = 0; // Mark buffer as size 0
@@ -168,6 +172,10 @@ void printLogging(log_t* log_struct, parameter_t* ps){
 	}
 }
 
+void resetLogging() {
+	arrayIndex = 0;
+}
+
 int format_log(int idx, log_t* log_struct, char* buf) {
 	int i;
 	int end = 0;
diff --git a/quad/sw/modular_quad_pid/src/log_data.h b/quad/sw/modular_quad_pid/src/log_data.h
index bb3622c931f7b56420175ea872420a39c7e904d7..0db1751bada3ad5cc9c3c1f9cb82f6dbc6ac721d 100644
--- a/quad/sw/modular_quad_pid/src/log_data.h
+++ b/quad/sw/modular_quad_pid/src/log_data.h
@@ -49,6 +49,11 @@ void addParamToLog(log_t* log_struct, int controller_id, int param_id);
   */
  void printLogging(log_t* log_struct, parameter_t* ps);
 
+ /**
+  * Resets and clears logged data
+  */
+ void resetLogging();
+
  /**
   * Fills the given buffer as ASCII for the recorded index, and returns the length of the string created
   */
diff --git a/quad/sw/modular_quad_pid/src/main.c b/quad/sw/modular_quad_pid/src/main.c
index 64d82b3a997b1619226249b20c626453398659dd..52f8c0a8379a297cd18fca00c4c4b4f0c1111bb1 100644
--- a/quad/sw/modular_quad_pid/src/main.c
+++ b/quad/sw/modular_quad_pid/src/main.c
@@ -41,9 +41,13 @@ int main()
 	protection_loops(&structs);
 #endif
 
+	int last_kill_condition = kill_condition(&(structs.user_input_struct));
+
 	// Main control loop
-	do
+	while (1)
 	{
+		int this_kill_condition = kill_condition(&(structs.user_input_struct));
+
 		// Processing of loop timer at the beginning of the control loop
 		timer_start_loop();
 
@@ -76,59 +80,63 @@ int main()
 		// Process the sensor data and put it into sensor_struct
 		sensor_processing(&(structs.log_struct), &(structs.user_input_struct), &(structs.raw_sensor_struct), &(structs.sensor_struct));
 
-		// Run the control algorithm
-		control_algorithm(&(structs.log_struct), &(structs.user_input_struct), &(structs.sensor_struct), &(structs.setpoint_struct),
-				&(structs.parameter_struct), &(structs.user_defined_struct), &(structs.actuator_command_struct), &structs);
-
-		// Process the commands going to the actuators
-		//actuator_command_processing(&(structs.log_struct), &(structs.user_input_struct), &(structs.raw_actuator_struct), &(structs.actuator_command_struct));
-
-		// send the actuator commands
-		send_actuator_commands(&(structs.log_struct), &(structs.actuator_command_struct));
+		if (!this_kill_condition) {
+			// Run the control algorithm
+			control_algorithm(&(structs.log_struct), &(structs.user_input_struct), &(structs.sensor_struct), &(structs.setpoint_struct),
+					&(structs.parameter_struct), &(structs.user_defined_struct), &(structs.actuator_command_struct), &structs);
 
+			// send the actuator commands
+			send_actuator_commands(&(structs.log_struct), &(structs.actuator_command_struct));
+		}
+		else {
+			pwm_kill();
+		}
 		// update the GUI
 		update_GUI(&(structs.log_struct));
 #endif
 		// Processing of loop timer at the end of the control loop
 		timer_end_loop(&(structs.log_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));
+		if (!this_kill_condition) {
+			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++;
+				static int loop_counter = 0;
+				loop_counter++;
 
-			// toggle the MIO7 on and off to show that the quad is in AUTO_FLIGHT_MODE
-			if(loop_counter == 10)
-			{
-				MIO7_led_off();
-			}
-			else if(loop_counter >= 20)
-			{
+				// toggle the MIO7 on and off to show that the quad is in AUTO_FLIGHT_MODE
+				if(loop_counter == 10)
+				{
+					MIO7_led_off();
+				}
+				else if(loop_counter >= 20)
+				{
 
+					MIO7_led_on();
+					loop_counter = 0;
+				}
+
+			}
+			if(structs.user_defined_struct.flight_mode == MANUAL_FLIGHT_MODE) {
 				MIO7_led_on();
-				loop_counter = 0;
 			}
+		}
 
+		if (this_kill_condition == 1 && last_kill_condition == 0) {
+			// Just disabled
+			printLogging(&(structs.log_struct), &(structs.parameter_struct));
+			resetLogging();
+			MIO7_led_off();
 		}
-		if(structs.user_defined_struct.flight_mode == MANUAL_FLIGHT_MODE)
-			MIO7_led_on();
 
-	} while(
-#ifndef BENCH_TEST
-			!kill_condition(&(structs.user_input_struct))
-#else
-			1
-#endif
-			);
+		last_kill_condition = this_kill_condition;
+	}
 
 	pwm_kill();
 
-	MIO7_led_off();
 
-	printLogging(&(structs.log_struct), &(structs.parameter_struct));
 
 	flash_MIO_7_led(10, 100);