diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c
index c3c2c44cfc..25c7d37742 100644
--- a/sw/airborne/modules/computer_vision/opticflow_module.c
+++ b/sw/airborne/modules/computer_vision/opticflow_module.c
@@ -213,7 +213,10 @@ void opticflow_module_stop(void)
   // Stop the capturing
   v4l2_stop_capture(opticflow_dev);
 
-  // TODO: fix thread stop
+  // Cancel the opticalflow calculation thread
+  if (pthread_cancel(opticflow_calc_thread) != 0) {
+    printf("Thread cancel did not work\n");
+  }
 }
 
 /**