Common time_scale, wind and infrared

conf/messages.xml

@@ -367,6 +367,14 @@
     <field name="if_value1" type="float"></field>
     <field name="if_value2" type="float"></field>
   </message>
+
+  <message name="WORLD_ENV" id="20">
+    <field name="wind_east" type="float" unit="m/s" format="%.1f"/>
+    <field name="wind_north" type="float" unit="m/s" format="%.1f"/>
+    <field name="ir_contrast" type="float"/>
+    <field name="time_scale" type="float"/>
+  </message>
+  <message name="WORLD_ENV_REQ" id="21"/>
 </class>
 
 </protocol>

sw/simulator/Makefile

@@ -54,7 +54,7 @@ SIMDIR=$(shell echo `pwd`)
 
 
 #all : simhitl.out sitl.cma $(GEN_DOWNLINK)
-all : sitl.cma $(GEN_DOWNLINK)
+all : gaia sitl.cma $(GEN_DOWNLINK)
 
 sim_sitl : $(OBJDIR)/simsitl
 
@@ -77,6 +77,12 @@ $(OBJDIR)/simsitl : $(OBJDIR)/$(SIMSA) $(OBJDIR)/simsitl.ml
 	echo 'lablgtk2 -I $$PAPARAZZI_SRC/sw/lib/ocaml -I $(OBJDIR) glibivy-ocaml.cma xml-light.cma lib-pprz.cma $(SIMSA) $$PAPARAZZI_SRC/sw/simulator/sitl.cma -I $$PAPARAZZI_SRC/sw/simulator $(OBJDIR)/simsitl.ml $$*' >> $@
 	chmod a+x $@
 
+gaia : gaia.ml
+	$(OCAMLC) $(INCLUDES) -o $@ glibivy-ocaml.cma xml-light.cma unix.cma lib-pprz.cma lablgtk.cma gtkInit.cmo gaia.ml # To check
+	cat ../../pprz_src_test.sh > $@
+	echo 'lablgtk2 -I $$PAPARAZZI_SRC/sw/lib/ocaml glibivy-ocaml.cma xml-light.cma unix.cma lib-pprz.cma lablgtk.cma gtkInit.cmo $$PAPARAZZI_SRC/sw/simulator/gaia.ml $$*' >> $@
+	chmod a+x $@
+
 $(OBJDIR)/simsitl.opt : $(SIMSO) $(OBJDIR)/simsitl.cmx
 	$(OCAMLOPT) $(INCLUDES) -o $@ str.cmxa glibivy-ocaml.cmxa xml-light.cmxa unix.cmxa lib.cmxa lablgtk.cmxa gtkInit.cmx $(SIMSO) sitl.cmxa  $(OBJDIR)/simsitl.cmx
 

sw/simulator/flightModel.ml

@@ -1,3 +1,29 @@
+(*
+ *  $Id$
+ *
+ * Basic flight model for simulation
+ *  
+ * Copyright (C) 2004 Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA. 
+ *
+ *)
+
 open Stdlib
 
 type meter = float
@@ -84,9 +110,8 @@ let yaw_response_factor = float_value simu_section "YAW_RESPONSE_FACTOR"
 
 let weight = float_value simu_section "WEIGHT"
 
-  let state_update = fun state (wx, wy) ->
-    let now = Unix.gettimeofday () -. state.start in
-    let dt = now -. state.t in
+  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;

sw/simulator/flightModel.mli

@@ -18,5 +18,5 @@ module Make :
       val do_servos : state -> Stdlib.us array -> unit
       val nb_servos : int
       val nominal_airspeed : float (* m/s *)
-      val state_update : state -> float * float -> unit
+      val state_update : state -> float * float -> float -> unit
     end

sw/simulator/gaia.ml

@@ -0,0 +1,101 @@
+(*
+ *  $Id$
+ *
+ * World environment (time, wind, ...) for multi-AC simulation
+ *  
+ * Copyright (C) 2004 Pascal Brisset, Antoine Drouin
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA. 
+ *
+ *)
+
+open Printf
+open Latlong
+
+let my_id = "gaia"
+
+module Ground_Pprz = Pprz.Protocol(struct let name = "ground" end)
+
+let ivy_bus = ref "127.255.255.255:2010"
+
+let parse_args = fun () ->
+  let options =
+    [ "-b", Arg.String (fun x -> ivy_bus := x), (sprintf "Bus\tDefault is %s" !ivy_bus)] in
+  Arg.parse (options)
+    (fun x -> Printf.fprintf stderr "Warning: Don't do anythig with %s\n" x)
+    "Usage: "
+
+let _ =
+  parse_args ();
+  let window = GWindow.window ~title:"Gaia" () in
+  let quit = fun () -> GMain.Main.quit (); exit 0 in
+  ignore (window#connect#destroy ~callback:quit);
+
+  let time_scale = GData.adjustment ~value:1. ~lower:(0.) ~upper:10. ~step_incr:1. () in
+  let wind_dir_adj = GData.adjustment ~value:0. ~lower:(0.) ~upper:370. ~step_incr:1.0 () in
+  let wind_speed_adj = GData.adjustment ~value:0. ~lower:(0.) ~upper:20. ~step_incr:0.1 () in
+  let gust_norm_max_adj = GData.adjustment ~value:0. ~lower:(0.) ~upper:20. ~step_incr:0.1 () in
+  let infrared_contrast_adj = GData.adjustment ~value:500. ~lower:(0.) ~upper:1010. ~step_incr:10. () in
+
+  let world_values = fun () ->
+    let wind_dir_rad = Latlong.pi /. 2. -. (Deg>>Rad) wind_dir_adj#value in
+    let wind_east = wind_speed_adj#value *. cos wind_dir_rad
+    and wind_north = wind_speed_adj#value *. sin wind_dir_rad in
+    [ "wind_east", Pprz.Float wind_east;
+	       "wind_north", Pprz.Float wind_north;
+	       "ir_contrast", Pprz.Float infrared_contrast_adj#value;
+	       "time_scale", Pprz.Float time_scale#value ] in
+  let world_send = fun () ->
+    Ground_Pprz.message_send my_id "WORLD_ENV" (world_values ()) in
+
+  List.iter 
+    (fun (a:GData.adjustment) -> ignore (a#connect#value_changed world_send))
+    [time_scale; wind_dir_adj; wind_speed_adj; gust_norm_max_adj; 
+     infrared_contrast_adj];
+    
+
+  let vbox = GPack.vbox ~packing:window#add () in
+
+  let hbox = GPack.hbox ~packing:vbox#pack () in
+  let _ =  GMisc.label ~text:"time scale:" ~packing:hbox#pack () in
+  let ts = GEdit.spin_button ~adjustment:time_scale ~packing:hbox#add () in
+        
+  let hbox = GPack.hbox ~packing:vbox#pack () in
+  ignore (GMisc.label ~text:"wind dir:" ~packing:hbox#pack ());
+  ignore (GRange.scale `HORIZONTAL ~adjustment:wind_dir_adj ~packing:hbox#add ());
+
+  let hbox = GPack.hbox ~packing:vbox#pack () in
+  ignore (GMisc.label ~text:"wind speed:" ~packing:hbox#pack ());
+  ignore (GRange.scale `HORIZONTAL ~adjustment:wind_speed_adj ~packing:hbox#add ());
+  
+  let hbox = GPack.hbox ~packing:vbox#pack () in
+  ignore (GMisc.label ~text:"gust max speed:" ~packing:hbox#pack ());
+  ignore (GRange.scale `HORIZONTAL ~adjustment:gust_norm_max_adj ~packing:hbox#add ());
+
+
+  let hbox = GPack.hbox ~packing:vbox#pack () in
+  ignore (GMisc.label ~text:"infrared:" ~packing:hbox#pack ());
+  ignore (GRange.scale `HORIZONTAL ~adjustment:infrared_contrast_adj ~packing:hbox#add ());
+
+  Ivy.init "Paparazzi gaia" "READY" (fun _ _ -> ());
+  Ivy.start !ivy_bus;
+
+  ignore (Ground_Pprz.message_answerer my_id "WORLD_ENV" (fun _ _ -> world_values ()));
+
+  window#show ();
+  Unix.handle_unix_error GMain.Main.main ()

sw/simulator/sim.ml

@@ -24,23 +24,24 @@
  *
  *)
 
+open Printf
 open Stdlib
-open Geometry_2d
+
+module Ground_Pprz = Pprz.Protocol(struct let name = "ground" end)
 
 let float_attrib xml a = float_of_string (ExtXml.attrib xml a)
 
-let wind = (0., 0.) (* m/s in local ref *)
-
-(* Frequencies for perdiodic tasks are expressed in periods of 100Hz *)
-let timebase = 10 (* ms *)
-let ir_period = 5
-let fm_period = 4
+(* Frequencies for perdiodic tasks are expressed in s *)
+let ir_period = 1./.20.
+let fm_period = 1./.25.
 
 
 module type AIRCRAFT = 
   sig
     val init : int -> GPack.box -> unit
-    val boot : unit -> unit
+    val boot : Stdlib.value -> unit
+    (** [boot time_acceleration] *)
+
     val servos : us array -> unit
 	(** Called once at init *)
 	
@@ -57,8 +58,9 @@ module type AIRCRAFT_ITL = functor (A : Data.MISSION) -> AIRCRAFT
 
 let ac_name = ref ""
 
+let ivy_bus = ref "127.255.255.255:2010"
 
-let common_options = []
+let common_options = [ "-b", Arg.String (fun x -> ivy_bus := x), "Bus\tDefault is 127.255.255.25:2010"]
 
 module Make(AircraftItl : AIRCRAFT_ITL) = struct
 
@@ -76,29 +78,17 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
   let lon0 = rad_of_deg (float_attrib flight_plan "lon0")
   let qfu = (float_attrib flight_plan "qfu")
   let alt0 = (float_attrib flight_plan "ground_alt")
-(*
-   let gust_dir = 0 and
-   let gust_speed = 0
 
-  let wind = fun ->  
-   let wind_dir = wind_dir_adj#value in
-   let wind_speed = wind_speed_adj#value in
-   let gust_max = gust_max_adj#value in
-   let gust_dir_fact = gust_dir_fact_adj#value in
-   let gust_speed_fact = gust_dir_fact_adj#value in
-   gust_speed = trim 0, gust_max (gust_speed + Random.float gust_dir_fact)
-   gust_dir = gust_dirspeed + Random.float gu)
-
-*)
   let main () =
-    let window = GWindow.dialog ~title:("Aircraft "^ !ac_name) () in
+    let window = GWindow.window ~title:("Aircraft "^ !ac_name) () in
     let quit = fun () -> GMain.Main.quit (); exit 0 in
     ignore (window#connect#destroy ~callback:quit);
+    let vbox = GPack.vbox ~packing:window#add () in
     Srtm.add_path (Env.paparazzi_home ^ "/data/srtm");
     
-    Aircraft.init A.ac.Data.id window#vbox;
+    Aircraft.init A.ac.Data.id vbox;
 
-    let gps_period = 25 in
+    let gps_period = 0.25 in
     
     let compute_gps_state = Gps.state lat0 lon0 (alt0) in
 
@@ -117,79 +107,70 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
     let north_label = GMisc.label ~text:"000" ()
     and east_label = GMisc.label ~text:"000" ()
     and alt_label = GMisc.label ~text:"000" () in
-    let wind_dir_adj = GData.adjustment ~value:0. ~lower:(0.) ~upper:370. ~step_incr:1.0 () in
-    let wind_speed_adj = GData.adjustment ~value:0. ~lower:(0.) ~upper:20. ~step_incr:0.1 () in
-    let gust_norm_max_adj = GData.adjustment ~value:0. ~lower:(0.) ~upper:20. ~step_incr:0.1 () in
-    let gust_norm_ch_fact_adj = GData.adjustment ~value:0. ~lower:(0.) ~upper:20. ~step_incr:0.1 () in
-    let gust_dir_ch_fact_adj = GData.adjustment ~value:0. ~lower:(0.) ~upper:20. ~step_incr:0.1 () in
-    let infrared_contrast_adj = GData.adjustment ~value:500. ~lower:(0.) ~upper:1010. ~step_incr:10. () in
-
-
 
     let half_aperture = (Latlong.pi /. 4.) in
     let last_gps_state = ref None in
     let run = ref false in
     let ir_srtm = ref false in
 
-    let scheduler =
-      let t = ref 0 in
-      let f =
-	fun () ->
-	  incr t;
-	  if !t mod fm_period = 0 then begin
-	    FM.do_servos !state servos;
-	    let wind_dir_rad = deg2rad wind_dir_adj#value in
-	    let wind_angle_rad = heading_of_to_angle_rad wind_dir_rad in
-	    let wind_speed_polar = {r2D = wind_speed_adj#value; theta2D = oposite_heading_rad wind_angle_rad} in
-	    let wind_speed_cart = polar2cart wind_speed_polar in
-	    FM.state_update !state ( wind_speed_cart.x2D, wind_speed_cart.y2D)
-	  end;
+    let wind_x = ref 0.
+    and wind_y = ref 0. in
+    let infrared_contrast = ref 500.
+    and time_scale = object val mutable v = 1. method value = v method set_value x = v <- x end in
 
-	  if !t mod ir_period = 0 then begin
-	    let phi = FlightModel.get_phi !state in
-	    let horizon_distance = 1000. in
-	    try
-	      match !last_gps_state with
-		None -> ()
-	      | Some gps_state ->
-		  let delta_ir = 
-		    if !ir_srtm then
-		      let altitude = (int_of_float gps_state.Gps.alt) in
-		      let horizon_right = Srtm.horizon_slope gps_state.Gps.wgs84 altitude (gps_state.Gps.course +. Latlong.pi /. 2.)  half_aperture horizon_distance in
-		      let horizon_left = Srtm.horizon_slope gps_state.Gps.wgs84 altitude (gps_state.Gps.course -. Latlong.pi /. 2.) half_aperture horizon_distance in 
-		      (***) Printf.printf "IR: %f-%f\n%!" horizon_right horizon_left;
-		      (***) Printf.printf "alt: %d\n%!" altitude;
-		      horizon_right -. horizon_left
-		    else
-		      0. in
-		  let ir_left = ( (phi +. delta_ir ) *. infrared_contrast_adj#value )
-		  and ir_front = 0. in
-		  Aircraft.infrared ir_left ir_front
-	    with
-	      x -> Printf.printf "%s\n%!" (Printexc.to_string x)
-		  
-	  end;
-	  
-	  if !t mod gps_period = 0 then begin
-	    let (x,y,z) = FlightModel.get_xyz !state in
-	    east_label#set_text (Printf.sprintf "%.0f" x);
-	    north_label#set_text (Printf.sprintf "%.0f" y);
-	    alt_label#set_text (Printf.sprintf "%.0f" z);
-	    let s = compute_gps_state (x,y,z) (FlightModel.get_time !state) in
-	    last_gps_state := Some s;
-	    Aircraft.gps s
-	  end;
-	  true in
-      fun () -> ignore (GMain.Timeout.add 10 f) in
+    let world_update = fun _ vs ->
+      wind_x := Pprz.float_assoc "wind_east" vs;      
+      wind_y := Pprz.float_assoc "wind_north" 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 fm_task = fun () ->
+      FM.do_servos !state servos;
+      FM.state_update !state (!wind_x, !wind_y) fm_period
+       
+    and ir_task = fun () ->
+      let phi = FlightModel.get_phi !state in
+      let horizon_distance = 1000. in
+      try
+	match !last_gps_state with
+	  None -> ()
+	| Some gps_state ->
+	    let delta_ir = 
+	      if !ir_srtm then
+		let altitude = (int_of_float gps_state.Gps.alt) in
+		let horizon_right = Srtm.horizon_slope gps_state.Gps.wgs84 altitude (gps_state.Gps.course +. Latlong.pi /. 2.)  half_aperture horizon_distance in
+		let horizon_left = Srtm.horizon_slope gps_state.Gps.wgs84 altitude (gps_state.Gps.course -. Latlong.pi /. 2.) half_aperture horizon_distance in
+		horizon_right -. horizon_left
+	      else
+		0. in
+	    let ir_left = ( (phi +. delta_ir ) *. !infrared_contrast)
+	    and ir_front = 0. in
+	    Aircraft.infrared ir_left ir_front
+      with
+	x -> Printf.printf "%s\n%!" (Printexc.to_string x)
+	    
+    and gps_task = fun () ->
+      let (x,y,z) = FlightModel.get_xyz !state in
+      east_label#set_text (Printf.sprintf "%.0f" x);
+      north_label#set_text (Printf.sprintf "%.0f" y);
+      alt_label#set_text (Printf.sprintf "%.0f" z);
+      let s = compute_gps_state (x,y,z) (FlightModel.get_time !state) in
+      last_gps_state := Some s;
+      Aircraft.gps s in
 
     let boot = fun () ->
-      Aircraft.boot ();
-      scheduler () in
+      Aircraft.boot (time_scale:>value);
+      Stdlib.timer ~scale:time_scale fm_period fm_task;
+      Stdlib.timer ~scale:time_scale ir_period ir_task;
+      Stdlib.timer ~scale:time_scale gps_period gps_task in
     
     let take_off = fun () -> prerr_endline "takeoff"; FlightModel.set_air_speed !state FM.nominal_airspeed in 
 
-    let hbox = GPack.hbox ~packing:window#vbox#pack () in
+    let hbox = GPack.hbox ~packing:vbox#pack () in
     let s = GButton.button ~label:"Boot" ~packing:(hbox#pack ~padding:5) () in
     ignore (s#connect#clicked ~callback:boot);
     let t = GButton.button ~label:"Launch" ~packing:hbox#pack () in
@@ -197,29 +178,14 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
     let ir_srtm_button = GButton.toggle_button ~label:"IR/srtm" ~packing:hbox#pack () in
     ignore (ir_srtm_button#connect#toggled (fun () -> ir_srtm := not !ir_srtm));
 
-    let hbox = GPack.hbox ~packing:window#vbox#pack () in
+    let hbox = GPack.hbox ~packing:vbox#pack () in
     let l = fun s -> ignore(GMisc.label ~text:s ~packing:hbox#pack ()) in
     l "East:"; hbox#pack east_label#coerce;
     l " North:"; hbox#pack north_label#coerce;
     l " Height:"; hbox#pack alt_label#coerce;
 
-    let hbox = GPack.hbox ~packing:window#vbox#pack () in
-    ignore (GMisc.label ~text:"wind dir:" ~packing:hbox#pack ());
-    ignore (GRange.scale `HORIZONTAL ~adjustment:wind_dir_adj ~packing:hbox#add ());
-
-    let hbox = GPack.hbox ~packing:window#vbox#pack () in
-    ignore (GMisc.label ~text:"wind speed:" ~packing:hbox#pack ());
-    ignore (GRange.scale `HORIZONTAL ~adjustment:wind_speed_adj ~packing:hbox#add ());
-
-    let hbox = GPack.hbox ~packing:window#vbox#pack () in
-    ignore (GMisc.label ~text:"gust max speed:" ~packing:hbox#pack ());
-    ignore (GRange.scale `HORIZONTAL ~adjustment:gust_norm_max_adj ~packing:hbox#add ());
-
-
-    let hbox = GPack.hbox ~packing:window#vbox#pack () in
-    ignore (GMisc.label ~text:"infrared:" ~packing:hbox#pack ());
-    ignore (GRange.scale `HORIZONTAL ~adjustment:infrared_contrast_adj ~packing:hbox#add ());
-
+    Ivy.init (sprintf "Paparazzi sim %d" A.ac.Data.id) "READY" (fun _ _ -> ());
+    Ivy.start !ivy_bus;
 
     window#show ();
     Unix.handle_unix_error GMain.Main.main ()

sw/simulator/sim.mli

@@ -7,7 +7,7 @@ val ac_name  : string ref
 module type AIRCRAFT =
   sig
     val init : int -> GPack.box -> unit
-    val boot : unit -> unit
+    val boot : Stdlib.value -> unit
     val servos : Stdlib.us array -> unit
     val infrared : float -> float -> unit
     val gps : Gps.state -> unit

sw/simulator/sitl.ml

@@ -29,14 +29,12 @@ open Printf
 let ios = int_of_string
 let fos = float_of_string
 
-let ivy_bus = ref "127.255.255.255:2010"
-
 module Make(A:Data.MISSION) = struct
 
-  let servos_period = 25 (* ms *)
-  let periodic_period = 16 (* ms *)
-  let rc_period = 25 (* ms *)
-
+  let servos_period = 1./.40. (* s *)
+  let periodic_period = 1./.61. (* s *)
+  let rc_period = 1./.40. (* s *)
+      
   let periodic = fun p f ->
     f ();
     ignore (GMain.Timeout.add p (fun () -> f (); true))
@@ -67,7 +65,7 @@ module Make(A:Data.MISSION) = struct
       let gaz = set_servos !rservos in
       (* 100% = 1W *)
       if bat_button#active then
-	let energy = float (gaz-1000) /. 1000. *. float servos_period /. 1000. in
+	let energy = float (gaz-1000) /. 1000. *. servos_period in
 	accu := !accu +. energy *. 0.00259259259259259252; (* To be improved !!! *)
 	printf "\b\b\b\b\b%.3f%!" !accu;
 	if !accu >= 0.1 then begin
@@ -123,8 +121,6 @@ module Make(A:Data.MISSION) = struct
   let bat_button = GButton.toggle_button ~label:"Bat" ()
 
   let init = fun id vbox ->
-    Ivy.init (sprintf "Paparazzi sim %d" id) "READY" (fun _ _ -> ());
-    Ivy.start !ivy_bus;
     rc ();
     sim_init id;
 
@@ -138,10 +134,10 @@ module Make(A:Data.MISSION) = struct
     update ()
 
 
-  let boot = fun () ->
-    periodic servos_period (update_servos bat_button);
-    periodic periodic_period periodic_task;
-    periodic rc_period rc_task
+  let boot = fun time_scale ->
+    Stdlib.timer ~scale:time_scale servos_period (update_servos bat_button);
+    Stdlib.timer ~scale:time_scale periodic_period periodic_task;
+    Stdlib.timer ~scale:time_scale rc_period rc_task
 
 (* Functions called by the simulator *)
   let servos = fun s -> rservos := s
@@ -158,6 +154,4 @@ module Make(A:Data.MISSION) = struct
     use_gps_pos (cm utm.utm_x) (cm utm.utm_y) utm.utm_zone gps.Gps.course gps.Gps.alt gps.Gps.gspeed gps.Gps.climb gps.Gps.time 
 
 end
-let options =
-  [ "-b", Arg.String (fun x -> ivy_bus := x), "Bus\tDefault is 127.255.255.25:2010"]
-
+let options = []

sw/simulator/stdlib.ml

@@ -40,3 +40,26 @@ let set_float = fun option var name ->
   (option, Arg.Set_float var, Printf.sprintf "%s (%f)" name !var)
 let set_string = fun option var name ->
   (option, Arg.Set_string var, Printf.sprintf "%s (%s)" name !var)
+
+let ms x = max 0 (truncate (1000.*.x))
+(* Non derivating timer *)
+class type value = object method value : float end
+
+let timer ?scale p f =
+  let scale = 
+    match scale with 
+      None -> object method value = 1. end 
+    | Some s -> (s :> value) in
+  let rec loop = fun expected ->
+    let next = expected +. p /. scale#value in
+    let dt = ms (next -. Unix.gettimeofday()) in
+    if dt < 1 then begin (* No timer needed, simply loop *)
+      f (); loop next
+    end else
+      GMain.Timeout.add 
+	dt
+	(fun () -> 
+	  loop next;
+	  f (); 
+	  false) in
+  ignore (loop (Unix.gettimeofday()))

sw/simulator/stdlib.mli

@@ -31,3 +31,8 @@ val deg_of_rad : float -> float
 val rad_of_deg : float -> float
 val set_float : string -> float ref -> string -> string * Arg.spec * string
 val set_string : string -> string ref -> string -> string * Arg.spec * string
+
+class type value = object method value : float end
+
+val timer : ?scale:#value -> float -> (unit -> 'a) -> unit
+(** [timer ?time_accel period_s callback] Non derivating periodic timer *)