diff --git a/libraries/APM_OBC/APM_OBC.cpp b/libraries/APM_OBC/APM_OBC.cpp
index 53e848b2e3..7ad8b7e8b9 100644
--- a/libraries/APM_OBC/APM_OBC.cpp
+++ b/libraries/APM_OBC/APM_OBC.cpp
@@ -125,7 +125,7 @@ APM_OBC::check(APM_OBC::control_mode mode,
 			_state = STATE_GPS_LOSS;
 			if (_wp_gps_loss) {
 				if (_command_index != NULL) {
-					_saved_wp = _command_index->get() + 1;
+					_saved_wp = _command_index->get();
 				}
 				change_command(_wp_gps_loss);
 			}