diff --git a/conf/flight_plans/Tudelft/rotorcraft_survey_competition.xml b/conf/flight_plans/Tudelft/rotorcraft_survey_competition.xml
index 0beeac36f0..4157ff9b18 100644
--- a/conf/flight_plans/Tudelft/rotorcraft_survey_competition.xml
+++ b/conf/flight_plans/Tudelft/rotorcraft_survey_competition.xml
@@ -16,7 +16,7 @@
     <waypoint lat="50.788775526101" lon="6.04652336854142" name="RZ1" height="15"/>
     <waypoint lat="50.7889170568219" lon="6.04640018582595" name="RZ2" height="15"/>
     <waypoint lat="50.7889755961272" lon="6.04642327267965" name="RZ3" height="15"/>
-    <waypoint lat="50.789522" lon="6.047118" name="FC"/>
+    <waypoint lat="50.789522" lon="6.047118" name="FC" height="20"/>
     <waypoint lat="50.788597" lon="6.046721" name="S1"/>
     <waypoint lat="50.788973" lon="6.045944" name="S2"/>
     <waypoint lat="50.789323" lon="6.046330" name="S3"/>
diff --git a/conf/flight_plans/Tudelft/rotorcraft_survey_mission.xml b/conf/flight_plans/Tudelft/rotorcraft_survey_mission.xml
index 3f8c0bbf60..5e72edd46c 100644
--- a/conf/flight_plans/Tudelft/rotorcraft_survey_mission.xml
+++ b/conf/flight_plans/Tudelft/rotorcraft_survey_mission.xml
@@ -12,6 +12,13 @@
 #ifndef DropOpen
 #define DropOpen() FALSE
 #endif
+#ifndef StartVision
+#define StartVision() FALSE
+#endif
+#ifndef StopVision
+#define StopVision() FALSE
+#endif
+#define WpAlt(X) (30)
   </header>
 
   <exceptions>
@@ -65,11 +72,28 @@
     <block name="Standby" strip_button="Standby" strip_icon="home.png" group="hover">
       <stay wp="STDBY"/>
     </block>
-    <block name="LookRobot" strip_button="LookRobot-FC" strip_icon="lookdown.png" group="hover">
+
+    <block name="GoTennis" strip_button="LookRobot-FC" strip_icon="lookdown.png" group="hover">
       <call fun="NavCopyWaypointPositionOnly(WP_CRUISE,WP_FC)"/>
       <go wp="CRUISE"/>
+    </block>
+    <block name="LookRobot" >
+      <stay climb="nav_descend_vspeed" vmode="climb" wp="FC" until="LessThan(stateGetPositionEnu_f()->z, 20)"/>
+      <call fun="NavCopyWaypointPositionOnly(WP_CAM,WP_FC)"/>
+    </block>
+    <block name="Vision" >
+      <exception cond="block_time > 60" deroute="ComeBack"/>
+      <call fun="StartVision()"/>
       <stay wp="FC"/>
     </block>
+    <block name="ComeBack" strip_button="Abort" strip_icon="home.png" group="hover">
+      <call fun="StopVision()"/>
+      <call fun="NavCopyWaypointPositionOnly(WP_CRUISE,WP_FC)"/>
+      <stay climb="nav_climb_vspeed" vmode="climb" wp="CRUISE" until="stateGetPositionEnu_f()->z > 30"/>
+      <call fun="NavCopyWaypointPositionOnly(WP_CRUISE,WP_TD)"/>
+      <go wp="CRUISE"/>
+      <deroute block="land"/>
+    </block>
 
     <block name="CAM" strip_button="Follow-Vision-Geolocation-Results" strip_icon="lookdown.png" group="hover">
       <stay wp="CAM"/>
@@ -163,7 +187,7 @@
       <deroute block="Survey Poly RUN"/>
     </block>
     <block name="Survey Poly RUN">
-      <exception cond="PolySurveySweepNum >= 5" deroute="Standby"/>
+      <exception cond="PolySurveySweepNum >= 5" deroute="GoTennis"/>
       <call fun="nav_survey_poly_run()"/>
     </block>
 
diff --git a/conf/modules/cv_blob_locator.xml b/conf/modules/cv_blob_locator.xml
index 57aa9147e6..52af1809fc 100644
--- a/conf/modules/cv_blob_locator.xml
+++ b/conf/modules/cv_blob_locator.xml
@@ -15,6 +15,7 @@
         <dl_setting var="color_cr_max"  min="0" step="1" max="255" shortname="v_max" />
         <dl_setting var="marker_size"  min="1" step="1" max="20" shortname="marker" />
         <dl_setting var="geofilter_length"  min="1" step="1" max="100" shortname="filter" />
+        <dl_setting var="record_video"  min="0" step="1" max="1" shortname="record"  values="OFF|ON" />
         <dl_setting var="cv_blob_locator_reset" max="1" min="0" step="1" module="computer_vision/cv_blob_locator"  handler="GeoReset">
           <strip_button name="Reset Geoprojection Filter" value="1" icon="resurrect.png" value="2" group="cv"/>
         </dl_setting>
diff --git a/sw/airborne/modules/computer_vision/cv_blob_locator.c b/sw/airborne/modules/computer_vision/cv_blob_locator.c
index 0d6f80c4a0..369c91fc3e 100644
--- a/sw/airborne/modules/computer_vision/cv_blob_locator.c
+++ b/sw/airborne/modules/computer_vision/cv_blob_locator.c
@@ -44,6 +44,7 @@ uint8_t cv_blob_locator_type;
 
 int geofilter_length = 5;
 int marker_size = 18;
+int record_video = 0;
 
 volatile uint32_t blob_locator = 0;
 
@@ -243,7 +244,9 @@ void cv_blob_locator_init(void) {
 }
 
 void cv_blob_locator_periodic(void) {
-
+  if (record_video == 1) {
+    video_thread_take_shot(TRUE);
+  }
 }
 
 
@@ -303,3 +306,14 @@ extern void cv_blob_locator_start(void) {
 extern void cv_blob_locator_stop(void) {
 
 }
+
+void start_vision(void) {
+  georeference_init();
+  record_video = 1;
+  cv_blob_locator_type = 3;
+}
+void stop_vision(void) {
+  georeference_init();
+  record_video = 0;
+  cv_blob_locator_type = 0;
+}
diff --git a/sw/airborne/modules/computer_vision/cv_blob_locator.h b/sw/airborne/modules/computer_vision/cv_blob_locator.h
index 798240c68a..ba49e2c8e3 100644
--- a/sw/airborne/modules/computer_vision/cv_blob_locator.h
+++ b/sw/airborne/modules/computer_vision/cv_blob_locator.h
@@ -42,6 +42,7 @@ extern uint8_t cv_blob_locator_type;
 
 extern int marker_size;
 extern int geofilter_length;
+extern int record_video;
 
 extern void cv_blob_locator_init(void);
 extern void cv_blob_locator_periodic(void);
@@ -53,6 +54,13 @@ extern void cv_blob_locator_stop(void);
     cv_blob_locator_start();                 \
   }
 
+#define StartVision(X) { start_vision(); FALSE; }
+#define StopVision(X) { stop_vision(); FALSE; }
+
+
+extern void start_vision(void);
+extern void stop_vision(void);
+
 
 #endif