diff --git a/quad/src/quad_app/control_algorithm.c b/quad/src/quad_app/control_algorithm.c
index 69729052e88c8f509a6bf27450aa01f979fb3351..624c0642abfd47bc4de3cc586d9226dca28fa2a6 100644
--- a/quad/src/quad_app/control_algorithm.c
+++ b/quad/src/quad_app/control_algorithm.c
@@ -354,8 +354,8 @@ int control_algorithm_init(parameter_t * ps)
 	// also reset the previous error and accumulated error from the position PIDs
 	if((cur_fm_switch == AUTO_FLIGHT_MODE) && (user_defined_struct->engaging_auto == 2))
 	{
-		graph_set_param_val(graph, ps->x_set, CONST_SET, 0);
-		graph_set_param_val(graph, ps->y_set, CONST_SET, 0);
+		graph_set_param_val(graph, ps->x_set, CONST_SET, sensor_struct->currentQuadPosition.x_pos);
+		graph_set_param_val(graph, ps->y_set, CONST_SET, sensor_struct->currentQuadPosition.y_pos);
 		graph_set_param_val(graph, ps->alt_set, CONST_SET, sensor_struct->currentQuadPosition.alt_pos);
 		graph_set_param_val(graph, ps->yaw_set, CONST_SET, sensor_struct->currentQuadPosition.yaw);