diff --git a/conf/airframes/twinstar1.xml b/conf/airframes/twinstar1.xml
index b5effe6dc9..79490cf2e6 100644
--- a/conf/airframes/twinstar1.xml
+++ b/conf/airframes/twinstar1.xml
@@ -67,21 +67,21 @@
     <define name="NOMINAL_AIRSPEED" value="12." unit="m/s"/>
     <define name="CARROT" value="5." unit="s"/>
   </section>
-	<section name="SIMU">
-		<define name="ROLL_RESPONSE_FACTOR" value="2."/>
-		<define name="YAW_RESPONSE_FACTOR" value="1.35"/>
-		<define name="WEIGHT" value="1.3"/>
-	</section>
-	<section name="FAILSAFE" prefix="FAILSAFE_">
-		<define name="DELAY_WITHOUT_GPS" value="2" unit="s"/>
+  <section name="SIMU">
+    <define name="ROLL_RESPONSE_FACTOR" value="2."/>
+    <define name="YAW_RESPONSE_FACTOR" value="1.35"/>
+    <define name="WEIGHT" value="1.3"/>
+  </section>
+  <section name="FAILSAFE" prefix="FAILSAFE_">
+    <define name="DELAY_WITHOUT_GPS" value="2" unit="s"/>
     <define name="DEFAULT_GAZ" value="CLIMB_LEVEL_GAZ+0.05" unit="%"/>
     <define name="DEFAULT_ROLL" value="0.17" unit="rad"/>
     <define name="DEFAULT_PITCH" value="0.08" unit="rad"/>
-		<define name="HOME_RADIUS" value="50" unit="m"/>
-	</section>
+    <define name="HOME_RADIUS" value="50" unit="m"/>
+  </section>
   <section name="CAM" prefix="CAM_">
-    <define name="THETA0" value="45" unit="deg"/>
-    <define name="PHI0" value="-45" unit="deg"/>
+    <define name="THETA0" value="0" unit="deg"/>
+    <define name="PHI0" value="30" unit="deg"/>
   </section>
   <makefile>
 LOCAL_CFLAGS += -DWAVECARD
diff --git a/conf/airframes/twinstar2.xml b/conf/airframes/twinstar2.xml
index 7a391218e9..52a0749961 100644
--- a/conf/airframes/twinstar2.xml
+++ b/conf/airframes/twinstar2.xml
@@ -80,7 +80,7 @@
 		<define name="HOME_RADIUS" value="50" unit="m"/>
 	</section>
   <section name="CAM" prefix="CAM_">
-    <define name="THETA0" value="45" unit="deg"/>
-    <define name="PHI0" value="45" unit="deg"/>
+    <define name="THETA0" value="15" unit="deg"/>
+    <define name="PHI0" value="-30" unit="deg"/>
   </section>
 </airframe>
diff --git a/conf/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd
index 273c9950b2..073782a940 100644
--- a/conf/flight_plans/flight_plan.dtd
+++ b/conf/flight_plans/flight_plan.dtd
@@ -96,7 +96,8 @@ from_qdr CDATA #IMPLIED
 from_dist CDATA #IMPLIED
 hmode CDATA #IMPLIED
 vmode CDATA #IMPLIED
-cam_mode (null|fix|manual|nadir|target) #IMPLIED
+cam_mode (null|fix|manual|nadir|target|follow) #IMPLIED
+cam_ac_target CDATA #IMPLIED
 target CDATA #IMPLIED
 pitch CDATA #IMPLIED
 alt CDATA #IMPLIED
@@ -112,10 +113,15 @@ until CDATA #IMPLIED>
 
 <!ATTLIST follow
 ac_id CDATA #REQUIRED
-distance CDATA #REQUIRED>
+distance CDATA #REQUIRED
+height CDATA #REQUIRED
+cam_mode (null|fix|manual|nadir|target|follow) #IMPLIED
+cam_ac_target CDATA #IMPLIED
+>
 
 <!ATTLIST xyz
-cam_mode (null|fix|manual|nadir|target) #IMPLIED
+cam_mode (null|fix|manual|nadir|target|follow) #IMPLIED
+cam_ac_target CDATA #IMPLIED
 target CDATA #IMPLIED
 radius CDATA #IMPLIED>
 
@@ -126,8 +132,9 @@ wp_dist CDATA #IMPLIED
 radius CDATA #REQUIRED
 alt CDATA #IMPLIED
 vmode CDATA #IMPLIED
-cam_mode (null|fix|manual|nadir|target) #IMPLIED
+cam_mode (null|fix|manual|nadir|target|follow) #IMPLIED
 target CDATA #IMPLIED
+cam_ac_target CDATA #IMPLIED
 climb CDATA #IMPLIED
 until CDATA #IMPLIED>
 
diff --git a/conf/flight_plans/mav05_ccw.xml b/conf/flight_plans/mav05_ccw.xml
index 6b23d62351..ea43028db7 100644
--- a/conf/flight_plans/mav05_ccw.xml
+++ b/conf/flight_plans/mav05_ccw.xml
@@ -22,7 +22,7 @@
       <deroute BLOCK="zz.takeoff"/>
     </block>
     <block NAME="follow">
-      <follow ac_id="1" distance="20"/>
+      <follow ac_id="1" distance="25" height="25" cam_mode="target" target="zz.TARGET"/>
     </block>
   </blocks>
 </flight_plan>
diff --git a/sw/airborne/autopilot/cam.c b/sw/airborne/autopilot/cam.c
index 1aa744b430..82df47c6d4 100755
--- a/sw/airborne/autopilot/cam.c
+++ b/sw/airborne/autopilot/cam.c
@@ -33,6 +33,7 @@
 #include "flight_plan.h"
 #include "estimator.h"
 #include "link_fbw.h"
+#include "traffic_info.h"
 
 #define MIN_PPRZ_CAM ((int16_t)(MAX_PPRZ * 0.05))
 #define DELTA_ALPHA 0.2
@@ -40,8 +41,6 @@
 #define MAX_CAM_ROLL M_PI/2
 #define MAX_CAM_PITCH M_PI/3
 
-#define target_alt GROUND_ALT
-
 #ifdef CAM_PHI0
 float phi_c = RadOfDeg(CAM_PHI0);
 #else
@@ -54,7 +53,7 @@ float theta_c = RadOfDeg(CAM_THETA0);
 float theta_c;
 #endif
 
-float target_x, target_y;
+float target_x, target_y, target_alt;
 
 void cam_manual( void ) {
   if (pprz_mode == PPRZ_MODE_AUTO2) {
@@ -102,12 +101,22 @@ void cam_manual_target( void ) {
     target_y = Min(target_y, MAX_DIST_TARGET + estimator_y);
     target_y = Max(target_y, -MAX_DIST_TARGET + estimator_y);
   }
+  target_alt = GROUND_ALT;
   cam_target();
 }
 
 void cam_waypoint_target( uint8_t wp ) {
   target_x = waypoints[wp].x;
   target_y = waypoints[wp].y;
+  target_alt = GROUND_ALT;
+  cam_target();
+}
+
+void cam_ac_target( uint8_t ac_id ) {
+  struct ac_info_ * ac = get_ac_info(ac_id);
+  target_x = ac->east;
+  target_y = ac->north;
+  target_alt = ac->alt;
   cam_target();
 }
 
diff --git a/sw/airborne/autopilot/nav.c b/sw/airborne/autopilot/nav.c
index 478839a225..58c2ab28e6 100644
--- a/sw/airborne/autopilot/nav.c
+++ b/sw/airborne/autopilot/nav.c
@@ -166,10 +166,10 @@ static float qdr;
 #define Max(x,y) (x > y ? x : y)
 #define Qdr(x) (Min(x, 350) < qdr && qdr < x+10)
 
-#define Follow(_ac_id, _distance) { \
+#define Follow(_ac_id, _distance, _height) { \
   struct ac_info_ * ac = get_ac_info(_ac_id); \
   vertical_mode = VERTICAL_MODE_AUTO_ALT; \
-  desired_altitude = ac->alt; \
+  desired_altitude = Max(ac->alt + _height, SECURITY_ALT); \
   float alpha = M_PI/2 - ac->course; \
   fly_to_xy(ac->east - _distance*cos(alpha), ac->north - _distance*sin(alpha)); \
 }
diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile
index 718db0bd75..b9cb28b7bc 100644
--- a/sw/simulator/Makefile
+++ b/sw/simulator/Makefile
@@ -92,7 +92,7 @@ $(OBJDIR)/%.o : $(AP)/%.c
 $(OBJDIR)/main.o : $(OBJDIR)/main.c
 	$(OCAMLCC) -c -o $@ -I $(SIMDIR) -I $(FBW) -I $(AP) -I ../include -I $(VARINCLUDE) $<
 
-$(OBJDIR)/sim_gps.o $(OBJDIR)/nav.o $(OBJDIR)/main.o $(OBJDIR)/sim_ir.o $(OBJDIR)/sim_ap.o $(OBJDIR)/pid.o $(OBJDIR)/estimator.o : $(ACINCLUDE)/flight_plan.h  $(ACINCLUDE)/airframe.h
+$(OBJDIR)/sim_gps.o $(OBJDIR)/nav.o $(OBJDIR)/main.o $(OBJDIR)/sim_ir.o $(OBJDIR)/sim_ap.o $(OBJDIR)/pid.o $(OBJDIR)/estimator.o $(OBJDIR)/cam.o : $(ACINCLUDE)/flight_plan.h  $(ACINCLUDE)/airframe.h
 
 $(OBJDIR)/sim_ap.o : $(AP)/traffic_info.h $(AP)/nav.h 
 
diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml
index 91907ea420..f389a33680 100644
--- a/sw/tools/gen_flight_plan.ml
+++ b/sw/tools/gen_flight_plan.ml
@@ -153,6 +153,9 @@ let output_cam_mode = fun x index_of_waypoints ->
 	with _ ->
 	  lprintf "cam_manual_target();\n"
       end
+  | "follow" ->
+      let i = ExtXml.attrib x "cam_ac_target" in
+      lprintf "cam_ac_target(%s);\n" i
   | _ -> failwith (sprintf "Error: unknown '%s' cam mode" m)
 
 let output_vmode x wp last_wp =
@@ -292,9 +295,10 @@ let rec print_stage = fun index_of_waypoints x ->
 	lprintf "return;\n"
     | "follow" ->
 	stage ();
-	let id = ExtXml.attrib x "ac_id" in
-	let d = ExtXml.attrib x "distance" in
-	lprintf "Follow(%s, %s);\n" id d;
+	let id = ExtXml.attrib x "ac_id"
+	and d = ExtXml.attrib x "distance"
+	and h = ExtXml.attrib x "height" in
+	lprintf "Follow(%s, %s, %s);\n" id d h;
 	output_cam_mode x index_of_waypoints;
 	lprintf "return;\n"
     | "attitude" ->