diff --git a/sw/simulator/diffusion.ml b/sw/simulator/diffusion.ml new file mode 100644 index 0000000000..9b6095bb0e --- /dev/null +++ b/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 ()