*** empty log message ***

sw/simulator/diffusion.ml

@@ -0,0 +1,135 @@
+open Printf
+
+module Ground_Pprz = Pprz.Messages(struct let name = "ground" end)
+module LL = Latlong
+open LL
+
+type plume = { mutable utm_x : float; mutable utm_y : float; mutable value : int }
+
+(* NW of Muret ref *)
+let muret = utm_of WGS84 {LL.posn_lat=(Deg>>Rad)43.4624; posn_long=(Deg>>Rad)1.2727}
+let source = fun () -> { utm_x = muret.LL.utm_x -. 500.; utm_y = muret.LL.utm_y +. 500.; value = 255}
+
+let available_ids = ref []
+let gen_id =
+  let x = ref 0 in
+  fun () -> 
+    match !available_ids with
+      [] ->
+	incr x; !x
+    | x::xs ->
+	available_ids := xs;
+	x
+
+let dt = 1. (* s *)
+let mixing_length = 5. (* m/s *)
+let wind_x = ref 0.
+let wind_y = ref 0.
+
+let ivy_bus = ref "127.255.255.255:2010"
+
+
+let plumes = Hashtbl.create 97
+let t = ref 0 
+
+let one_step = fun () ->
+  incr t;
+
+  (* New plume *)
+  if !t mod 5 = 0 then
+    Hashtbl.add plumes (gen_id ()) (source ());
+  
+  (* Eddy + wind *)
+  Hashtbl.iter (fun id plume ->
+    let a = Random.float (2.*.LL.pi) in
+    plume.utm_x <- plume.utm_x +. (mixing_length*.cos a +. !wind_x)*. dt;
+    plume.utm_y <- plume.utm_y +. (mixing_length*.sin a +. !wind_y)*. dt;
+    plume.value <- plume.value - 1;
+    if plume.value <= 0 then begin
+      Hashtbl.remove plumes id;
+      available_ids := id :: !available_ids;
+    end)
+    plumes 
+
+
+let my_id = "diffusion"
+let send_on_ivy = fun () ->
+  if Hashtbl.length plumes > 0 then
+  let ids = ref []
+  and xs= ref []
+  and ys = ref []
+  and vs = ref [] in
+  Hashtbl.iter (fun id plume ->
+    ids := string_of_int id :: !ids;
+    let wgs84 = LL.of_utm WGS84 {LL.utm_zone = 31; utm_x = plume.utm_x; utm_y = plume.utm_y } in
+    xs := sprintf "%.6f" ((Rad>>Deg)wgs84.posn_lat) :: !xs;
+    ys := sprintf "%.6f" ((Rad>>Deg)wgs84.posn_long) :: !ys;
+    vs := sprintf "%d" plume.value :: !vs)
+    plumes ;
+  let ids = String.concat "," !ids
+  and xs = String.concat "," !xs
+  and ys = String.concat "," !ys
+  and vs = String.concat "," !vs in
+  Ground_Pprz.message_send my_id "PLUMES"
+    [ "ids", Pprz.String ids;
+      "lats", Pprz.String xs;
+      "longs", Pprz.String ys;
+      "values", Pprz.String vs ]
+
+let debug = let i = ref 0 in fun () ->
+  incr i;
+  let f = open_out (sprintf "plumes_%d.txt" !i) in
+  Hashtbl.iter (fun id plume ->
+    fprintf f "%.1f %.1f\n" plume.utm_x plume.utm_y)
+    plumes;
+  close_out f
+
+let detect_distance = 20.
+
+
+
+let flight_param_msg = fun _sender vs ->
+  let lat = Pprz.float_assoc "lat" vs
+  and long = Pprz.float_assoc "long" vs in
+  let utm_ac = utm_of WGS84 {LL.posn_lat=(Deg>>Rad)lat; posn_long=(Deg>>Rad)long} in
+  Hashtbl.iter (fun id plume ->
+    let utm_plume = {LL.utm_zone = 31; utm_x = plume.utm_x; utm_y = plume.utm_y } in
+    let d = utm_distance utm_ac utm_plume in
+    if d < detect_distance then begin
+      let ac_id = Pprz.string_assoc "ac_id" vs in
+      Ground_Pprz.message_send my_id "DL_SETTING"
+	["ac_id", Pprz.String ac_id; "index", Pprz.Int 0(***FIXME***); "value", Pprz.Float (float plume.value)]
+    end)
+    plumes
+
+
+
+let safe_bind = fun msg cb ->
+  let safe_cb = fun sender vs ->
+    try cb sender vs with x -> prerr_endline (Printexc.to_string x) in
+  ignore (Ground_Pprz.message_bind msg safe_cb)
+
+let gaia = fun time_scale _sender vs ->
+  time_scale#set_value (Pprz.float_assoc "time_scale" vs);
+  wind_x := (Pprz.float_assoc "wind_east" vs);
+  wind_y := (Pprz.float_assoc "wind_north" vs)
+
+
+let _ =
+  Ivy.init "Paparazzi gaia" "READY" (fun _ _ -> ());
+  Ivy.start !ivy_bus;
+
+  let periodic = fun () ->
+    one_step ();
+    send_on_ivy ();
+    (*** debug ();***)
+    true in
+
+  let time_scale = object val mutable v = 1. method value = v method set_value x = v <- x end in
+
+  Stdlib.timer ~scale:time_scale dt periodic;
+
+  safe_bind "FLIGHT_PARAM" flight_param_msg;
+  safe_bind "WORLD_ENV" (gaia time_scale);
+
+  Unix.handle_unix_error GMain.Main.main ()