diff --git a/APMrover2/APM_Config.h b/APMrover2/APM_Config.h
index 30a5ae2646..93d61b4b20 100644
--- a/APMrover2/APM_Config.h
+++ b/APMrover2/APM_Config.h
@@ -14,24 +14,3 @@
 
 #define TRACE               DISABLED
 
-//#define LOGGING_ENABLED DISABLED
-
-//#include "APM_Config_HILmode.h"  // for test in HIL mode with AeroSIM Rc 3.83
-//#include "APM_Config_Rover.h"      // to be used with the real Traxxas model Monster Jam Grinder
-
-// Radio setup:
-// APM INPUT (Rec = receiver)
-// Rec ch1: Roll 
-// Rec ch2: Throttle
-// Rec ch3: Pitch
-// Rec ch4: Yaw
-// Rec ch5: not used
-// Rec ch6: 
-// Rec ch7: Option channel to 2 positions switch
-// Rec ch8: Mode channel to 3 positions switch
-// APM OUTPUT
-// Ch1: Wheel servo (direction)
-// Ch2: not used
-// Ch3: to the motor ESC
-// Ch4: not used
-
diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index b05e5ad093..d3f88cae74 100644
--- a/APMrover2/APMrover2.pde
+++ b/APMrover2/APMrover2.pde
@@ -354,9 +354,7 @@ static int16_t 	failsafe;
 // Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold
 // RC receiver should be set up to output a low throttle value when signal is lost
 static bool 	ch3_failsafe;
-// A timer used to help recovery from unusual attitudes.  If we enter an unusual attitude 
-// while in autonomous flight this variable is used  to hold roll at 0 for a recovery period
-static byte    crash_timer;
+
 // A timer used to track how long since we have received the last GCS heartbeat or rc override message
 static uint32_t rc_override_fs_timer = 0;
 // A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal
@@ -481,8 +479,6 @@ static bool speed_boost = false;
 ////////////////////////////////////////////////////////////////////////////////
 // The instantaneous desired bank angle.  Hundredths of a degree
 static int32_t nav_roll;
-// The instantaneous desired pitch angle.  Hundredths of a degree
-static int32_t nav_pitch;
 // Calculated radius for the wp turn based on ground speed and max turn angle
 static int32_t    wp_radius;
 
@@ -935,7 +931,6 @@ static void update_current_flight_mode(void)
     case LEARNING:
     case MANUAL:
         nav_roll        = 0;
-        nav_pitch       = 0;
         g.channel_roll.servo_out = g.channel_roll.pwm_to_angle();
         g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle();
         g.channel_rudder.servo_out = g.channel_roll.pwm_to_angle();
@@ -945,25 +940,20 @@ static void update_current_flight_mode(void)
 
 static void update_navigation()
 {
-	// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
-	// ------------------------------------------------------------------------
-
-	// distance and bearing calcs only
-	if(control_mode == AUTO){
+    switch (control_mode) {
+    case AUTO:
 		verify_commands();
-	}else{
+        break;
 
-		switch(control_mode){
-			case RTL:        // no loitering around the wp with the rover, goes direct to the wp position
-			case GUIDED:
-				calc_nav_roll();
-				calc_bearing_error();
-                if(verify_RTL()) {  
-                    g.channel_throttle.servo_out = g.throttle_min.get();
-                    set_mode(MANUAL);
-                }
-				break;
-
-		}
+    case RTL:
+    case GUIDED:
+        // no loitering around the wp with the rover, goes direct to the wp position
+        calc_nav_roll();
+        calc_bearing_error();
+        if(verify_RTL()) {  
+            g.channel_throttle.servo_out = g.throttle_min.get();
+            set_mode(MANUAL);
+        }
+        break;
 	}
 }
diff --git a/APMrover2/Attitude.pde b/APMrover2/Attitude.pde
index 77bacd5104..690504a634 100644
--- a/APMrover2/Attitude.pde
+++ b/APMrover2/Attitude.pde
@@ -54,7 +54,6 @@ static void calc_throttle()
 
 static void calc_nav_roll()
 {
-
 	// Adjust gain based on ground speed
 	nav_gain_scaler = (float)ground_speed / (g.airspeed_cruise * 100.0);
 	nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);
@@ -62,17 +61,16 @@ static void calc_nav_roll()
 	// Calculate the required turn of the wheels rover
 	// ----------------------------------------
 
-        // negative error = left turn
+    // negative error = left turn
 	// positive error = right turn
 
 	nav_roll = g.pidNavRoll.get_pid(bearing_error, nav_gain_scaler);	//returns desired bank angle in degrees*100
 
-      if(obstacle) {  // obstacle avoidance 
+    if(obstacle) {  // obstacle avoidance 
 	    nav_roll += 9000;    // if obstacle in front turn 90° right	
-            speed_boost = false;
-      }
+        speed_boost = false;
+    }
 	nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
-
 }
 
 // Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
@@ -81,7 +79,6 @@ static void reset_I(void)
 {
 	g.pidNavRoll.reset_I();
 	g.pidTeThrottle.reset_I();
-//	g.pidAltitudeThrottle.reset_I();
 }
 
 /*****************************************
diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde
index 81a4975ba2..72b48d428c 100644
--- a/APMrover2/GCS_Mavlink.pde
+++ b/APMrover2/GCS_Mavlink.pde
@@ -218,7 +218,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
     mavlink_msg_nav_controller_output_send(
         chan,
         nav_roll / 1.0e2,
-        nav_pitch / 1.0e2,
+        0,
         bearing,
         target_bearing / 100,
         wp_distance,
diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde
index 3597557c88..bd0d12e80a 100644
--- a/APMrover2/Log.pde
+++ b/APMrover2/Log.pde
@@ -304,7 +304,7 @@ static void Log_Write_Control_Tuning()
 	DataFlash.WriteInt((int)nav_roll);
 	DataFlash.WriteInt((int)ahrs.roll_sensor);
 	DataFlash.WriteInt((int)(g.channel_pitch.servo_out));
-	DataFlash.WriteInt((int)nav_pitch);
+	DataFlash.WriteInt(0); // nav_pitch
 	DataFlash.WriteInt((int)ahrs.pitch_sensor);
 	DataFlash.WriteInt((int)(g.channel_throttle.servo_out));
 	DataFlash.WriteInt((int)(g.channel_rudder.servo_out));
diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde
index 0deba92466..0f7c3f9fae 100644
--- a/APMrover2/commands_logic.pde
+++ b/APMrover2/commands_logic.pde
@@ -172,9 +172,8 @@ static bool verify_condition_command()		// Returns true if command complete
 
 static void do_RTL(void)
 {
-        prev_WP 		= current_loc;
+    prev_WP 		= current_loc;
 	control_mode 	= RTL;
-	crash_timer 	= 0;
 	next_WP 		= home;
 }
 
diff --git a/APMrover2/system.pde b/APMrover2/system.pde
index 83cc166740..975d57124c 100644
--- a/APMrover2/system.pde
+++ b/APMrover2/system.pde
@@ -364,7 +364,6 @@ static void set_mode(byte mode)
 		trim_control_surfaces();
 
 	control_mode = mode;
-	crash_timer = 0;
     throttle_last = 0;
     throttle = 500;