diff --git a/sw/airborne/sim/sim_ir.c b/sw/airborne/sim/sim_ir.c
index 2087c0c9f5..752f1d43a6 100644
--- a/sw/airborne/sim/sim_ir.c
+++ b/sw/airborne/sim/sim_ir.c
@@ -14,8 +14,9 @@
 void ir_gain_calib(void) {
 }
 
-value set_ir(value roll, value top) {
+value set_ir(value roll, value front, value top) {
   ir_roll = Int_val(roll);
+  ir_pitch = Int_val(front);
   ir_top = Int_val(top);
   return Val_unit;
 }
diff --git a/sw/simulator/flightModel.ml b/sw/simulator/flightModel.ml
index 3570a44b2c..711e0c4ae0 100644
--- a/sw/simulator/flightModel.ml
+++ b/sw/simulator/flightModel.ml
@@ -42,22 +42,17 @@ type state = {
     mutable z : meter;
     mutable psi : radian; (* Trigonometric *)
     mutable phi : radian;
+    mutable theta : radian;
     mutable phi_dot : radian_s;
     mutable delta_a : float;
+    mutable delta_b : float;
     mutable thrust : float;
     mutable air_speed : meter_s
   }
 
-let init route = {
-  start = Unix.gettimeofday (); t = 0.; x = 0.; y = 0. ; z = 0.;
-  psi = route; phi = 0.; phi_dot = 0.;
-  delta_a = 0.; thrust = 0.; air_speed = 0.
-}
-
 let get_xyz state = (state.x, state.y, state.z)
 let get_time state = state.t
-let get_phi state = state.phi
-let get_attitude state = (state.phi, 0., state.psi)
+let get_attitude state = (state.phi, state.theta, state.psi)
 
 let set_air_speed state x = state.air_speed <- x
 
@@ -104,32 +99,6 @@ module Make(A:Data.MISSION) = struct
 
 
 
-(* Minimum complexity *)
-(*
-   http://controls.ae.gatech.edu/papers/johnson_dasc_01.pdf
-   http://controls.ae.gatech.edu/papers/johnson_mst_01.pdf
- *)
-  let state_update = fun state (wx, wy) dt ->
-    let now = state.t +. dt in
-    if state.air_speed > 0. then begin
-      let phi_dot_dot = roll_response_factor *. state.delta_a +. c_lp *. state.phi_dot /. state.air_speed in
-      state.phi_dot <- state.phi_dot +. phi_dot_dot *. dt;
-      state.phi <- bound (state.phi +. state.phi_dot *. dt) (-.max_phi) max_phi;
-      let psi_dot = -. g /. state.air_speed *. tan (yaw_response_factor *. state.phi) in
-      state.psi <- norm_angle (state.psi +. psi_dot *. dt);
-      let dx = state.air_speed *. cos state.psi *. dt +. wx *. dt
-      and dy = state.air_speed *. sin state.psi *. dt +. wy *. dt in
-      state.x <- state.x +.dx ;
-      state.y <- state.y +. dy;
-      let gamma = (state.thrust -. drag) /. weight in
-      let dz = sin gamma *. state.air_speed *. dt in
-      state.z <- state.z +. dz
-    end;
-    state.t <- now
-
-
-
-
   let commands =
     try
       let l = ExtXml.child A.ac.airframe "commands" in
@@ -153,12 +122,14 @@ module Make(A:Data.MISSION) = struct
 
   let adc_roll_neutral = ios (defined_value infrared_section "ADC_ROLL_NEUTRAL")
   let roll_neutral_default = rad_of_deg (float_value infrared_section "ROLL_NEUTRAL_DEFAULT")
+  let pitch_neutral_default = rad_of_deg (float_value infrared_section "PITCH_NEUTRAL_DEFAULT")
 
   let min_thrust =  0
   let max_thrust =  max_pprz
       
   let command_throttle = command "THROTTLE"
   let command_roll = command "ROLL"
+  let command_pitch = command "PITCH"
      
   let float_attrib = fun x a -> float_of_string (ExtXml.attrib x a)
   let int_attrib = fun x a -> int_of_string (ExtXml.attrib x a)
@@ -167,7 +138,41 @@ module Make(A:Data.MISSION) = struct
   let do_commands = fun state commands ->
     let c_lda = 4e-5 in (* FIXME *)
     state.delta_a <- -. c_lda *. float commands.(command_roll);
+    state.delta_b <- float commands.(command_pitch);
     state.thrust <- (float (commands.(command_throttle) - min_thrust) /. float (max_thrust - min_thrust))
 
   let nb_commands = 10 (* FIXME *)
-end
+
+  let init route = {
+    start = Unix.gettimeofday (); t = 0.; x = 0.; y = 0. ; z = 0.;
+    psi = route; phi = 0.; phi_dot = 0.;
+    delta_a = 0.; delta_b = 0.; thrust = 0.; air_speed = 0.;
+    theta = pitch_neutral_default;
+  }
+
+(* Minimum complexity *)
+(*
+   http://controls.ae.gatech.edu/papers/johnson_dasc_01.pdf
+   http://controls.ae.gatech.edu/papers/johnson_mst_01.pdf
+ *)
+  let state_update = fun state (wx, wy) dt ->
+    let now = state.t +. dt in
+    if state.air_speed > 0. then begin
+      let phi_dot_dot = roll_response_factor *. state.delta_a +. c_lp *. state.phi_dot /. state.air_speed in
+      state.phi_dot <- state.phi_dot +. phi_dot_dot *. dt;
+      state.phi <- bound (state.phi +. state.phi_dot *. dt) (-.max_phi) max_phi;
+      let psi_dot = -. g /. state.air_speed *. tan (yaw_response_factor *. state.phi) in
+      state.psi <- norm_angle (state.psi +. psi_dot *. dt);
+      let dx = state.air_speed *. cos state.psi *. dt +. wx *. dt
+      and dy = state.air_speed *. sin state.psi *. dt +. wy *. dt in
+      state.x <- state.x +.dx ;
+      state.y <- state.y +. dy;
+      let gamma = (state.thrust -. drag) /. weight +. state.theta in
+      let dz = sin gamma *. state.air_speed *. dt in
+      state.z <- state.z +. dz;
+
+      (* Awfull: just to respond to the controller ... *)
+      state.theta <- state.theta +. 0.1 *. (4e-4 *. state.delta_b -. state.theta)
+    end;
+    state.t <- now
+end (* Make functor *)
diff --git a/sw/simulator/flightModel.mli b/sw/simulator/flightModel.mli
index 681b45f52a..3da310ac5e 100644
--- a/sw/simulator/flightModel.mli
+++ b/sw/simulator/flightModel.mli
@@ -30,11 +30,8 @@ type radian = float
 type radian_s = float
 type state
 
-val init : radian -> state
-
 val get_xyz : state -> meter * meter * meter
 val get_time : state -> float
-val get_phi : state -> radian
 val get_attitude : state -> radian * radian * radian
 
 val set_air_speed : state -> meter_s -> unit
@@ -42,10 +39,12 @@ val set_air_speed : state -> meter_s -> unit
 module Make :
   functor (A : Data.MISSION) ->
     sig
+      val init : radian -> state
       val do_commands : state -> Stdlib.pprz_t array -> unit
       val nb_commands : int
       val nominal_airspeed : float (* m/s *)
       val roll_neutral_default : float (* rad *)
+      val pitch_neutral_default : float (* rad *)
       val state_update : state -> float * float -> float -> unit
       (** [state_update state (wind_x, wind_y) dt] With m/s for wind and s for
 	 dt *)
diff --git a/sw/simulator/sim.ml b/sw/simulator/sim.ml
index e3429635f7..73781e3227 100644
--- a/sw/simulator/sim.ml
+++ b/sw/simulator/sim.ml
@@ -109,7 +109,7 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
     
     let compute_gps_state = Gps.state () in
 
-    let initial_state = FlightModel.init (pi/.2. -. qfu/.180.*.pi) in
+    let initial_state = FM.init (pi/.2. -. qfu/.180.*.pi) in
 
     let state = ref initial_state in
 
@@ -150,7 +150,7 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
       FM.state_update !state (!wind_x, !wind_y) fm_period
        
     and ir_task = fun () ->
-      let phi = FlightModel.get_phi !state in
+      let phi, theta, _ = FlightModel.get_attitude !state in
       let horizon_distance = 1000. in
       try
 	match !last_gps_state with
@@ -166,7 +166,7 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
 		0. in
 	    let phi = phi +. FM.roll_neutral_default in
 	    let ir_left = (phi +. delta_ir ) *. !infrared_contrast
-	    and ir_front = 0.
+	    and ir_front = (FM.pitch_neutral_default +. theta) *. !infrared_contrast
 	    and ir_top = pi /. 2. *. !infrared_contrast in
 	    Aircraft.infrared ir_left ir_front ir_top
       with
diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml
index 83a18f7136..549da0a889 100644
--- a/sw/simulator/sitl.ml
+++ b/sw/simulator/sitl.ml
@@ -197,10 +197,10 @@ module Make(A:Data.MISSION) = struct
 (* Functions called by the simulator *)
   let commands = fun s -> rcommands := s
 
-  external set_ir : int -> int -> unit = "set_ir"
+  external set_ir : int -> int -> int -> unit = "set_ir"
   let infrared = fun ir_left ir_front ir_top ->
     (** ADC neutral is not taken into account in the soft sim (c.f. sim_ir.c)*)
-    set_ir (truncate ir_left) (truncate ir_top)
+    set_ir (truncate ir_left) (truncate ir_front) (truncate ir_top)
 
   external use_gps_pos: int -> int -> int -> float -> float -> float -> float -> float -> bool -> unit = "sim_use_gps_pos_bytecode" "sim_use_gps_pos"
   let gps = fun gps ->