diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml
index 808b556134b..af3f680a58e 100644
--- a/conf/modules/cv_opticflow.xml
+++ b/conf/modules/cv_opticflow.xml
@@ -43,16 +43,18 @@
 
   <init fun="opticflow_module_init()"/>
 
-  <periodic fun="opticflow_module_run()" freq="60" start="opticflow_module_start()" stop="opticflow_module_stop()" autorun="TRUE"/>
+  <periodic fun="opticflow_module_run()" start="opticflow_module_start()" stop="opticflow_module_stop()" autorun="TRUE"/>
   <makefile target="ap">
     <define name="ARDRONE_VIDEO_PORT" value="2002" />
 
     <file name="opticflow_module.c"/>
+    <file name="opticflow_thread.c" dir="modules/computer_vision/opticflow"/>
     <file name="visual_estimator.c" dir="modules/computer_vision/opticflow"/>
     <file name="hover_stabilization.c" dir="modules/computer_vision/opticflow"/>
     <file name="optic_flow_int.c" dir="modules/computer_vision/cv/opticflow"/>
     <file name="fastRosten.c" dir="modules/computer_vision/cv/opticflow/fast9"/>
     <file name="trig.c" dir="modules/computer_vision/cv"/>
+    <file name="framerate.c" dir="modules/computer_vision/cv"/>
     <file name="jpeg.c" dir="modules/computer_vision/cv/encoding"/>
     <file name="rtp.c" dir="modules/computer_vision/cv/encoding"/>
     <file name="socket.c" dir="modules/computer_vision/lib/udp"/>
diff --git a/sw/airborne/modules/computer_vision/cv/framerate.c b/sw/airborne/modules/computer_vision/cv/framerate.c
new file mode 100644
index 00000000000..4fc3b4e282c
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/cv/framerate.c
@@ -0,0 +1,54 @@
+
+#include "std.h"
+#include "framerate.h"
+
+// Frame Rate (FPS)
+#include <sys/time.h>
+
+// local variables
+volatile long timestamp;
+struct timeval start_time;
+struct timeval end_time;
+
+#define USEC_PER_SEC 1000000L
+
+float FPS;
+
+static long time_elapsed(struct timeval *t1, struct timeval *t2)
+{
+  long sec, usec;
+  sec = t2->tv_sec - t1->tv_sec;
+  usec = t2->tv_usec - t1->tv_usec;
+  if (usec < 0) {
+    --sec;
+    usec = usec + USEC_PER_SEC;
+  }
+  return sec * USEC_PER_SEC + usec;
+}
+
+static void start_timer(void)
+{
+  gettimeofday(&start_time, NULL);
+}
+
+static long end_timer(void)
+{
+  gettimeofday(&end_time, NULL);
+  return time_elapsed(&start_time, &end_time);
+}
+
+
+void framerate_init(void) {
+  // Frame Rate Initialization
+  FPS = 0.0;
+  timestamp = 0;
+  start_timer();
+}
+
+void framerate_run(void) {
+  // FPS
+  timestamp = end_timer();
+  FPS = (float) 1000000 / (float)timestamp;
+  //  printf("dt = %d, FPS = %f\n",timestamp, FPS);
+  start_timer();
+}
diff --git a/sw/airborne/modules/computer_vision/cv/framerate.h b/sw/airborne/modules/computer_vision/cv/framerate.h
new file mode 100644
index 00000000000..060811fff6d
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/cv/framerate.h
@@ -0,0 +1,5 @@
+
+extern float FPS;
+
+void framerate_init(void);
+void framerate_run(void);
diff --git a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c
index e3185b8f911..4dfee89e59b 100644
--- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c
+++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c
@@ -29,11 +29,7 @@
 // Own Header
 #include "hover_stabilization.h"
 
-// Vision Data
-#include "visual_estimator.h"
-
 // Stabilization
-//#include "stabilization.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
 #include "firmwares/rotorcraft/guidance/guidance_v.h"
 #include "autopilot.h"
@@ -144,15 +140,12 @@ void init_hover_stabilization_onvision()
   Vely_Int = 0;
 }
 
-void run_hover_stabilization_onvision(void)
+void run_hover_stabilization_onvision(struct CVresults* vision )
 {
-  if (autopilot_mode == AP_MODE_MODULE) {
-    run_opticflow_hover();
+  if (autopilot_mode != AP_MODE_MODULE) {
+    return;
   }
-}
 
-void run_opticflow_hover(void)
-{
   struct FloatVect3 V_body;
   if (activate_opticflow_hover == TRUE) {
     // Compute body velocities from ENU
@@ -161,9 +154,9 @@ void run_opticflow_hover(void)
     float_quat_vmult(&V_body, q_n2b, vel_ned);
   }
 
-  if (flow_count) {
-    Error_Velx = Velx - vision_desired_vx;
-    Error_Vely = Vely - vision_desired_vy;
+  if (vision->flow_count) {
+    Error_Velx = vision->Velx - vision_desired_vx;
+    Error_Vely = vision->Vely - vision_desired_vy;
   } else {
     Error_Velx = 0;
     Error_Vely = 0;
@@ -204,6 +197,6 @@ void run_opticflow_hover(void)
   else if (cmd_euler.theta > CMD_OF_SAT) {cmd_euler.theta = CMD_OF_SAT; saturateY = 1;}
 
   stabilization_attitude_set_rpy_setpoint_i(&cmd_euler);
-  DOWNLINK_SEND_VISION_STABILIZATION(DefaultChannel, DefaultDevice, &Velx, &Vely, &Velx_Int,
+  DOWNLINK_SEND_VISION_STABILIZATION(DefaultChannel, DefaultDevice, &vision->Velx, &vision->Vely, &Velx_Int,
                                      &Vely_Int, &cmd_euler.phi, &cmd_euler.theta);
 }
diff --git a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h
index 3f14b09f15c..285ac708649 100644
--- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h
+++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h
@@ -30,6 +30,7 @@
 #define HOVER_STABILIZATION_H_
 
 #include <std.h>
+#include "inter_thread_data.h"
 
 // Controller module
 
@@ -46,7 +47,7 @@ extern void guidance_h_module_run(bool_t in_flight);
 
 
 void init_hover_stabilization_onvision(void);
-void run_hover_stabilization_onvision(void);
+void run_hover_stabilization_onvision(struct CVresults *vision);
 
 extern bool activate_opticflow_hover;
 extern float vision_desired_vx;
diff --git a/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h
new file mode 100644
index 00000000000..512f75bb46b
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h
@@ -0,0 +1,28 @@
+
+
+#ifndef _INTER_THREAD_DATA_H
+#define _INTER_THREAD_DATA_H
+
+
+// Inter-thread communication: Unix Socket
+
+// Data from thread to module
+struct CVresults {
+  int x;
+  int status;
+  float Velx;
+  float Vely;
+  int flow_count;
+};
+
+// Data from module to thread
+struct PPRZinfo {
+  int cnt;
+  float theta;
+  float phi;
+  float agl;
+};
+
+
+
+#endif
diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c
new file mode 100644
index 00000000000..00d9a0f2759
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c
@@ -0,0 +1,121 @@
+
+
+
+// Sockets
+#include <stdio.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+
+#include "opticflow_thread.h"
+
+
+/////////////////////////////////////////////////////////////////////////
+// COMPUTER VISION THREAD
+
+// Video
+#include "v4l/video.h"
+#include "resize.h"
+
+// Payload Code
+#include "visual_estimator.h"
+
+// Downlink Video
+//#define DOWNLINK_VIDEO 1
+
+#ifdef DOWNLINK_VIDEO
+#include "encoding/jpeg.h"
+#include "encoding/rtp.h"
+#endif
+
+#include <stdio.h>
+
+static volatile enum{RUN,EXIT} computer_vision_thread_command = RUN;  /** request to close: set to 1 */
+
+void computervision_thread_request_exit(void) {
+  computer_vision_thread_command = 0;
+}
+
+void *computervision_thread_main(void *args)
+{
+  int thread_socket = *(int *) args;
+  int cnt = 0;
+
+  computer_vision_thread_command = RUN;
+
+  struct CVresults vision_results;
+  struct PPRZinfo autopilot_data;
+
+  // Video Input
+  struct vid_struct vid;
+  vid.device = (char *)"/dev/video2"; // video1 = front camera; video2 = bottom camera
+  vid.w = 320;
+  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;
+  }
+
+  // Video Grabbing
+  struct img_struct *img_new = video_create_image(&vid);
+
+#ifdef DOWNLINK_VIDEO
+  // Video Compression
+  uint8_t *jpegbuf = (uint8_t *)malloc(vid.h * vid.w * 2);
+
+  // Network Transmit
+  struct UdpSocket *vsock;
+  //#define FMS_UNICAST 0
+  //#define FMS_BROADCAST 1
+  vsock = udp_socket("192.168.1.255", 5000, 5001, FMS_BROADCAST);
+#endif
+
+  // First Apply Settings before init
+  opticflow_plugin_init(vid.w, vid.h, &vision_results);
+
+  while (computer_vision_thread_command > 0) {
+    vision_results.status = 2;
+    video_grab_image(&vid, img_new);
+
+    // Get most recent State
+    int bytes_read = read(thread_socket, &autopilot_data, sizeof(autopilot_data));
+    if (bytes_read <= sizeof(autopilot_data)) {
+      if (bytes_read != 0) {
+        printf("Failed to read PPRZ info from socket.\n");
+      }
+    }
+
+    // Run Image Processing
+    opticflow_plugin_run(img_new->buf, &autopilot_data, &vision_results);
+
+    /* send results to main */
+    vision_results.x = cnt++;
+    int bytes_written = write(thread_socket, &vision_results, sizeof(vision_results));
+    if (bytes_written != sizeof(vision_results)){
+      perror("failed to write to socket.\n");
+    }
+
+
+#ifdef DOWNLINK_VIDEO
+    // JPEG encode the image:
+    uint32_t quality_factor = 10; //20 if no resize,
+    uint8_t dri_header = 0;
+    uint32_t image_format = FOUR_TWO_TWO;  // format (in jpeg.h)
+    uint8_t *end = encode_image(small.buf, jpegbuf, quality_factor, image_format, small.w, small.h, dri_header);
+    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);
+#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
new file mode 100644
index 00000000000..eaa48a1d385
--- /dev/null
+++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h
@@ -0,0 +1,32 @@
+/*
+ * 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
+ * @brief computer vision thread
+ *
+ * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
+ */
+
+
+#include "std.h"
+
+void *computervision_thread_main(void *args);  /* computer vision thread: should be given a pointer to a socketpair as argument */
+void computervision_thread_request_exit(void);
diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c
index 3012b92ef6f..f69d8aa6b87 100644
--- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c
+++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c
@@ -25,7 +25,10 @@
  * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
  */
 
+// Warning: all this code is called form the Vision-Thread: do not access any autopilot data in here.
+
 #include <stdio.h>
+#include <string.h>
 #include <stdlib.h>
 
 // Own Header
@@ -36,17 +39,7 @@
 #include "opticflow/fast9/fastRosten.h"
 
 // for FPS
-#include "modules/computer_vision/opticflow_module.h"
-
-// Paparazzi Data
-#include "state.h"
-#include "subsystems/abi.h"
-
-// Downlink
-#include "subsystems/datalink/downlink.h"
-
-// Timer
-#include <sys/time.h>
+#include "modules/computer_vision/cv/framerate.h"
 
 // Image size set at init
 unsigned int imgWidth, imgHeight;
@@ -71,7 +64,6 @@ int max_count = 25;
 // Corner Tracking
 int *new_x, *new_y, *status, *dx, *dy;
 int error_opticflow;
-int flow_count = 0;
 int remove_point;
 int c;
 int borderx = 24, bordery = 24;
@@ -84,30 +76,10 @@ float distance2, min_distance, min_distance2;
 float curr_pitch, curr_roll, prev_pitch, prev_roll;
 float cam_h, diff_roll, diff_pitch, OFx_trans, OFy_trans;
 
-// Lateral Velocity Computation
-float Velx, Vely;
-
-/** height above ground level, from ABI
- * Used for scale computation, negative value means invalid.
- */
-volatile float estimator_agl;
-
-/** default sonar/agl to use in opticflow visual_estimator */
-#ifndef OPTICFLOW_AGL_ID
-#define OPTICFLOW_AGL_ID ABI_BROADCAST
-#endif
-abi_event agl_ev;
-static void agl_cb(uint8_t sender_id, const float *distance);
 
-static void agl_cb(uint8_t sender_id __attribute__((unused)), const float *distance)
-{
-  if (*distance > 0) {
-    estimator_agl = *distance;
-  }
-}
 
 // Called by plugin
-void opticflow_plugin_init(unsigned int w, unsigned int h)
+void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *results)
 {
   // Initialize variables
   imgWidth = w;
@@ -136,16 +108,17 @@ void opticflow_plugin_init(unsigned int w, unsigned int h)
   curr_roll = 0.0;
   OFx_trans = 0.0;
   OFy_trans = 0.0;
-  Velx = 0.0;
-  Vely = 0.0;
+  results->Velx = 0.0;
+  results->Vely = 0.0;
+  results->flow_count = 0;
 
-  // get AGL from sonar via ABI
-  estimator_agl = -1.0;
-  AbiBindMsgAGL(OPTICFLOW_AGL_ID, &agl_ev, agl_cb);
+  framerate_init();
 }
 
-void opticflow_plugin_run(unsigned char *frame)
+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);
@@ -213,7 +186,7 @@ void opticflow_plugin_run(unsigned char *frame)
   error_opticflow = opticFlowLK(gray_frame, prev_gray_frame, x, y, count_fil, imgWidth,
                                 imgHeight, new_x, new_y, status, 5, 100);
 
-  flow_count = count_fil;
+  results->flow_count = count_fil;
   for (int i = count_fil - 1; i >= 0; i--) {
     remove_point = 1;
 
@@ -223,13 +196,13 @@ void opticflow_plugin_run(unsigned char *frame)
     }
 
     if (remove_point) {
-      for (c = i; c < flow_count - 1; c++) {
+      for (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];
         new_y[c] = new_y[c + 1];
       }
-      flow_count--;
+      results->flow_count--;
     }
   }
 
@@ -237,27 +210,26 @@ void opticflow_plugin_run(unsigned char *frame)
   dy_sum = 0.0;
 
   // Optical Flow Computation
-  for (int i = 0; i < flow_count; i++) {
+  for (int i = 0; i < results->flow_count; i++) {
     dx[i] = new_x[i] - x[i];
     dy[i] = new_y[i] - y[i];
   }
 
   // Median Filter
-  if (flow_count) {
-    quick_sort_int(dx, flow_count); // 11
-    quick_sort_int(dy, flow_count); // 11
+  if (results->flow_count) {
+    quick_sort_int(dx, results->flow_count); // 11
+    quick_sort_int(dy, results->flow_count); // 11
 
-    dx_sum = (float) dx[flow_count / 2];
-    dy_sum = (float) dy[flow_count / 2];
+    dx_sum = (float) dx[results->flow_count / 2];
+    dy_sum = (float) dy[results->flow_count / 2];
   } else {
     dx_sum = 0.0;
     dy_sum = 0.0;
   }
 
   // Flow Derotation
-  // !!WARNING!! Accessing of the state interface is NOT tread safe!!!
-  curr_pitch = stateGetNedToBodyEulers_f()->theta;
-  curr_roll = stateGetNedToBodyEulers_f()->phi;
+  curr_pitch = info->theta;
+  curr_roll = info->phi;
 
   diff_pitch = (curr_pitch - prev_pitch) * imgHeight / FOV_H;
   diff_roll = (curr_roll - prev_roll) * imgWidth / FOV_W;
@@ -266,7 +238,7 @@ void opticflow_plugin_run(unsigned char *frame)
   prev_roll = curr_roll;
 
 #ifdef FLOW_DEROTATION
-  if (flow_count) {
+  if (results->flow_count) {
     OFx_trans = dx_sum - diff_roll;
     OFy_trans = dy_sum - diff_pitch;
 
@@ -284,22 +256,22 @@ void opticflow_plugin_run(unsigned char *frame)
 #endif
 
   // Average Filter
-  OFfilter(&OFx, &OFy, OFx_trans, OFy_trans, flow_count, 1);
+  OFfilter(&OFx, &OFy, OFx_trans, OFy_trans, results->flow_count, 1);
 
   // Velocity Computation
-  if (estimator_agl < 0) {
+  if (info->agl < 0) {
     cam_h = 1;
   }
   else {
-    cam_h = estimator_agl;
+    cam_h = info->agl;
   }
 
-  if (flow_count) {
-    Velx = OFy * cam_h * FPS / Fy_ARdrone + 0.05;
-    Vely = -OFx * cam_h * FPS / Fx_ARdrone - 0.1;
+  if (results->flow_count) {
+    results->Velx = OFy * cam_h * FPS / Fy_ARdrone + 0.05;
+    results->Vely = -OFx * cam_h * FPS / Fx_ARdrone - 0.1;
   } else {
-    Velx = 0.0;
-    Vely = 0.0;
+    results->Velx = 0.0;
+    results->Vely = 0.0;
   }
 
   // *************************************************************************************
diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h
index a5fce5296e4..5fef180b6d7 100644
--- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h
+++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h
@@ -28,16 +28,15 @@
 #ifndef VISUAL_ESTIMATOR_H
 #define VISUAL_ESTIMATOR_H
 
-// Variables used by the controller
-extern float Velx, Vely;
-extern int flow_count;
+
+#include "inter_thread_data.h"
 
 /**
  * Initialize visual estimator.
  * @param w  image width
  * @param h  image height
  */
-void opticflow_plugin_init(unsigned int w, unsigned int h);
-void opticflow_plugin_run(unsigned char *frame);
+void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *results);
+void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CVresults* results);
 
 #endif /* VISUAL_ESTIMATOR_H */
diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c
index 247409ce7df..e47810b308a 100644
--- a/sw/airborne/modules/computer_vision/opticflow_module.c
+++ b/sw/airborne/modules/computer_vision/opticflow_module.c
@@ -28,200 +28,101 @@
 
 #include "opticflow_module.h"
 
+// Computervision Runs in a thread
+#include "opticflow/opticflow_thread.h"
+#include "opticflow/inter_thread_data.h"
+
 // Navigate Based On Vision, needed to call init/run_hover_stabilization_onvision
 #include "opticflow/hover_stabilization.h"
 
 // Threaded computer vision
 #include <pthread.h>
 
-// Frame Rate (FPS)
-#include <sys/time.h>
-
 // Sockets
 #include <stdio.h>
 #include <unistd.h>
 #include <sys/types.h>
 #include <sys/socket.h>
 
-#define USEC_PER_SEC 1000000L
-
-float FPS;
-
+int cv_sockets[2];
 
-struct CVresults {
-  int x;
-};
+// Paparazzi Data
+#include "state.h"
+#include "subsystems/abi.h"
 
+// Downlink
+#include "subsystems/datalink/downlink.h"
 
-int cv_sockets[2];
 
+struct PPRZinfo opticflow_module_data;
 
-// local variables
-volatile long timestamp;
-struct timeval start_time;
-struct timeval end_time;
+/** height above ground level, from ABI
+ * Used for scale computation, negative value means invalid.
+ */
+/** default sonar/agl to use in opticflow visual_estimator */
+#ifndef OPTICFLOW_AGL_ID
+#define OPTICFLOW_AGL_ID ABI_BROADCAST
+#endif
+abi_event agl_ev;
+static void agl_cb(uint8_t sender_id, const float *distance);
 
-static long time_elapsed(struct timeval *t1, struct timeval *t2)
+static void agl_cb(uint8_t sender_id __attribute__((unused)), const float *distance)
 {
-  long sec, usec;
-  sec = t2->tv_sec - t1->tv_sec;
-  usec = t2->tv_usec - t1->tv_usec;
-  if (usec < 0) {
-    --sec;
-    usec = usec + USEC_PER_SEC;
+  if (*distance > 0) {
+    opticflow_module_data.agl = *distance;
   }
-  return sec * USEC_PER_SEC + usec;
-}
-
-static void start_timer(void)
-{
-  gettimeofday(&start_time, NULL);
-}
-
-static long end_timer(void)
-{
-  gettimeofday(&end_time, NULL);
-  return time_elapsed(&start_time, &end_time);
 }
 
 
 void opticflow_module_init(void)
 {
-  // Immediately start the vision thread when the module initialized
-  opticflow_module_start();
+  // get AGL from sonar via ABI
+  AbiBindMsgAGL(OPTICFLOW_AGL_ID, &agl_ev, agl_cb);
+
+  // Initialize local data
+  opticflow_module_data.cnt = 0;
+  opticflow_module_data.phi = 0;
+  opticflow_module_data.theta = 0;
+  opticflow_module_data.agl = 0;
 
   // Stabilization Code Initialization
   init_hover_stabilization_onvision();
-
-  // Frame Rate Initialization
-  FPS = 0.0;
-  timestamp = 0;
-  start_timer();
 }
 
 
-volatile uint8_t computervision_thread_has_results = 0;
-
 void opticflow_module_run(void)
 {
-  // Read Latest Vision Module Results
-  if (computervision_thread_has_results) {
-    struct CVresults res;
-    if (read(cv_sockets[0], &res, sizeof(res))) {
-      perror("Failed to read CV results from socket.\n");
-    }
-    /* TODO: use results */
-    printf("result x=%i", res.x);
-
-    computervision_thread_has_results = 0;
-    run_hover_stabilization_onvision();
-  }
-}
-
-
-/////////////////////////////////////////////////////////////////////////
-// COMPUTER VISION THREAD
-
-// Video
-#include "v4l/video.h"
-#include "resize.h"
-
-// Payload Code
-#include "opticflow/visual_estimator.h"
-
-// Downlink Video
-//#define DOWNLINK_VIDEO 1
-
-#ifdef DOWNLINK_VIDEO
-#include "encoding/jpeg.h"
-#include "encoding/rtp.h"
-#endif
-
-#include <stdio.h>
-
-pthread_t computervision_thread;
-volatile uint8_t computervision_thread_status = 0;
-volatile uint8_t computer_vision_thread_command = 0;
-void *computervision_thread_main(void *args);
-void *computervision_thread_main(void *args)
-{
-  int socket = *(int *) args;
-
-  // Video Input
-  struct vid_struct vid;
-  vid.device = (char *)"/dev/video2"; // video1 = front camera; video2 = bottom camera
-  vid.w = 320;
-  vid.h = 240;
-  vid.n_buffers = 4;
-
-  if (video_init(&vid) < 0) {
-    printf("Error initialising video\n");
-    computervision_thread_status = -1;
-    return 0;
+  // Send Updated data to thread
+  opticflow_module_data.cnt++;
+  opticflow_module_data.phi = stateGetNedToBodyEulers_f()->phi;
+  opticflow_module_data.theta = stateGetNedToBodyEulers_f()->theta;
+  int bytes_written = write(cv_sockets[0], &opticflow_module_data, sizeof(opticflow_module_data));
+  if (bytes_written != sizeof(opticflow_module_data)){
+    perror("module failed to write to socket.\n");
   }
 
-  // Video Grabbing
-  struct img_struct *img_new = video_create_image(&vid);
-
-#ifdef DOWNLINK_VIDEO
-  // Video Compression
-  uint8_t *jpegbuf = (uint8_t *)malloc(vid.h * vid.w * 2);
-
-  // Network Transmit
-  struct UdpSocket *vsock;
-  //#define FMS_UNICAST 0
-  //#define FMS_BROADCAST 1
-  vsock = udp_socket("192.168.1.255", 5000, 5001, FMS_BROADCAST);
-#endif
-
-  // First Apply Settings before init
-  opticflow_plugin_init(vid.w, vid.h);
-
-  while (computer_vision_thread_command > 0) {
-    video_grab_image(&vid, img_new);
-
-    // FPS
-    timestamp = end_timer();
-    FPS = (float) 1000000 / (float)timestamp;
-    //  printf("dt = %d, FPS = %f\n",timestamp, FPS);
-    start_timer();
-
-    // Run Image Processing
-    opticflow_plugin_run(img_new->buf);
-
-    /* send results to main */
-    struct CVresults data;
-    /* TODO: fill in results here */
-    data.x = 42;
-    if (write(socket, &data, sizeof(data))) {
-      printf("failed to write to socket.\n");
+  // Read Latest Vision Module Results
+  struct CVresults vision_results;
+  int bytes_read = read(cv_sockets[0], &vision_results, sizeof(vision_results));
+  if (bytes_read <= sizeof(vision_results)) {
+    if (bytes_read != 0) {
+      printf("Failed to read CV results from socket.\n");
     }
-
-#ifdef DOWNLINK_VIDEO
-    // JPEG encode the image:
-    uint32_t quality_factor = 10; //20 if no resize,
-    uint8_t dri_header = 0;
-    uint32_t image_format = FOUR_TWO_TWO;  // format (in jpeg.h)
-    uint8_t *end = encode_image(small.buf, jpegbuf, quality_factor, image_format, small.w, small.h, dri_header);
-    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);
-#endif
-    computervision_thread_has_results++;
+  } else {
+    ////////////////////////////////////////////
+    // Module-Side Code
+    ////////////////////////////////////////////
+    run_hover_stabilization_onvision(&vision_results);
   }
-  printf("Thread Closed\n");
-  video_close(&vid);
-  computervision_thread_status = -100;
-  return 0;
 }
 
 void opticflow_module_start(void)
 {
-  /* create socket pair for communication */
+  pthread_t computervision_thread;
   if (socketpair(AF_UNIX, SOCK_DGRAM, 0, cv_sockets) == 0) {
-    computer_vision_thread_command = 1;
+    ////////////////////////////////////////////
+    // Thread-Side Code
+    ////////////////////////////////////////////
     int rc = pthread_create(&computervision_thread, NULL, computervision_thread_main,
                             &cv_sockets[1]);
     if (rc) {
@@ -235,5 +136,5 @@ void opticflow_module_start(void)
 
 void opticflow_module_stop(void)
 {
-  computer_vision_thread_command = 0;
+  computervision_thread_request_exit();
 }