diff --git a/sw/airborne/modules/computer_vision/cv/framerate.c b/sw/airborne/modules/computer_vision/cv/framerate.c
index c3fec4e2a70..9ec262d0bcc 100644
--- a/sw/airborne/modules/computer_vision/cv/framerate.c
+++ b/sw/airborne/modules/computer_vision/cv/framerate.c
@@ -19,7 +19,7 @@
  */
 
 /**
- * @file modules/computer_vision/cv/framerate.h
+ * @file modules/computer_vision/cv/framerate.c
  *
  */
 
@@ -36,12 +36,6 @@ struct timeval end_time;
 
 #define USEC_PER_SEC 1000000L
 
-static float framerate_FPS;
-
-float framerate_get(void) {
-  return framerate_FPS;
-}
-
 static long time_elapsed(struct timeval *t1, struct timeval *t2)
 {
   long sec, usec;
@@ -68,15 +62,15 @@ static long end_timer(void)
 
 void framerate_init(void) {
   // Frame Rate Initialization
-  framerate_FPS = 0.0;
   timestamp = 0;
   start_timer();
 }
 
-void framerate_run(void) {
+float framerate_run(void) {
   // FPS
   timestamp = end_timer();
-  framerate_FPS = (float) 1000000 / (float)timestamp;
+  float framerate_FPS = (float) 1000000 / (float)timestamp;
   //  printf("dt = %d, FPS = %f\n",timestamp, FPS);
   start_timer();
+  return framerate_FPS;
 }
diff --git a/sw/airborne/modules/computer_vision/cv/framerate.h b/sw/airborne/modules/computer_vision/cv/framerate.h
index 47b236f1790..9044881ff05 100644
--- a/sw/airborne/modules/computer_vision/cv/framerate.h
+++ b/sw/airborne/modules/computer_vision/cv/framerate.h
@@ -25,5 +25,4 @@
 
 
 void framerate_init(void);
-void framerate_run(void);
-float framerate_get(void);
+float framerate_run(void);
diff --git a/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h
index eed79db6e0d..e28f44ab2fb 100644
--- a/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h
+++ b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h
@@ -8,24 +8,26 @@
 
 // Data from thread to module
 struct CVresults {
-  int cnt;
-  int status;
-  float FPS;
-  float Velx;
+  int cnt;          // Number of processed frames
+
+  float Velx;       // Velocity as measured by camera
   float Vely;
   int flow_count;
-  float cam_h;
+
+  float cam_h;      // Debug parameters
   int count;
   float OFx, OFy, dx_sum, dy_sum;
-  float diff_roll, diff_pitch;
+  float diff_roll;
+  float diff_pitch;
+  float FPS;
 };
 
 // Data from module to thread
 struct PPRZinfo {
-  int cnt;
-  float theta;
-  float phi;
-  float agl;
+  int cnt;        // IMU msg counter
+  float phi;      // roll [rad]
+  float theta;    // pitch [rad]
+  float agl;      // height above ground [m]
 };
 
 
diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c
index cf0ad3db340..d5d8e46e7f3 100644
--- a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c
+++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c
@@ -1,3 +1,27 @@
+/*
+ * Copyright (C) 2015 The Paparazzi Community
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file modules/computer_vision/opticflow/opticflow_thread.c
+ *
+ */
 
 
 
@@ -29,10 +53,8 @@
 #endif
 
 #include <stdio.h>
-
 #define DEBUG_INFO(X, ...) ;
 
-
 static volatile enum{RUN,EXIT} computer_vision_thread_command = RUN;  /** request to close: set to 1 */
 
 void computervision_thread_request_exit(void) {
@@ -43,11 +65,13 @@ void *computervision_thread_main(void *args)
 {
   int thread_socket = *(int *) args;
 
-  computer_vision_thread_command = RUN;
-
+  // Local data in/out
   struct CVresults vision_results;
   struct PPRZinfo autopilot_data;
 
+  // Status
+  computer_vision_thread_command = RUN;
+
   // Video Input
   struct vid_struct vid;
   vid.device = (char *)"/dev/video2"; // video1 = front camera; video2 = bottom camera
@@ -55,11 +79,8 @@ void *computervision_thread_main(void *args)
   vid.h = 240;
   vid.n_buffers = 4;
 
-  vision_results.status = 1;
-
   if (video_init(&vid) < 0) {
     printf("Error initialising video\n");
-    vision_results.status = -1;
     return 0;
   }
 
@@ -81,10 +102,11 @@ void *computervision_thread_main(void *args)
   opticflow_plugin_init(vid.w, vid.h, &vision_results);
 
   while (computer_vision_thread_command == RUN) {
-    vision_results.status = 2;
+
+    // Wait for a new frame
     video_grab_image(&vid, img_new);
 
-    // Get most recent State
+    // Get most recent State information
     int bytes_read = sizeof(autopilot_data);
     while (bytes_read == sizeof(autopilot_data))
     {
@@ -95,13 +117,12 @@ void *computervision_thread_main(void *args)
         }
       }
     }
-
     DEBUG_INFO("[thread] Read # %d\n",autopilot_data.cnt);
 
-    // Run Image Processing
+    // Run Image Processing with image and data and get results
     opticflow_plugin_run(img_new->buf, &autopilot_data, &vision_results);
 
-    /* send results to main */
+    /* Send results to main */
     vision_results.cnt++;
     int bytes_written = write(thread_socket, &vision_results, sizeof(vision_results));
     if (bytes_written != sizeof(vision_results)){
@@ -109,7 +130,6 @@ void *computervision_thread_main(void *args)
     }
     DEBUG_INFO("[thread] Write # %d, (bytes %d)\n",vision_results.cnt, bytes_written);
 
-
 #ifdef DOWNLINK_VIDEO
     // JPEG encode the image:
     uint32_t quality_factor = 10; //20 if no resize,
@@ -119,12 +139,11 @@ void *computervision_thread_main(void *args)
     uint32_t size = end - (jpegbuf);
 
     printf("Sending an image ...%u\n", size);
-    uint32_t delta_t_per_frame = 0; // 0 = use drone clock
-    send_rtp_frame(vsock, jpegbuf, size, small.w, small.h, 0, quality_factor, dri_header, delta_t_per_frame);
+    send_rtp_frame(vsock, jpegbuf, size, small.w, small.h, 0, quality_factor, dri_header, 0);
 #endif
   }
+
   printf("Thread Closed\n");
   video_close(&vid);
-  vision_results.status = -100;
   return 0;
 }
diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h
index 157de7273c5..06ad218cae5 100644
--- a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h
+++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h
@@ -19,11 +19,10 @@
  */
 
 /**
- * @file modules/computer_vision/opticflow/opticflow_thread.c
+ * @file modules/computer_vision/opticflow/opticflow_thread.h
  * @brief computer vision thread
  *
- * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
- */
+  */
 
 
 void *computervision_thread_main(void *args);  /* computer vision thread: should be given a pointer to a socketpair as argument */
diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c
index b560427f6ea..cb10364ca73 100644
--- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c
+++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c
@@ -27,6 +27,8 @@
 
 // Warning: all this code is called form the Vision-Thread: do not access any autopilot data in here.
 
+#include "std.h"
+
 #include <stdio.h>
 #include <string.h>
 #include <stdlib.h>
@@ -41,12 +43,26 @@
 // for FPS
 #include "modules/computer_vision/cv/framerate.h"
 
-// Image size set at init
-unsigned int imgWidth, imgHeight;
 
 // Local variables
-unsigned char *prev_frame, *gray_frame, *prev_gray_frame;
-int old_img_init;
+struct visual_estimator_struct
+{
+  // Image size
+  unsigned int imgWidth;
+  unsigned int imgHeight;
+
+  // Images
+  uint8_t *prev_frame;
+  uint8_t *gray_frame;
+  uint8_t *prev_gray_frame;
+
+  // Initialization
+  int old_img_init;
+
+  // Store previous
+  float prev_pitch;
+  float prev_roll;
+} visual_estimator;
 
 // ARDrone Vertical Camera Parameters
 #define FOV_H 0.67020643276
@@ -55,44 +71,27 @@ int old_img_init;
 #define Fy_ARdrone 348.5053
 
 // Corner Detection
-int *x, *y;
-int max_count = 25;
 #define MAX_COUNT 100
 
-// Corner Tracking
-int *new_x, *new_y, *status, *dx, *dy;
-int error_opticflow;
-int remove_point;
-int c;
-int borderx = 24, bordery = 24;
-
-// Remove bad corners
-float distance2, min_distance, min_distance2;
-
 // Flow Derotation
 #define FLOW_DEROTATION
-float curr_pitch, curr_roll, prev_pitch, prev_roll;
-float OFx_trans, OFy_trans;
-
 
 
 // Called by plugin
 void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *results)
 {
   // Initialize variables
-  imgWidth = w;
-  imgHeight = h;
-  gray_frame = (unsigned char *) calloc(imgWidth * imgHeight, sizeof(unsigned char));
-  prev_frame = (unsigned char *) calloc(imgWidth * imgHeight * 2, sizeof(unsigned char));
-  prev_gray_frame = (unsigned char *) calloc(imgWidth * imgHeight, sizeof(unsigned char));
-  x = (int *) calloc(MAX_COUNT, sizeof(int));
-  new_x = (int *) calloc(MAX_COUNT, sizeof(int));
-  y = (int *) calloc(MAX_COUNT, sizeof(int));
-  new_y = (int *) calloc(MAX_COUNT, sizeof(int));
-  status = (int *) calloc(MAX_COUNT, sizeof(int));
-  dx = (int *) calloc(MAX_COUNT, sizeof(int));
-  dy = (int *) calloc(MAX_COUNT, sizeof(int));
-  old_img_init = 1;
+  visual_estimator.imgWidth = w;
+  visual_estimator.imgHeight = h;
+
+  visual_estimator.gray_frame = (unsigned char *) calloc(w * h, sizeof(uint8_t));
+  visual_estimator.prev_frame = (unsigned char *) calloc(w * h * 2, sizeof(uint8_t));
+  visual_estimator.prev_gray_frame = (unsigned char *) calloc(w * h, sizeof(uint8_t));
+
+  visual_estimator.old_img_init = 1;
+  visual_estimator.prev_pitch = 0.0;
+  visual_estimator.prev_roll = 0.0;
+
   results->OFx = 0.0;
   results->OFy = 0.0;
   results->dx_sum = 0.0;
@@ -100,17 +99,10 @@ void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *res
   results->diff_roll = 0.0;
   results->diff_pitch = 0.0;
   results->cam_h = 0.0;
-  prev_pitch = 0.0;
-  prev_roll = 0.0;
-  curr_pitch = 0.0;
-  curr_roll = 0.0;
-  OFx_trans = 0.0;
-  OFy_trans = 0.0;
   results->Velx = 0.0;
   results->Vely = 0.0;
   results->flow_count = 0;
   results->cnt = 0;
-  results->status = 0;
   results->count = 0;
 
   framerate_init();
@@ -118,12 +110,24 @@ void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *res
 
 void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CVresults *results)
 {
-  framerate_run();
-
-  if (old_img_init == 1) {
-    memcpy(prev_frame, frame, imgHeight * imgWidth * 2);
-    CvtYUYV2Gray(prev_gray_frame, prev_frame, imgWidth, imgHeight);
-    old_img_init = 0;
+  // Corner Tracking
+  // Working Variables
+  int max_count = 25;
+  int borderx = 24, bordery = 24;
+  int x[MAX_COUNT], y[MAX_COUNT];
+  int new_x[MAX_COUNT], new_y[MAX_COUNT];
+  int status[MAX_COUNT];
+  int dx[MAX_COUNT], dy[MAX_COUNT];
+  int w = visual_estimator.imgWidth;
+  int h = visual_estimator.imgHeight;
+
+  // Framerate Measuring
+  results->FPS = framerate_run();
+
+  if (visual_estimator.old_img_init == 1) {
+    memcpy(visual_estimator.prev_frame, frame, w * h * 2);
+    CvtYUYV2Gray(visual_estimator.prev_gray_frame, visual_estimator.prev_frame, w, h);
+    visual_estimator.old_img_init = 0;
   }
 
   // *************************************************************************************
@@ -133,9 +137,8 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV
   // FAST corner detection
   int fast_threshold = 20;
   xyFAST *pnts_fast;
-  pnts_fast = fast9_detect((const byte *)prev_gray_frame, imgWidth, imgHeight, imgWidth,
+  pnts_fast = fast9_detect((const byte *)visual_estimator.prev_gray_frame, w, h, w,
                            fast_threshold, &results->count);
-
   if (results->count > MAX_COUNT) { results->count = MAX_COUNT; }
   for (int i = 0; i < results->count; i++) {
     x[i] = pnts_fast[i].x;
@@ -143,15 +146,14 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV
   }
   free(pnts_fast);
 
-  // Remove neighbouring corners
-  min_distance = 3;
-  min_distance2 = min_distance * min_distance;
-  int *labelmin;
-  labelmin = (int *) calloc(MAX_COUNT, sizeof(int));
+  // Remove neighboring corners
+  const float min_distance = 3;
+  float min_distance2 = min_distance * min_distance;
+  int labelmin[MAX_COUNT];
   for (int i = 0; i < results->count; i++) {
     for (int j = i + 1; j < results->count; j++) {
       // distance squared:
-      distance2 = (x[i] - x[j]) * (x[i] - x[j]) + (y[i] - y[j]) * (y[i] - y[j]);
+      float distance2 = (x[i] - x[j]) * (x[i] - x[j]) + (y[i] - y[j]) * (y[i] - y[j]);
       if (distance2 < min_distance2) {
         labelmin[i] = 1;
       }
@@ -160,14 +162,14 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV
 
   int count_fil = results->count;
   for (int i = results->count - 1; i >= 0; i--) {
-    remove_point = 0;
+    int remove_point = 0;
 
     if (labelmin[i]) {
       remove_point = 1;
     }
 
     if (remove_point) {
-      for (c = i; c < count_fil - 1; c++) {
+      for (int c = i; c < count_fil - 1; c++) {
         x[c] = x[c + 1];
         y[c] = y[c + 1];
       }
@@ -177,27 +179,26 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV
 
   if (count_fil > max_count) { count_fil = max_count; }
   results->count = count_fil;
-  free(labelmin);
 
   // *************************************************************************************
   // Corner Tracking
   // *************************************************************************************
-  CvtYUYV2Gray(gray_frame, frame, imgWidth, imgHeight);
+  CvtYUYV2Gray(visual_estimator.gray_frame, frame, w, h);
 
-  error_opticflow = opticFlowLK(gray_frame, prev_gray_frame, x, y, count_fil, imgWidth,
-                                imgHeight, new_x, new_y, status, 5, 100);
+ opticFlowLK(visual_estimator.gray_frame, visual_estimator.prev_gray_frame, x, y, count_fil, w,
+                                h, new_x, new_y, status, 5, 100);
 
   results->flow_count = count_fil;
   for (int i = count_fil - 1; i >= 0; i--) {
-    remove_point = 1;
+    int remove_point = 1;
 
-    if (status[i] && !(new_x[i] < borderx || new_x[i] > (imgWidth - 1 - borderx) ||
-                       new_y[i] < bordery || new_y[i] > (imgHeight - 1 - bordery))) {
+    if (status[i] && !(new_x[i] < borderx || new_x[i] > (w - 1 - borderx) ||
+                       new_y[i] < bordery || new_y[i] > (h - 1 - bordery))) {
       remove_point = 0;
     }
 
     if (remove_point) {
-      for (c = i; c < results->flow_count - 1; c++) {
+      for (int c = i; c < results->flow_count - 1; c++) {
         x[c] = x[c + 1];
         y[c] = y[c + 1];
         new_x[c] = new_x[c + 1];
@@ -229,15 +230,12 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV
   }
 
   // Flow Derotation
-  curr_pitch = info->theta;
-  curr_roll = info->phi;
-
-  results->diff_pitch = (curr_pitch - prev_pitch) * imgHeight / FOV_H;
-  results->diff_roll = (curr_roll - prev_roll) * imgWidth / FOV_W;
-
-  prev_pitch = curr_pitch;
-  prev_roll = curr_roll;
+  results->diff_pitch = (info->theta - visual_estimator.prev_pitch) * h / FOV_H;
+  results->diff_roll = (info->phi - visual_estimator.prev_roll) * w / FOV_W;
+  visual_estimator.prev_pitch = info->theta;
+  visual_estimator.prev_roll = info->phi;
 
+  float OFx_trans, OFy_trans;
 #ifdef FLOW_DEROTATION
   if (results->flow_count) {
     OFx_trans = results->dx_sum - results->diff_roll;
@@ -252,23 +250,21 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV
     OFy_trans = results->dy_sum;
   }
 #else
-  OFx_trans = dx_sum;
-  OFy_trans = dy_sum;
+  OFx_trans = results->dx_sum;
+  OFy_trans = results->dy_sum;
 #endif
 
   // Average Filter
   OFfilter(&results->OFx, &results->OFy, OFx_trans, OFy_trans, results->flow_count, 1);
 
   // Velocity Computation
-  if (info->agl < 0) {
-    results->cam_h = 1;
+  if (info->agl < 0.01) {
+    results->cam_h = 0.01;
   }
   else {
     results->cam_h = info->agl;
   }
 
-  results->FPS = framerate_get();
-
   if (results->flow_count) {
     results->Velx = results->OFy * results->cam_h * results->FPS / Fy_ARdrone + 0.05;
     results->Vely = -results->OFx * results->cam_h * results->FPS / Fx_ARdrone - 0.1;
@@ -281,8 +277,8 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV
   // Next Loop Preparation
   // *************************************************************************************
 
-  memcpy(prev_frame, frame, imgHeight * imgWidth * 2);
-  memcpy(prev_gray_frame, gray_frame, imgHeight * imgWidth);
+  memcpy(visual_estimator.prev_frame, frame, w * h * 2);
+  memcpy(visual_estimator.prev_gray_frame, visual_estimator.gray_frame, w * h);
 
 }