diff --git a/conf/abi.xml b/conf/abi.xml
index e27227e45c..0afa505585 100644
--- a/conf/abi.xml
+++ b/conf/abi.xml
@@ -93,6 +93,11 @@
       <field name="thrust_increment" type="float" unit="m/s^2"/>
     </message>
 
+    <message name="OBSTACLE_DETECTION" id="17">
+      <field name="distance_obstacle" type="float" unit="m"/>
+      <field name="relative_heading" type="float" unit="rad"/>
+    </message>
+
   </msg_class>
 
 </protocol>
diff --git a/conf/airframes/examples/ardrone2_gazebo.xml b/conf/airframes/examples/ardrone2_gazebo.xml
index 6db02e1b3b..214f5763a4 100644
--- a/conf/airframes/examples/ardrone2_gazebo.xml
+++ b/conf/airframes/examples/ardrone2_gazebo.xml
@@ -36,6 +36,7 @@
     </module>
     <module name="cv_colorfilter">
       <define name="COLORFILTER_CAMERA" value="front_camera"/>
+      <!--define name="COLORFILTER_SEND_OBSTACLE" value="TRUE"/-->  <!-- If you want to test out the guided flightplan, uncomment this define-->  
     </module>
 
     <module name="video_rtp_stream">
@@ -165,9 +166,9 @@
   </section>
 
   <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
-    <define name="HOVER_KP" value="283"/>
-    <define name="HOVER_KD" value="82"/>
-    <define name="HOVER_KI" value="13"/>
+    <define name="HOVER_KP" value="460"/>
+    <define name="HOVER_KD" value="160"/>
+    <define name="HOVER_KI" value="26"/>
     <define name="NOMINAL_HOVER_THROTTLE" value="0.655"/>
     <define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
   </section>
@@ -177,9 +178,9 @@
     <define name="MAX_BANK" value="20" unit="deg"/>
     <!-- Bad weather -->
     <!-- define name="MAX_BANK" value="32" unit="deg"/ -->
-    <define name="PGAIN" value="79"/>
-    <define name="DGAIN" value="100"/>
-    <define name="IGAIN" value="30"/>
+    <define name="PGAIN" value="160"/>
+    <define name="DGAIN" value="200"/>
+    <define name="IGAIN" value="60"/>
   </section>
 
   <section name="SIMULATOR" prefix="NPS_">
diff --git a/conf/conf_example.xml b/conf/conf_example.xml
index 1cc7083575..95cf79c8e7 100644
--- a/conf/conf_example.xml
+++ b/conf/conf_example.xml
@@ -241,6 +241,17 @@
    settings_modules="modules/video_rtp_stream.xml modules/geo_mag.xml modules/air_data.xml modules/gps_ubx_ucenter.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
    gui_color="red"
   />
+  <aircraft
+   name="ardrone2_flightplan_guided_gazebo"
+   ac_id="35"
+   airframe="airframes/examples/ardrone2_gazebo.xml"
+   radio="radios/dummy.xml"
+   telemetry="telemetry/default_rotorcraft.xml"
+   flight_plan="flight_plans/rotorcraft_guided_flightplan.xml"
+   settings="settings/rotorcraft_basic.xml settings/nps.xml"
+   settings_modules="modules/video_rtp_stream.xml modules/cv_colorfilter.xml modules/geo_mag.xml modules/air_data.xml modules/gps_ubx_ucenter.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
+   gui_color="red"
+  />
   <aircraft
    name="ardrone2_gazebo"
    ac_id="32"
diff --git a/conf/flight_plans/rotorcraft_guided_flightplan.xml b/conf/flight_plans/rotorcraft_guided_flightplan.xml
new file mode 100644
index 0000000000..7614bdb847
--- /dev/null
+++ b/conf/flight_plans/rotorcraft_guided_flightplan.xml
@@ -0,0 +1,107 @@
+<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
+
+<flight_plan alt="1.0" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="20" name="Rotorcraft Optitrack (Delft)" security_height="0.3">
+  <header>
+  	#include "autopilot_guided.h"
+
+		// Useful Combination of the flags fir the autopilot_guided_update
+		#define GUIDED_FLAG_XY_VEL_BODY GUIDED_FLAG_XY_BODY|GUIDED_FLAG_XY_VEL
+		#define GUIDED_FLAG_XY_VEL_BODY_YAW_OFFSET GUIDED_FLAG_XY_BODY|GUIDED_FLAG_XY_VEL|GUIDED_FLAG_YAW_OFFSET
+		#define GUIDED_FLAG_XY_VEL_BODY_YAW_RATE GUIDED_FLAG_XY_BODY|GUIDED_FLAG_XY_VEL|GUIDED_FLAG_YAW_RATE
+		#define GUIDED_FLAG_XY_OFFSET_Z_VEL_YAW_OFFSET GUIDED_FLAG_XY_OFFSET|GUIDED_FLAG_Z_VEL|GUIDED_FLAG_YAW_OFFSET
+		#define GUIDED_FLAG_XY_OFFSET_YAW_OFFSET GUIDED_FLAG_XY_OFFSET|GUIDED_FLAG_YAW_OFFSET
+  </header>
+  <waypoints>
+    <waypoint name="HOME" x="0.0" y="0.0"/>
+    <waypoint name="STDBY" x="0.0" y="0.0"/>
+  </waypoints>
+  <variables>
+	  <variable var="desired_heading" init="0.0f" type="float" min="0." max="10." step="0.1"/>
+	  <variable var="turn_angle" init="90.0f" type="float" min="-180" max="180" step="1"/>
+	  <variable var="turn_rate" init="0.5f" type="float" min="0." max="10." step="0.1"/>
+    <variable var="climb_descent_vel" init="0.3f" type="float" min="0." max="1" step="0.1"/>
+    <variable var="forward_vel" init="0.5f" type="float" min="0." max="2" step="0.1"/>
+	  <variable var="nominal_alt" init="NAV_DEFAULT_ALT" type="float" min="0." max="10." step="0.1"/>
+    <abi_binding name="OBSTACLE_DETECTION" vars="distance_obstacle_FP, _"/>
+	</variables>
+
+<!--Example Guided flight plan:
+
+1. Flies in a small square, just based on guided flight commands (no waypoints)
+2. If it detects an obstacle (example shows only from color detector), it will turn the 90 degrees more quickly
+
+Remember to choose AP_MODE_GUIDED or else it won't work!!!
+-->
+  <blocks>
+    <block name="FPInit">
+      <while cond="!GpsFixValid()"/>
+      <set var = "distance_obstacle_FP" value="10.0"/><!--Or else it will turn everytime if the abi message is not received-->
+      <call_once fun="ins_reset_altitude_ref()"/>
+      <while cond="1"/>
+    </block>
+
+    <block name="Start Engine">
+      <call_once fun="autopilot_set_motors_on(TRUE)"/>
+      <while cond="1"/>
+    </block>
+    
+ <!--Take off:
+ 1. The MAV will first climb with 0.3 m/s upwards (z is down)   
+ 2. If it detects that the MAV has surpased the nominal altitude (given obove in the flightplan)"
+ 3. It will hold the nominal altitude and wait for 1 second before it moves to Go Forward-->
+
+    <block name="Take off">
+      <call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_OFFSET_Z_VEL_YAW_OFFSET,0.,0.,-climb_descent_vel,0.)"/>
+      <while cond="LessThan(stateGetPositionEnu_f()->z,nominal_alt)"/>
+      <call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_OFFSET_YAW_OFFSET,0.,0.,-nominal_alt,0.)"/>    
+      <while cond="LessThan(stage_time,1)"/>
+    </block>
+
+<!-- Go Forward:
+1. The MAV will move forward with a constant speed (0.5 m/s), keeping the sideways speed zero while maintaining it's height on the nominal Altitude
+2. If 5 seconds has passed in the block, It will go to the "Turn 90 deg" block
+An exception is triggered if an obstacle is detected (in this example given by the color detector). Then it will go to the "Turn 90 deg" block immediatly -->
+
+    <block name="Go Forward">
+      <call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_VEL_BODY_YAW_OFFSET,forward_vel,0.,-nominal_alt,0.)"/>
+      <while cond="LessThan(block_time,5)" />
+      <exception cond="2>distance_obstacle_FP" deroute="Turn 90 deg"/> 
+    </block>
+
+<!-- Turn 90 deg:
+1. First the MAV will hover with all velocities on zero for 1 second
+2. An wanted heading is determined from the MAVs current state (90 deg offset is default)
+3. Then the drone turns with an constant rate (default: 0.5 rad/s)
+4. Once the desired heading is reached, the MAV will stop turning and keep this heading
+5. After 1 second, it will go back to the "Go Forward" block  -->
+		
+    <block name="Turn 90 deg">
+      <call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_VEL_BODY_YAW_OFFSET,0.,0.,-nominal_alt,0.)"/>
+      <while cond="LessThan(stage_time,1)"/>
+      <set var="desired_heading" value="stateGetNedToBodyEulers_f()->psi + RadOfDeg(turn_angle)"/>
+      <call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_VEL_BODY_YAW_RATE,0.,0.,-nominal_alt,turn_rate)"/>
+      <while cond="!CloseDegAngles(DegOfRad(stateGetNedToBodyEulers_f()->psi),DegOfRad(desired_heading))"/>
+      <call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_VEL_BODY,0.,0.,-nominal_alt,desired_heading)"/>
+      <while cond="LessThan(stage_time,1)"/>
+      <deroute block="Go Forward"/>
+    </block>
+
+<!-- Land:
+1. Same as take off, but now it descents with an constant speed (default: 0.3 m/s)
+2. If the MAV is close to the ground (0.1 m), the drone will set its desired height to zero
+3. After 3 seconds, it will shut off its motors -->
+
+    <block name="Land">
+      <call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_OFFSET_Z_VEL_YAW_OFFSET,0.,0.,climb_descent_vel,0.)"/>
+      <while cond="MoreThan(stateGetPositionEnu_f()->z,0.1)"/>
+      <call_once fun="autopilot_guided_update(GUIDED_FLAG_XY_OFFSET_YAW_OFFSET,0.,0.,0.,0.)"/>  
+      <while cond="LessThan(block_time,5)"/>
+      <deroute block="Kill Engines"/>
+    </block>
+
+    <block name="Kill Engines">
+      <call_once fun="autopilot_set_motors_on(FALSE)"/>
+      <while cond="1"/>
+    </block>
+  </blocks>
+</flight_plan>
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_guided.h b/sw/airborne/firmwares/rotorcraft/autopilot_guided.h
index 6f7b5bd13c..0abf307648 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot_guided.h
+++ b/sw/airborne/firmwares/rotorcraft/autopilot_guided.h
@@ -96,9 +96,8 @@ extern void autopilot_guided_update(uint8_t flags, float x, float y, float z, fl
 #define GUIDED_FLAG_Z_OFFSET    (1<<2)
 #define GUIDED_FLAG_YAW_OFFSET  (1<<3)
 #define GUIDED_FLAG_XY_VEL      (1<<5)
-#define GUIDED_FLAG_2_VEL       (1<<6)
+#define GUIDED_FLAG_Z_VEL       (1<<6)
 #define GUIDED_FLAG_YAW_RATE    (1<<7)
 
-
 #endif /* AUTOPILOT_GUIDED_H */
 
diff --git a/sw/airborne/modules/computer_vision/colorfilter.c b/sw/airborne/modules/computer_vision/colorfilter.c
index 43bc02738b..fc496da74d 100644
--- a/sw/airborne/modules/computer_vision/colorfilter.c
+++ b/sw/airborne/modules/computer_vision/colorfilter.c
@@ -34,6 +34,12 @@
 #endif
 PRINT_CONFIG_VAR(COLORFILTER_FPS)
 
+
+#ifndef COLORFILTER_SEND_OBSTACLE
+#define COLORFILTER_SEND_OBSTACLE FALSE    ///< Default sonar/agl to use in opticflow visual_estimator
+#endif
+PRINT_CONFIG_VAR(COLORFILTER_SEND_OBSTACLE)
+
 struct video_listener *listener = NULL;
 
 // Filter Settings
@@ -47,6 +53,10 @@ uint8_t color_cr_max  = 255;
 // Result
 int color_count = 0;
 
+#include "subsystems/abi.h"
+
+
+
 // Function
 struct image_t *colorfilter_func(struct image_t *img);
 struct image_t *colorfilter_func(struct image_t *img)
@@ -58,6 +68,17 @@ struct image_t *colorfilter_func(struct image_t *img)
                                        color_cr_min, color_cr_max
                                       );
 
+
+  if (COLORFILTER_SEND_OBSTACLE) {
+    if (color_count > 20)
+      AbiSendMsgOBSTACLE_DETECTION(CV_COLORDETECTION, 1,
+                                   0);
+    else
+      AbiSendMsgOBSTACLE_DETECTION(CV_COLORDETECTION, 10,
+                                   0);
+
+  }
+
   return img; // Colorfilter did not make a new image
 }
 
diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h
index 3954a90435..8dc186fb2c 100644
--- a/sw/airborne/subsystems/abi_sender_ids.h
+++ b/sw/airborne/subsystems/abi_sender_ids.h
@@ -302,4 +302,14 @@
 #define MAG_CALIB_UKF_ID 20
 #endif
 
+
+/*
+ * IDs of Computer Vision Calculated variables
+ */
+
+#ifndef CV_COLORDETECTION
+#define CV_COLORDETECTION 1
+#endif
+
+
 #endif /* ABI_SENDER_IDS_H */
diff --git a/sw/tools/generators/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml
index d4064d188c..6143bf6cfb 100644
--- a/sw/tools/generators/gen_flight_plan.ml
+++ b/sw/tools/generators/gen_flight_plan.ml
@@ -1014,6 +1014,7 @@ let () =
       check_altitude (float_of_string alt) xml;
       check_altitude_srtm (float_of_string alt) xml !fp_wgs84;
 
+      Xml2h.define "NAV_DEFAULT_ALT" (sprintf "%.0f /* nominal altitude of the flight plan */" (float_of_string alt));
       Xml2h.define "NAV_UTM_EAST0" (sprintf "%.0f" utm0.utm_x);
       Xml2h.define "NAV_UTM_NORTH0" (sprintf "%.0f" utm0.utm_y);
       Xml2h.define "NAV_UTM_ZONE0" (sprintf "%d" utm0.utm_zone);