diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml
index 5113ee0d89..a2cd0b37ae 100644
--- a/conf/modules/cv_opticflow.xml
+++ b/conf/modules/cv_opticflow.xml
@@ -13,18 +13,31 @@
     </description>
 
     <section name="VISION" prefix="VISION_">
-      <define name="PHI_PGAIN" value="500" description="optic flow pgain"/>
-      <define name="PHI_IGAIN" value="10" description="optic flow igain"/>
-      <define name="THETA_PGAIN" value="500" description="optic flow pgain"/>
-      <define name="THETA_IGAIN" value="10" description="optic flow igain"/>
-      <define name="DESIRED_VX" value="0" description="feedforward optic flow vx"/>
-      <define name="DESIRED_VY" value="0" description="feedforward optic flow vy"/>
+      <define name="PHI_PGAIN" value="400" description="Optic flow proportional gain on the roll velocity error"/>
+      <define name="PHI_IGAIN" value="20" description="Optic flow integrated gain on the summed roll velocity error"/>
+      <define name="THETA_PGAIN" value="400" description="Optic flow proportional gain on the pitch velocity error"/>
+      <define name="THETA_IGAIN" value="20" description="Optic flow integrated gain on the summed pitch velocity error"/>
+      <define name="DESIRED_VX" value="0" description="The desired velocity in the body frame x direction"/>
+      <define name="DESIRED_VY" value="0" description="The desired velocity in the body frame y direction"/>
+    </section>
+
+    <section name="OPTICFLOW" prefix="OPTICFLOW_">
+      <define name="AGL_ID" value="ABI_SENDER_ID" description="ABI sender id for AGL message (sonar measurement) (default: ABI_BROADCAST)"/>
+      <define name="MAX_TRACK_CORNERS" value="25" description="The maximum amount of corners the Lucas Kanade algorithm is tracking between two frames"/>
+      <define name="WINDOW_SIZE" value="10" description="Window size used in Lucas Kanade algorithm"/>
+      <define name="SUBPIXEL_FACTOR" value="10" description="Amount of subpixels per pixel, used for more precise (subpixel) calculations of the flow"/>
+      <define name="MAX_ITERATIONS" value="10" description="Maximum number of iterations the Lucas Kanade algorithm should take"/>
+      <define name="THRESHOLD_VEC" value="2" description="TThreshold in subpixels when the iterations of Lucas Kanade should stop"/>
+
+      <define name="FAST9_ADAPTIVE" value="TRUE" description="Whether we should use and adapative FAST9 crner detection threshold"/>
+      <define name="FAST9_THRESHOLD" value="20" description="FAST9 default threshold"/>
+      <define name="FAST9_MIN_DISTANCE" value="10" description="The amount of pixels between corners that should be detected"/>
     </section>
-    <define name="OPTICFLOW_AGL_ID" value="ABI_SENDER_ID" description="ABI sender id for AGL message (sonar measurement) (default: ABI_BROADCAST)"/>
   </doc>
+
   <settings>
-	  <dl_settings NAME="Vision Loop">
-      <dl_settings name="vision">
+	  <dl_settings NAME="Vision stabilization">
+      <dl_settings name="vision_stab">
         <dl_setting var="opticflow_stab.phi_pgain" module="computer_vision/opticflow_module" min="0" step="1" max="10000" shortname="kp_v_phi" param="VISION_PHI_PGAIN"/>
         <dl_setting var="opticflow_stab.phi_igain" module="computer_vision/opticflow_module" min="0" step="1" max="1000" shortname="ki_v_phi" param="VISION_PHI_IGAIN"/>
         <dl_setting var="opticflow_stab.theta_pgain" module="computer_vision/opticflow_module" min="0" step="1" max="10000" shortname="kp_v_theta" param="VISION_THETA_PGAIN"/>
@@ -32,6 +45,18 @@
         <dl_setting var="opticflow_stab.desired_vx" module="computer_vision/opticflow_module" min="-5" step="0.01" max="5" shortname="desired_vx" param="VISION_DESIRED_VX"/>
         <dl_setting var="opticflow_stab.desired_vy" module="computer_vision/opticflow_module" min="-5" step="0.01" max="5" shortname="desired_vy" param="VISION_DESIRED_VY"/>
       </dl_settings>
+
+      <dl_settings name="vision_calc">
+        <dl_setting var="opticflow.max_track_corners" module="computer_vision/opticflow_module" min="0" step="1" max="500" shortname="max_trck_corners" param="OPTICFLOW_MAX_TRACK_CORNERS"/>
+        <dl_setting var="opticflow.window_size" module="computer_vision/opticflow_module" min="0" step="1" max="500" shortname="window_size" param="OPTICFLOW_WINDOW_SIZE"/>
+        <dl_setting var="opticflow.subpixel_factor" module="computer_vision/opticflow_module" min="0" step="1" max="100" shortname="subpixel_factor" param="OPTICFLOW_SUBPIXEL_FACTOR"/>
+        <dl_setting var="opticflow.max_iterations" module="computer_vision/opticflow_module" min="0" step="1" max="100" shortname="max_iterations" param="OPTICFLOW_MAX_ITERATIONS"/>
+        <dl_setting var="opticflow.threshold_vec" module="computer_vision/opticflow_module" min="0" step="1" max="100" shortname="threshold_vec" param="OPTICFLOW_THRESHOLD_VEC"/>
+
+        <dl_setting var="opticflow.fast9_adaptive" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="TRUE|FALSE" shortname="fast9_adaptive" param="OPTICFLOW_FAST9_ADAPTIVE"/>
+        <dl_setting var="opticflow.fast9_threshold" module="computer_vision/opticflow_module" min="0" step="1" max="255" shortname="fast9_threshold" param="OPTICFLOW_FAST9_THRESHOLD"/>
+        <dl_setting var="opticflow.fast9_min_distance" module="computer_vision/opticflow_module" min="0" step="1" max="500" shortname="fast9_min_distance" param="OPTICFLOW_FAST9_MIN_DISTANCE"/>
+      </dl_settings>
     </dl_settings>
   </settings>
 
diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
index e4ff24095d..8e037fcee8 100644
--- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
+++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
@@ -46,6 +46,47 @@
 #define Fx_ARdrone 343.1211
 #define Fy_ARdrone 348.5053
 
+/* Set the default values */
+#ifndef OPTICFLOW_MAX_TRACK_CORNERS
+#define OPTICFLOW_MAX_TRACK_CORNERS 25
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_MAX_TRACK_CORNERS);
+
+#ifndef OPTICFLOW_WINDOW_SIZE
+#define OPTICFLOW_WINDOW_SIZE 10
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_WINDOW_SIZE);
+
+#ifndef OPTICFLOW_SUBPIXEL_FACTOR
+#define OPTICFLOW_SUBPIXEL_FACTOR 10
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_SUBPIXEL_FACTOR);
+
+#ifndef OPTICFLOW_MAX_ITERATIONS
+#define OPTICFLOW_MAX_ITERATIONS 10
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_MAX_ITERATIONS);
+
+#ifndef OPTICFLOW_THRESHOLD_VEC
+#define OPTICFLOW_THRESHOLD_VEC 2
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_THRESHOLD_VEC);
+
+#ifndef OPTICFLOW_FAST9_ADAPTIVE
+#define OPTICFLOW_FAST9_ADAPTIVE TRUE
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_FAST9_ADAPTIVE);
+
+#ifndef OPTICFLOW_FAST9_THRESHOLD
+#define OPTICFLOW_FAST9_THRESHOLD 20
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_FAST9_THRESHOLD);
+
+#ifndef OPTICFLOW_FAST9_MIN_DISTANCE
+#define OPTICFLOW_FAST9_MIN_DISTANCE 10
+#endif
+PRINT_CONFIG_VAR(OPTICFLOW_FAST9_MIN_DISTANCE);
+
 /* Functions only used here */
 static uint32_t timeval_diff(struct timeval *starttime, struct timeval *finishtime);
 static int cmp_flow(const void *a, const void *b);
@@ -66,6 +107,17 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
   opticflow->got_first_img = FALSE;
   opticflow->prev_phi = 0.0;
   opticflow->prev_theta = 0.0;
+
+  /* Set the default values */
+  opticflow->max_track_corners = OPTICFLOW_MAX_TRACK_CORNERS;
+  opticflow->window_size = OPTICFLOW_WINDOW_SIZE;
+  opticflow->subpixel_factor = OPTICFLOW_SUBPIXEL_FACTOR;
+  opticflow->max_iterations = OPTICFLOW_MAX_ITERATIONS;
+  opticflow->threshold_vec = OPTICFLOW_THRESHOLD_VEC;
+
+  opticflow->fast9_adaptive = OPTICFLOW_FAST9_ADAPTIVE;
+  opticflow->fast9_threshold = OPTICFLOW_FAST9_THRESHOLD;
+  opticflow->fast9_min_distance = OPTICFLOW_FAST9_MIN_DISTANCE;
 }
 
 /**
@@ -95,14 +147,18 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
   // *************************************************************************************
 
   // FAST corner detection (TODO: non fixed threashold)
-  static uint8_t threshold = 20;
-  struct point_t *corners = fast9_detect(img, threshold, 10, 20, 20, &result->corner_cnt);
+  struct point_t *corners = fast9_detect(img, opticflow->fast9_threshold, opticflow->fast9_min_distance,
+    20, 20, &result->corner_cnt);
 
   // Adaptive threshold
-  if(result->corner_cnt < 40 && threshold > 5)
-    threshold--;
-  else if(result->corner_cnt > 50 && threshold < 60)
-    threshold++;
+  if(opticflow->fast9_adaptive) {
+
+    // Decrease and increase the threshold based on previous values
+    if(result->corner_cnt < 40 && opticflow->fast9_threshold > 5)
+      opticflow->fast9_threshold--;
+    else if(result->corner_cnt > 50 && opticflow->fast9_threshold < 60)
+      opticflow->fast9_threshold++;
+  }
 
 #if OPTICFLOW_DEBUG && OPTICFLOW_SHOW_CORNERS
   image_show_points(img, corners, result->corner_cnt);
@@ -119,18 +175,14 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
   // Corner Tracking
   // *************************************************************************************
 
-#define MAX_TRACK_CORNERS 25
-#define HALF_WINDOW_SIZE 5
-#define SUBPIXEL_FACTOR 10
-#define MAX_ITERATIONS 10
-#define THRESHOLD_VEC 2
   // Execute a Lucas Kanade optical flow
   result->tracked_cnt = result->corner_cnt;
   struct flow_t *vectors = opticFlowLK(&opticflow->img_gray, &opticflow->prev_img_gray, corners, &result->tracked_cnt,
-    HALF_WINDOW_SIZE, SUBPIXEL_FACTOR, MAX_ITERATIONS, THRESHOLD_VEC, MAX_TRACK_CORNERS);
+    opticflow->window_size/2, opticflow->subpixel_factor, opticflow->max_iterations,
+    opticflow->threshold_vec, opticflow->max_track_corners);
 
 #if OPTICFLOW_DEBUG && OPTICFLOW_SHOW_FLOW
-  image_show_flow(img, vectors, result->tracked_cnt, SUBPIXEL_FACTOR);
+  image_show_flow(img, vectors, result->tracked_cnt, opticflow->subpixel_factor);
 #endif
 
   // Get the median flow
@@ -158,14 +210,14 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
   // Flow Derotation
   float diff_flow_x = (state->phi - opticflow->prev_phi) * img->w / FOV_W;
   float diff_flow_y = (state->theta - opticflow->prev_theta) * img->h / FOV_H;
-  result->flow_der_x = result->flow_x - diff_flow_x * SUBPIXEL_FACTOR;
-  result->flow_der_y = result->flow_y - diff_flow_y * SUBPIXEL_FACTOR;
+  result->flow_der_x = result->flow_x - diff_flow_x * opticflow->subpixel_factor;
+  result->flow_der_y = result->flow_y - diff_flow_y * opticflow->subpixel_factor;
   opticflow->prev_phi = state->phi;
   opticflow->prev_theta = state->theta;
 
   // Velocity calculation
-  result->vel_x = -result->flow_der_x * result->fps / SUBPIXEL_FACTOR * img->w / Fx_ARdrone;
-  result->vel_y =  result->flow_der_y * result->fps / SUBPIXEL_FACTOR * img->h / Fy_ARdrone;
+  result->vel_x = -result->flow_der_x * result->fps / opticflow->subpixel_factor * img->w / Fx_ARdrone;
+  result->vel_y =  result->flow_der_y * result->fps / opticflow->subpixel_factor * img->h / Fy_ARdrone;
 
   // *************************************************************************************
   // Next Loop Preparation
diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
index f010fc8284..f30c0dc566 100644
--- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
+++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.h
@@ -42,6 +42,16 @@ struct opticflow_t
   struct image_t img_gray;          //< Current gray image frame
   struct image_t prev_img_gray;     //< Previous gray image frame
   struct timeval prev_timestamp;    //< Timestamp of the previous frame, used for FPS calculation
+
+  uint8_t max_track_corners;        //< Maximum amount of corners Lucas Kanade should track
+  uint16_t window_size;             //< Window size of the Lucas Kanade calculation (needs to be even)
+  uint8_t subpixel_factor;          //< The amount of subpixels per pixel
+  uint8_t max_iterations;           //< The maximum amount of iterations the Lucas Kanade algorithm should do
+  uint8_t threshold_vec;            //< The threshold in x, y subpixels which the algorithm should stop
+
+  bool_t fast9_adaptive;            //< Whether the FAST9 threshold should be adaptive
+  uint8_t fast9_threshold;          //< FAST9 corner detection threshold
+  uint16_t fast9_min_distance;      //< Minimum distance in pixels between corners
 };
 
 
diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c
index 92312b985f..b1071a5167 100644
--- a/sw/airborne/modules/computer_vision/opticflow_module.c
+++ b/sw/airborne/modules/computer_vision/opticflow_module.c
@@ -64,7 +64,7 @@ PRINT_CONFIG_MSG("OPTICFLOW_DEVICE_SIZE = " _SIZE_HELPER(OPTICFLOW_DEVICE_SIZE))
 PRINT_CONFIG_VAR(VIEWVIDEO_DEVICE_BUFFERS);
 
 /* The main opticflow variables */
-static struct opticflow_t opticflow;                //< Opticflow calculations
+struct opticflow_t opticflow;                       //< Opticflow calculations
 static struct opticflow_result_t opticflow_result;  //< The opticflow result
 static struct opticflow_state_t opticflow_state;    //< State of the drone to communicate with the opticflow
 static struct v4l2_device *opticflow_dev;           //< The opticflow camera V4L2 device
diff --git a/sw/airborne/modules/computer_vision/opticflow_module.h b/sw/airborne/modules/computer_vision/opticflow_module.h
index 5e583ce5a6..eb158cc83c 100644
--- a/sw/airborne/modules/computer_vision/opticflow_module.h
+++ b/sw/airborne/modules/computer_vision/opticflow_module.h
@@ -32,6 +32,9 @@
 #include "opticflow/opticflow_calculator.h"
 #include "opticflow/stabilization_opticflow.h"
 
+// Needed for settings
+extern struct opticflow_t opticflow;
+
 // Module functions
 extern void opticflow_module_init(void);
 extern void opticflow_module_run(void);