diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h
index 34c6017c03..328afef9b3 100644
--- a/ArduCopter/APM_Config.h
+++ b/ArduCopter/APM_Config.h
@@ -52,3 +52,22 @@
 
 //#define RATE_ROLL_I 	0.18
 //#define RATE_PITCH_I	0.18
+
+
+
+
+// agmatthews USERHOOKS
+// the choice of function names is up to the user and does not have to match these
+// uncomment these hooks and ensure there is a matching function un your "UserCode.pde" file
+//#define USERHOOK_FASTLOOP userhook_FastLoop();
+#define USERHOOK_50HZLOOP userhook_50Hz();
+//#define USERHOOK_MEDIUMLOOP userhook_MediumLoop();
+//#define USERHOOK_SLOWLOOP userhook_SlowLoop();
+//#define USERHOOK_SUPERSLOWLOOP userhook_SuperSlowLoop();
+#define USERHOOK_INIT userhook_init();
+
+// the choice of includeed variables file (*.h) is up to the user and does not have to match this one
+// Ensure the defined file exists and is in the arducopter directory
+#define USERHOOK_VARIABLES "UserVariables.h"
+
+
diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 9f447f9170..fb017f079d 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -210,6 +210,14 @@ ModeFilter sonar_mode_filter;
     #error Unrecognised SONAR_TYPE setting.
 #endif
 
+// agmatthews USERHOOKS
+////////////////////////////////////////////////////////////////////////////////
+// User variables
+////////////////////////////////////////////////////////////////////////////////
+#ifdef USERHOOK_VARIABLES
+#include USERHOOK_VARIABLES
+#endif
+
 ////////////////////////////////////////////////////////////////////////////////
 // Global variables
 ////////////////////////////////////////////////////////////////////////////////
@@ -565,6 +573,12 @@ static void fast_loop()
 
 	//if(motor_armed)
 		//Log_Write_Attitude();
+
+// agmatthews - USERHOOKS
+#ifdef USERHOOK_FASTLOOP
+   USERHOOK_FASTLOOP
+#endif
+
 }
 
 static void medium_loop()
@@ -719,6 +733,10 @@ static void medium_loop()
 			medium_loopCounter = 0;
 			break;
 	}
+// agmatthews - USERHOOKS
+#ifdef USERHOOK_MEDIUMLOOP
+   USERHOOK_MEDIUMLOOP
+#endif
 
 }
 
@@ -735,6 +753,10 @@ static void fifty_hz_loop()
 	if(g.sonar_enabled){
 		sonar_alt = sonar.read();
 	}
+	// agmatthews - USERHOOKS
+	#ifdef USERHOOK_50HZLOOP
+	  USERHOOK_50HZLOOP
+	#endif
 
 	#if HIL_MODE != HIL_MODE_DISABLED && FRAME_CONFIG != HELI_FRAME
 		// HIL for a copter needs very fast update of the servo values
@@ -831,6 +853,11 @@ static void slow_loop()
 			break;
 
 	}
+	// agmatthews - USERHOOKS
+	#ifdef USERHOOK_SLOWLOOP
+	   USERHOOK_SLOWLOOP
+	#endif
+
 }
 
 // 1Hz loop
@@ -840,6 +867,10 @@ static void super_slow_loop()
 		Log_Write_Current();
 
     gcs_send_message(MSG_HEARTBEAT);
+	// agmatthews - USERHOOKS
+	#ifdef USERHOOK_SUPERSLOWLOOP
+	   USERHOOK_SUPERSLOWLOOP
+	#endif
 }
 
 static void update_GPS(void)
diff --git a/ArduCopter/UserCode.pde b/ArduCopter/UserCode.pde
new file mode 100644
index 0000000000..88c72429e9
--- /dev/null
+++ b/ArduCopter/UserCode.pde
@@ -0,0 +1,15 @@
+// agmatthews USERHOOKS
+
+void userhook_init()
+{
+	// put your initialisation code here
+
+
+}
+
+void userhook_50Hz()
+{
+	// put your 50Hz code here
+
+
+}
diff --git a/ArduCopter/UserVariables.h b/ArduCopter/UserVariables.h
new file mode 100644
index 0000000000..8c38cba984
--- /dev/null
+++ b/ArduCopter/UserVariables.h
@@ -0,0 +1,13 @@
+// agmatthews USERHOOKS
+// user defined variables
+
+// example variables used in Wii camera testing - replace with your own variables
+#if WII_CAMERA == 1
+WiiCamera               ircam;
+int                     WiiRange=0;
+int                     WiiRotation=0;
+int                     WiiDisplacementX=0;
+int                     WiiDisplacementY=0;
+#endif
+
+
diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index b38be7d787..0863cbe784 100644
--- a/ArduCopter/navigation.pde
+++ b/ArduCopter/navigation.pde
@@ -95,16 +95,9 @@ static void calc_loiter(int x_error, int y_error)
 // nav_roll, nav_pitch
 static void calc_loiter_pitch_roll()
 {
-
-	float temp  	 = radians((float)(9000 - (dcm.yaw_sensor))/100.0);
-	float _cos_yaw_x = cos(temp);
-	float _sin_yaw_y = sin(temp);
-
-	Serial.printf("ys %ld, cyx %1.4f, _cyx %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x);
-
 	// rotate the vector
-	nav_roll 	=  (float)nav_lon * _sin_yaw_y - (float)nav_lat * _cos_yaw_x;
-	nav_pitch 	=  (float)nav_lon * _cos_yaw_x + (float)nav_lat * _sin_yaw_y;
+	nav_roll 	=  (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;
+	nav_pitch 	=  (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y;
 
 	// flip pitch because forward is negative
 	nav_pitch = -nav_pitch;