Reverting first setpoint for autonomous back to current position. Was swtiched...

Reverting first setpoint for autonomous back to current position. Was swtiched to (0, 0) for optical flow.

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);