diff --git a/sw/simulator/fg.c b/sw/simulator/fg.c
index 90f8df4233..838a509cc5 100644
--- a/sw/simulator/fg.c
+++ b/sw/simulator/fg.c
@@ -14,23 +14,26 @@ value fg_sizeof(value unit) {
   return Val_int(sizeof(struct FGNetGUI));
 }
 
-value fg_msg(value s, value x, value y, value z, value phi) {
-#if 0
-  const double earth_radius = 6372795.; 
-  double lat =  0.656480 + asin(Double_val(x)/earth_radius);
-  double lon = -2.135537 + asin(Double_val(y)/earth_radius);
-  
+value fg_msg_native(value s, value lat, value lon, value z, value phi, value theta, value psi);
+
+value fg_msg_bytecode(value *argv, int argc) {
+  return fg_msg_native(argv[0], argv[1], argv[2], argv[3], argv[4], argv[5], argv[6]);
+
+}
+
+
+value fg_msg_native(value s, value lat, value lon, value z, value phi, value theta, value psi) {
 
   struct FGNetGUI msg;
   msg.version = FG_NET_GUI_VERSION;
 
-  msg.longitude = lon;
-  msg.latitude = lat;
-  msg.altitude = Double_val(z);
+  msg.longitude = Double_val(lon);
+  msg.latitude = Double_val(lat);
+  msg.altitude = Double_val(z) + 20;
   msg.agl = 0.;
   msg.phi = Double_val(phi);
-  msg.theta = 0.;
-  msg.psi = 0.;
+  msg.theta = Double_val(theta);
+  msg.psi = - Double_val(psi) + M_PI_2;
   
   msg.vcas = 0.;
   msg.climb_rate = 0.;
@@ -38,7 +41,7 @@ value fg_msg(value s, value x, value y, value z, value phi) {
   msg.num_tanks = 1;
   msg.fuel_quantity[0] = 10.;
   
-  msg.cur_time = time(NULL);
+  msg.cur_time = 3213092700ul;//time(NULL);
   msg.warp = 0;
   msg.ground_elev = 0.;
 
@@ -47,66 +50,7 @@ value fg_msg(value s, value x, value y, value z, value phi) {
   msg.in_range = 1;
   msg.course_deviation_deg = 12.;
   msg.gs_deviation_deg = 123.;
-#endif
 
-  static double _x = 0.;
-  static double _y = 0.;
-  static double _z = 10;
-
-  const double v = 150.;
-  const double psi = 292.80 * M_PI / 180.;
-  
-  const double vx = v * cos(psi);
-  const double vy = v * sin(psi);
-  const double vz = 1.;
-
-  const double dt = 0.01666666;
-  
-  _x += vx * dt;
-  _y += vy * dt;
-  _z += vz * dt;
-
-  const double earth_radius = 6372795.;
-
-  double lat =  0.656480 + asin(_x/earth_radius);
-  double lon = -2.135537 + asin(_y/earth_radius);
-
-  struct FGNetGUI msg;
-  //  net_gui_init(&gui);
-  msg.version = FG_NET_GUI_VERSION; 
-  //  gui.latitude = 0.656480;
-  //  gui.longitude = -2.135537;
-  //  gui.altitude = 0.807609;
-  msg.agl = 1.111652;
-  
-  msg.phi = 0.;
-  msg.theta = 0.;
-  msg.psi = 5.20;
-  
-  msg.vcas = 0.;
-  msg.climb_rate = 0.;
-
-  msg.num_tanks = 1;
-  msg.fuel_quantity[0] = 0.;
-
-  msg.cur_time = 3198060679ul;
-  msg.warp = 1122474394ul;
-
-  msg.ground_elev = 0.;
-
-  msg.tuned_freq = 125.65;
-  msg.nav_radial = 90.;
-  msg.in_range = 1;
-  msg.dist_nm = 10.;
-  msg.course_deviation_deg = 0.;
-  msg.gs_deviation_deg = 0.;
-
-
-  msg.latitude = lat;
-  msg.longitude = lon;
-  msg.altitude = _z;
-
-  s = alloc_string(sizeof(msg));
   memcpy(String_val(s), (char*)&msg, sizeof(msg));
 
   return Val_unit;
diff --git a/sw/simulator/flightModel.ml b/sw/simulator/flightModel.ml
index 1d3eda25ed..ad3daf594a 100644
--- a/sw/simulator/flightModel.ml
+++ b/sw/simulator/flightModel.ml
@@ -56,6 +56,7 @@ let init route = {
 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 set_air_speed state x = state.air_speed <- x
 
diff --git a/sw/simulator/flightModel.mli b/sw/simulator/flightModel.mli
index 70dd9b3d80..6d97b2bd05 100644
--- a/sw/simulator/flightModel.mli
+++ b/sw/simulator/flightModel.mli
@@ -35,6 +35,7 @@ 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
 
diff --git a/sw/simulator/sim.ml b/sw/simulator/sim.ml
index 43be8901ad..bfe27638b4 100644
--- a/sw/simulator/sim.ml
+++ b/sw/simulator/sim.ml
@@ -34,7 +34,7 @@ let float_attrib xml a = float_of_string (ExtXml.attrib xml a)
 (* Frequencies for perdiodic tasks are expressed in s *)
 let ir_period = 1./.20.
 let fm_period = 1./.25.
-let fg_period = 1./.50.
+let fg_period = 1./.25.
 
 
 module type AIRCRAFT = 
@@ -57,7 +57,7 @@ module type AIRCRAFT =
 module type AIRCRAFT_ITL = functor (A : Data.MISSION) -> AIRCRAFT
 
 external fg_sizeof : unit -> int = "fg_sizeof"
-external fg_msg : string -> float -> float -> float -> float -> unit = "fg_msg"
+external fg_msg : string -> float -> float -> float -> float -> float -> float -> unit = "fg_msg_bytecode" "fg_msg_native"
 
 
 let ac_name = ref ""
@@ -178,10 +178,16 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
 
     (** Sending to Flight Gear *)
     let fg_task = fun socket buffer () ->
-      let (x,y,z) = FlightModel.get_xyz !state
-      and phi = FlightModel.get_phi !state in
-      fg_msg buffer x y z phi;
-(**)       for i = 0 to String.length buffer - 1 do fprintf stderr "%x " (Char.code buffer.[i]) done; fprintf stderr "\n"; (**)
+      match !last_gps_state with
+	None -> ()
+      | Some s ->
+	  let lat = s.Gps.wgs84.Latlong.posn_lat
+	  and lon = s.Gps.wgs84.Latlong.posn_long
+	  and alt = s.Gps.alt
+(*	  and theta_ = s.Gps.course *)
+	  and (phi, theta, psi) = FlightModel.get_attitude !state in
+	  fg_msg buffer lat lon alt phi theta psi;
+(**       for i = 0 to String.length buffer - 1 do fprintf stderr "%x " (Char.code buffer.[i]) done; fprintf stderr "\n"; **)
       try
 	ignore (Unix.send socket buffer 0 (String.length buffer) [])
       with
@@ -199,7 +205,7 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
 	try
 	  let inet_addr = Unix.inet_addr_of_string !fg_client in
 	  let socket = Unix.socket Unix.PF_INET Unix.SOCK_DGRAM 0 in
-	  Unix.connect socket (Unix.ADDR_INET (inet_addr, 5500));
+	  Unix.connect socket (Unix.ADDR_INET (inet_addr, 5501));
 	  let buffer = String.create (fg_sizeof ()) in
 	  Stdlib.timer ~scale:time_scale fg_period (fg_task socket buffer)
 	with
diff --git a/sw/simulator/simsitl.pl b/sw/simulator/simsitl.pl
index da38124089..09c6bd917a 100755
--- a/sw/simulator/simsitl.pl
+++ b/sw/simulator/simsitl.pl
@@ -15,9 +15,11 @@ my $options = {};
 GetOptions (
 	    "b=s" => \$options->{ivy_bus},
 	    "a=s" => \$options->{aircraft},
+	    "fg=s" => \$options->{fg},
 	   );
 my @args = ();
 push @args, "-b", $options->{ivy_bus};
+push @args, "-fg", $options->{fg} if exists $options->{fg};
 my $sim_binary = Paparazzi::Environment::paparazzi_home()."/var/".$options->{aircraft}."/sim/simsitl";
 die "$sim_binary not found. try make AIRCRAFT=$options->{aircraft} ac\n" unless -e $sim_binary;
 exec ($sim_binary, @args)