diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde
index 852178c43f..438bec2d6b 100644
--- a/ArduCopter/control_modes.pde
+++ b/ArduCopter/control_modes.pde
@@ -92,6 +92,22 @@ static void read_trim_switch()
 		}
 	}
 
+	#elif CH7_OPTION == CH7_SAVE_WP
+
+	if (g.rc_7.control_in > 800){
+		trim_flag = true;
+
+	}else{ // switch is disengaged
+
+		if(trim_flag){
+			// set the next_WP
+			CH7_wp_index++;
+			current_loc.id = MAV_CMD_NAV_WAYPOINT;
+			g.waypoint_total.set_and_save(CH7_wp_index);
+			set_command_with_index(current_loc, CH7_wp_index);
+		}
+	}
+
 	#elif CH7_OPTION == CH7_ADC_FILTER
 	if (g.rc_7.control_in > 800){
 		adc.filter_result = true;
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 19d14c4580..003375460a 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -37,6 +37,7 @@
 #define CH7_RTL 4
 #define CH7_AUTO_TRIM 5
 #define CH7_ADC_FILTER 6
+#define CH7_SAVE_WP 7
 
 // Frame types
 #define QUAD_FRAME 0