diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp
index a0786ac74956a2199381d13bc4f607fdd7c60d9d..869306a293464316a460caa09c2ec65d402fb3d1 100644
--- a/libraries/AP_Mount/AP_Mount.cpp
+++ b/libraries/AP_Mount/AP_Mount.cpp
@@ -9,6 +9,13 @@
 #define ENABLED                 1
 #define DISABLED                0
 
+#define MASK_TILT           (1<<0)
+#define MASK_ROLL           (1<<1)
+#define MASK_YAW            (1<<2)
+#define MASK_RETRACT        (1<<3)
+
+
+
 #if defined( __AVR_ATmega1280__ )
  # define MNT_JSTICK_SPD_OPTION DISABLED // Allow RC joystick to control the speed of the mount movements instead of the position of the mount
  # define MNT_RETRACT_OPTION    DISABLED // Use a servo to retract the mount inside the fuselage (i.e. for landings)
@@ -276,21 +283,31 @@ void
 AP_Mount::update_mount_type()
 {
 	bool have_roll, have_tilt, have_pan;
+
 	have_roll = RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount_roll) ||
 		RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount2_roll);
+
 	have_tilt = RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount_tilt) ||
 		RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount2_tilt);
+
 	have_pan = RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount_pan) ||
 		RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount2_pan);
-    if (have_pan && have_tilt && !have_roll) {
+    
+    mount_axis_mask = 0;
+    
+    if (have_pan)
+        mount_axis_mask |= MASK_YAW;
+    if (have_tilt)
+        mount_axis_mask |= MASK_TILT;
+    if (have_roll)
+        mount_axis_mask |= MASK_ROLL;
+    
+    if (have_pan && have_tilt && !have_roll)
         _mount_type = k_pan_tilt;
-    }
-    if (!have_pan && have_tilt && have_roll) {
+    if (!have_pan && have_tilt && have_roll)
         _mount_type = k_tilt_roll;
-    }
-    if (have_pan && have_tilt && have_roll) {
+    if (have_pan && have_tilt && have_roll)
         _mount_type = k_pan_tilt_roll;
-    }
 }
 
 /// sets the servo angles for retraction, note angles are in degrees
@@ -598,8 +615,18 @@ AP_Mount::calc_GPS_target_angle(const struct Location *target)
     float GPS_vector_z = (target->alt-_current_loc->alt);                 // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter).
     float target_distance = 100.0f*pythagorous2(GPS_vector_x, GPS_vector_y);      // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
     _roll_control_angle  = 0;
-    _tilt_control_angle  = atan2f(GPS_vector_z, target_distance);
-    _pan_control_angle   = atan2f(GPS_vector_x, GPS_vector_y);
+    
+    if (mount_axis_mask & MASK_TILT) {
+        _tilt_control_angle = atan2f(GPS_vector_z, target_distance);
+    } else {
+        _tilt_control_angle = 0;
+    }
+        
+    if (mount_axis_mask & MASK_YAW) {
+        _pan_control_angle = atan2f(GPS_vector_x, GPS_vector_y);
+    } else {
+        _pan_control_angle = 0;
+    }
 }
 
 /// Stabilizes mount relative to the Earth's frame
diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h
index fa99d03d5c607f2e5c2f0fe342c7b6d8fdd04705..92e9629ffa053dab8626687a98a55d4f7134712f 100644
--- a/libraries/AP_Mount/AP_Mount.h
+++ b/libraries/AP_Mount/AP_Mount.h
@@ -98,6 +98,8 @@ private:
     uint8_t                         _pan_idx;  ///< RC_Channel_aux mount pan  function index
     uint8_t                         _open_idx; ///< RC_Channel_aux mount open function index
 
+    uint8_t                         mount_axis_mask; //  used to track user input on each axis
+    
     float                           _roll_control_angle; ///< radians
     float                           _tilt_control_angle; ///< radians
     float                           _pan_control_angle;  ///< radians