diff --git a/Makefile b/Makefile
index 6a3e0167ca..295c677a3a 100644
--- a/Makefile
+++ b/Makefile
@@ -156,7 +156,7 @@ ac: hard_ac
 #
 # call with : make bl PROC=[TINY|FBW|AP|GENERIC]
 bl:
-	cd $(AIRBORNE)/arm7/test/bootloader; make 
+	cd $(AIRBORNE)/arm7/test/bootloader; make clean; make 
 
 upload_bl: bl
 	lpc21isp -control $(AIRBORNE)/arm7/test/bootloader/bl.hex /dev/ttyS0 38400 12000
diff --git a/conf/airframes/microjet5.xml b/conf/airframes/microjet5.xml
index 74b8b7dc88..ecb2148137 100644
--- a/conf/airframes/microjet5.xml
+++ b/conf/airframes/microjet5.xml
@@ -51,18 +51,18 @@
   </section>
 
   <section name="INFRARED" prefix="IR_">
-    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
-    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
+    <define name="ROLL_NEUTRAL_DEFAULT" value="6" unit="deg"/>
+    <define name="PITCH_NEUTRAL_DEFAULT" value="9" unit="deg"/>
     <define name="DEFAULT_CONTRAST" value="200"/>
-    <define name="RAD_OF_IR_CONTRAST" value="0.75"/>
-    <linear name="RollOfIrs" arity="2" coeff1="-0.5" coeff2="0.5"/>
-    <linear name="PitchOfIrs" arity="2" coeff1="-0.5" coeff2="-0.5"/>
+    <define name="RAD_OF_IR_CONTRAST" value="1."/>
+    <linear name="RollOfIrs" arity="2" coeff1="-0.7" coeff2="0.7"/>
+    <linear name="PitchOfIrs" arity="2" coeff1="-0.7" coeff2="-0.7"/>
     <linear name="TopOfIr" arity="1" coeff1="-1"/>
     <define name="RAD_OF_IR_MAX_VALUE" value="0.0045"/>
     <define name="RAD_OF_IR_MIN_VALUE" value="0.00075"/>
     <define name="ADC_ROLL_NEUTRAL" value="0"/>
-    <define name="ADC_PITCH_NEUTRAL" value="-512"/>
-    <define name="ADC_TOP_NEUTRAL" value="512"/>
+    <define name="ADC_PITCH_NEUTRAL" value="-716"/>
+    <define name="ADC_TOP_NEUTRAL" value="716"/>
   </section>
 
  <section name="GYRO" prefix="GYRO_">
@@ -89,7 +89,7 @@
  
   <section name="PID">
     <define name="ROLL_PGAIN" value="5000."/>
-    <define name="PITCH_OF_ROLL" value="0.0"/>
+    <define name="PITCH_OF_ROLL" value="0.25"/>
     <define name="PITCH_PGAIN" value="8000."/>
     <define name="MAX_ROLL" value="0.35"/>
     <define name="MAX_PITCH" value="0.35"/>
@@ -100,7 +100,7 @@
   <section name="ALT" prefix="CLIMB_">
     <define name="PITCH_PGAIN" value="-0.1"/>
     <define name="PITCH_IGAIN" value="0.025"/>
-    <define name="PGAIN" value="-0.03"/>
+    <define name="PGAIN" value="-0.02"/>
     <define name="IGAIN" value="0.1"/>
     <define name="MAX" value="1."/>
     <define name="LEVEL_GAZ" value="0.50"/>
@@ -109,7 +109,7 @@
   </section>
  
   <section name="NAV">
-    <define name="COURSE_PGAIN" value="-0.2"/>
+    <define name="COURSE_PGAIN" value="-0.5"/>
     <define name="ALTITUDE_PGAIN" value="-0.025"/>
     <define name="NAV_PITCH" value="0."/>
   </section>
diff --git a/conf/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd
index 93b65ab42b..b940209a30 100644
--- a/conf/flight_plans/flight_plan.dtd
+++ b/conf/flight_plans/flight_plan.dtd
@@ -121,7 +121,8 @@ pitch CDATA #IMPLIED
 alt CDATA #IMPLIED
 until CDATA #IMPLIED
 approaching_time CDATA #IMPLIED
-gaz CDATA #IMPLIED>
+gaz CDATA #IMPLIED
+climb CDATA #IMPLIED>
 
 <!ATTLIST set
 var CDATA #REQUIRED
@@ -153,6 +154,8 @@ cam_mode (null|fix|manual|nadir|target|follow) #IMPLIED
 target CDATA #IMPLIED
 cam_ac_target CDATA #IMPLIED
 climb CDATA #IMPLIED
+pitch CDATA #IMPLIED
+gaz CDATA #IMPLIED
 until CDATA #IMPLIED>
 
 <!ATTLIST survey_rectangle
diff --git a/conf/flight_plans/generic.xml b/conf/flight_plans/generic.xml
index 15b26202dc..4f2495947a 100644
--- a/conf/flight_plans/generic.xml
+++ b/conf/flight_plans/generic.xml
@@ -1,4 +1,4 @@
-<flight_plan ALT="450" GROUND_ALT="400" LAT0="43.4624" LON0="1.2727" MAX_DIST_FROM_HOME="1500" NAME="Generic" QFU="270" SECURITY_HEIGHT="25">
+<flight_plan ALT="450" GROUND_ALT="400" LAT0="43.4624" LON0="1.2727" MAX_DIST_FROM_HOME="500" NAME="Generic" QFU="270" SECURITY_HEIGHT="25">
   <dl_settings>
     <dl_settings NAME="flight params">
       <dl_setting MAX="2" MIN="0" STEP="1" VAR="light_mode"/>
@@ -16,20 +16,22 @@
         <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ir_pitch_neutral"/>
         <dl_setting MAX="15000" MIN="1000" STEP="250" VAR="roll_pgain"/>
         <dl_setting MAX="15000" MIN="1000" STEP="250" VAR="pitch_pgain"/>
+        <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="pitch_of_roll"/>
+        <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="pitch_of_vz"/>
       </dl_settings>
+
       <dl_settings name="rate_loop">
-        <dl_setting VAR="roll_rate_pgain" MIN="0." STEP="1.0" MAX="2000."/>
-        <dl_setting VAR="attitude_pgain" MAX="100" STEP="1" MIN="10"/>
+        <dl_setting MAX="2000." MIN="0." STEP="1.0" VAR="roll_rate_pgain"/>
+        <dl_setting MAX="100" MIN="10" STEP="1" VAR="attitude_pgain"/>
       </dl_settings>
 
       <dl_settings name="nav">
-        <dl_setting VAR="climb_level_gaz" MAX="0.7" STEP="0.05" MIN="0.3"/>
-        <dl_setting VAR="climb_pgain" MAX="-0.01" STEP="0.01" MIN="-0.5"/>
-        <dl_setting VAR="course_pgain" MAX="-0.1" STEP="0.05" MIN="-2"/>
+        <dl_setting MAX="0.7" MIN="0.3" STEP="0.05" VAR="climb_level_gaz"/>
+        <dl_setting MAX="-0.01" MIN="-0.1" STEP="0.01" VAR="climb_pgain"/>
+        <dl_setting MAX="-0.1" MIN="-2" STEP="0.05" VAR="course_pgain"/>
       </dl_settings>
     </dl_settings>
   </dl_settings>
-
   <waypoints>
     <waypoint NAME="HOME" X="0" Y="0"/>
     <waypoint NAME="1" X="200" Y="0"/>
@@ -37,13 +39,6 @@
     <waypoint ALT="450." NAME="S1" X="-181.37499716" Y="122.374998203"/>
     <waypoint ALT="450." NAME="S2" X="136.75000196" Y="308.874998301"/>
   </waypoints>
-
-<!--
-  <include name="fs" procedure="failsafe_modes.xml">
-    <arg name="kill_pitch" value="-0.5"/>
-  </include>
--->
-
   <blocks>
     <block NAME="wait GPS">
       <while COND="!GPS_FIX_VALID(gps_mode)"/>
@@ -57,15 +52,40 @@
     <block NAME="circlehome">
       <circle RADIUS="75" WP="HOME"/>
     </block>
+    <block NAME="circlehome pitch">
+      <circle RADIUS="75" WP="HOME" gaz="0.6" pitch="auto"/>
+    </block>
+    <block name="att20">
+      <attitude gaz="0.5" roll="20" until="FALSE" vmode="gaz"/>
+    </block>
+    <block name="att30">
+      <attitude gaz="0.5" roll="30" until="FALSE" vmode="gaz"/>
+    </block>
+    <block name="climb0_att">
+      <attitude climb="0" roll="0" until="block_time > 20" vmode="climb"/>
+      <deroute block="circlehome"/>
+    </block>
+    <block name="climb1_att">
+      <attitude climb="1" roll="0" until="block_time > 20" vmode="climb"/>
+      <deroute block="circlehome"/>
+    </block>
+    <block name="climb0">
+      <go climb="0" vmode="climb" wp="1"/>
+      <deroute block="circlehome"/>
+    </block>
+    <block name="climb1">
+      <go climb="1" vmode="climb" wp="1"/>
+      <deroute block="circlehome"/>
+    </block>
     <block NAME="stack 1">
       <circle RADIUS="75" WP="1"/>
     </block>
     <block NAME="route12">
-      <go approaching_time="1" from="1" hmode="route" wp="2"/>
+      <go approaching_time="0" from="1" hmode="route" wp="2"/>
       <deroute block="stack 2"/>
     </block>
     <block NAME="route21">
-      <go approaching_time="1" from="2" hmode="route" wp="1"/>
+      <go approaching_time="0" from="2" hmode="route" wp="1"/>
       <deroute block="stack 1"/>
     </block>
     <block NAME="stack 2">
diff --git a/sw/airborne/arm7/test/bootloader/Makefile b/sw/airborne/arm7/test/bootloader/Makefile
index 75523fb35d..6ff15d8120 100644
--- a/sw/airborne/arm7/test/bootloader/Makefile
+++ b/sw/airborne/arm7/test/bootloader/Makefile
@@ -1,5 +1,5 @@
 #PROC_AP PROC_FBW PROC_TINY
-PROC = AP
+#PROC = AP
 ALLFLAGS = -DPROC_$(PROC)
 
 LIBNAME	= usbstack
diff --git a/sw/airborne/infrared.c b/sw/airborne/infrared.c
index 6d77b57765..dd03293eeb 100644
--- a/sw/airborne/infrared.c
+++ b/sw/airborne/infrared.c
@@ -205,7 +205,7 @@ void estimator_update_state_infrared( void ) {
   z_constrast_mode = 0;
 #endif
   ir_top = Max(ir_top, 1);
-  float c = rad_of_ir*(1-z_constrast_mode)+z_constrast_mode*(IR_RAD_OF_IR_CONTRAST/ir_top);
+  float c = rad_of_ir*(1-z_constrast_mode)+z_constrast_mode*((float)IR_RAD_OF_IR_CONTRAST/ir_top);
   estimator_phi  = c * ir_roll - ir_roll_neutral;
   estimator_theta = c * ir_pitch - ir_pitch_neutral;
 }
diff --git a/sw/airborne/nav.c b/sw/airborne/nav.c
index 4da1d9ad5d..5a1dfd1dac 100644
--- a/sw/airborne/nav.c
+++ b/sw/airborne/nav.c
@@ -312,6 +312,13 @@ static unit_ reset_waypoints( void ) __attribute__ ((unused));
 
 extern float nav_altitude;
 
+#define CLIMB_MODE_GAZ 0 
+#define CLIMB_MODE_PITCH 1
+#define CLIMB_MODE_AGRESSIVE 2
+#define CLIMB_MODE_BLENDED 3
+
+uint8_t climb_mode = CLIMB_MODE_GAZ;
+
 #include "flight_plan.h"
 
 float ground_alt = GROUND_ALT;
@@ -517,12 +524,6 @@ float desired_course = 0.;
 float max_roll = MAX_ROLL;
 float altitude_error;
 
-#define CLIMB_MODE_GAZ 0 
-#define CLIMB_MODE_PITCH 1
-#define CLIMB_MODE_AGRESSIVE 2
-#define CLIMB_MODE_BLENDED 3
-
-uint8_t climb_mode = CLIMB_MODE_GAZ;
 
 
 /** \brief Computes ::nav_desired_roll from course estimation and expected
diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml
index 62e03c7187..8c4771969c 100644
--- a/sw/tools/gen_flight_plan.ml
+++ b/sw/tools/gen_flight_plan.ml
@@ -151,12 +151,18 @@ let output_cam_mode = fun x index_of_waypoints ->
       lprintf "cam_ac_target(%s);\n" i
   | _ -> failwith (sprintf "Error: unknown '%s' cam mode" m)
 
+let pprz_gaz = fun s ->
+  let g = float_of_string s in
+  if g < 0. || g > 1. then
+    failwith "gaz must be > 0 and < 1";
+  g*. 9600.
+
 let output_vmode x wp last_wp =
   let pitch = try Xml.attrib x "pitch" with _ -> "0.0" in
   if pitch = "auto"
   then begin
     lprintf "climb_mode = CLIMB_MODE_PITCH;\n";
-    lprintf "nav_desired_gaz = %s;\n" (parsed_attrib x "gaz")
+    lprintf "nav_desired_gaz = %f;\n" (pprz_gaz (parsed_attrib x "gaz"))
   end else begin
     lprintf "nav_pitch = %s;\n" (parse pitch);
   end;
@@ -193,7 +199,7 @@ let output_vmode x wp last_wp =
 	if (pitch = "auto") then
 	  failwith "auto pich mode not compatible with vmode=gaz";
 	lprintf "vertical_mode = VERTICAL_MODE_AUTO_GAZ;\n";
-	lprintf "nav_desired_gaz = %s;\n" (parsed_attrib x "gaz")
+	lprintf "nav_desired_gaz = %f;\n" (pprz_gaz (parsed_attrib x "gaz"))
     | x -> failwith (sprintf "Unknown vmode '%s'" x)
   end;
   vmode