[paparazzi-commits] [4381] change wind handling: ask with the current position ( sending a WORLD_ENV_REQ message)

[Pascal Brisset]

Revision: 4381
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4381
Author:   hecto
Date:     2009-12-22 16:12:17 +0000 (Tue, 22 Dec 2009)
Log Message:
-----------
 change wind handling: ask with the current position (sending a WORLD_ENV_REQ 
message)

Modified Paths:
--------------
    paparazzi3/trunk/sw/simulator/sim.ml

Modified: paparazzi3/trunk/sw/simulator/sim.ml
===================================================================
--- paparazzi3/trunk/sw/simulator/sim.ml        2009-12-22 16:08:53 UTC (rev 
4380)
+++ paparazzi3/trunk/sw/simulator/sim.ml        2009-12-22 16:12:17 UTC (rev 
4381)
@@ -150,7 +150,8 @@
     let _run = ref false in
 
     let wind_x = ref 0.
-    and wind_y = ref 0. in
+    and wind_y = ref 0.
+    and wind_z = ref 0. in
     let infrared_contrast = ref 266.
     and time_scale = object val mutable v = 1. method value = v method 
set_value x = v <- x end
     and gps_availability = ref 1 in
@@ -159,13 +160,30 @@
       gps_availability := Pprz.int_assoc "gps_availability" vs;
       wind_x := Pprz.float_assoc "wind_east" vs;      
       wind_y := Pprz.float_assoc "wind_north" vs;      
+      wind_z := Pprz.float_assoc "wind_up" vs;      
       infrared_contrast := Pprz.float_assoc "ir_contrast" vs;
       time_scale#set_value (Pprz.float_assoc "time_scale" vs)
     in
 
-    ignore (Ground_Pprz.message_bind "WORLD_ENV" world_update);
+    let ask_for_world_env = fun () ->
+      try
+       let (x, y, z) = FlightModel.get_xyz !state in
+       
+       let gps_sol = compute_gps_state (x,y,z) (FlightModel.get_time !state) in
+       
+       let float = fun f -> Pprz.Float f in
+       let values = ["east", float x; "north", float y; "up", float z;
+                     "lat", float ((Rad>>Deg)gps_sol.Gps.wgs84.posn_lat);
+                     "long", float ((Rad>>Deg)gps_sol.Gps.wgs84.posn_long);
+                     "alt", float gps_sol.Gps.alt ] in
+       Ground_Pprz.message_req "sim" "WORLD_ENV" values world_update
+      with
+       exc -> fprintf stderr "Error in sim: %s\n%!" (Printexc.to_string exc)
+    in
 
+    ignore (GMain.Timeout.add 1000 (fun () -> ask_for_world_env (); true));
 
+
     let fm_task = fun () ->
       FM.do_commands !state commands;
       let agl =
@@ -178,7 +196,7 @@
                  _ -> s.Gps.alt
              end
          | None -> 0. in
-      FM.state_update !state FM.nominal_airspeed (!wind_x, !wind_y) agl 
fm_period
+      FM.state_update !state FM.nominal_airspeed (!wind_x, !wind_y, !wind_z) 
agl fm_period
        
     and ir_task = fun () ->
       let phi, theta, _ = FlightModel.get_attitude !state in
@@ -280,7 +298,7 @@
        t#misc#set_sensitive false in
       ignore (t#connect#clicked ~callback);
 
-      (* Monitor an AUTO2 lauch to disable the button *)
+      (* Monitor an AUTO2 launch to disable the button *)
       let monitor = fun () ->
        if FlightModel.get_air_speed !state > 0. then begin
          t#misc#set_sensitive false;