diff --git a/conf/messages.xml b/conf/messages.xml index a480be3d9b..f6e7724a0c 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -275,10 +275,12 @@ - + + + diff --git a/sw/ground_segment/cockpit/Makefile b/sw/ground_segment/cockpit/Makefile index 1312ecc3ca..a5a85fdbab 100644 --- a/sw/ground_segment/cockpit/Makefile +++ b/sw/ground_segment/cockpit/Makefile @@ -11,7 +11,7 @@ all : map2d map2d : map2d.ml $(OCAMLC) -custom $(INCLUDES) $(LIBS) gtkInit.cmo $< -o $@ #to Check cat ../../../pprz_src_test.sh > $@ - echo 'exec lablgtk2 -I +camlimages ci_core.cma ci_png.cma ci_gif.cma ci_jpeg.cma ci_tiff.cma ci_bmp.cma ci_ppm.cma ci_ps.cma -I $$PAPARAZZI_SRC/sw/lib/ocaml glibivy-ocaml.cma lib-pprz.cma xlib-pprz.cma $$PAPARAZZI_SRC/sw/ground_segment/cockpit/$< $$*' >> $@ + echo 'exec lablgtk2 lablgtk.cma -I +camlimages ci_core.cma ci_png.cma ci_gif.cma ci_jpeg.cma ci_tiff.cma ci_bmp.cma ci_ppm.cma ci_ps.cma -I $$PAPARAZZI_SRC/sw/lib/ocaml glibivy-ocaml.cma lib-pprz.cma xlib-pprz.cma $$PAPARAZZI_SRC/sw/ground_segment/cockpit/$< $$*' >> $@ chmod a+x $@ diff --git a/sw/ground_segment/tmtc/Makefile b/sw/ground_segment/tmtc/Makefile index 494020a325..354e49b5bf 100644 --- a/sw/ground_segment/tmtc/Makefile +++ b/sw/ground_segment/tmtc/Makefile @@ -64,10 +64,10 @@ stereo_demod : ../multimon/multimon.cma stereo_demod.ml ../../lib/ocaml/lib-pprz @echo 'exec lablgtk2 -I $$PAPARAZZI_SRC/sw/lib/ocaml -I $$PAPARAZZI_SRC/sw/ground_segment/multimon glibivy-ocaml.cma lib-pprz.cma -I $$PAPARAZZI_SRC/sw/ground_segment/tmtc $$PAPARAZZI_SRC/sw/ground_segment/multimon/multimon.cma $$PAPARAZZI_SRC/sw/ground_segment/tmtc/stereo_demod.ml $$*' >> $@ @chmod a+x $@ -receive : receive.ml ../../lib/ocaml/lib-pprz.cma - $(OCAMLC) $(INCLUDES) -o $@ lablgtk.cma glibivy-ocaml.cma lib-pprz.cma $^ +receive : wind.cmo receive.ml ../../lib/ocaml/lib-pprz.cma + $(OCAMLC) $(INCLUDES) -o $@.out lablgtk.cma glibivy-ocaml.cma lib-pprz.cma $^ @cat ../../../pprz_src_test.sh > $@ - @echo 'exec lablgtk2 -I $$PAPARAZZI_SRC/sw/lib/ocaml glibivy-ocaml.cma lib-pprz.cma $$PAPARAZZI_SRC/sw/ground_segment/tmtc/receive.ml $$*' >> $@ + @echo 'exec lablgtk2 -I $$PAPARAZZI_SRC/sw/lib/ocaml glibivy-ocaml.cma lib-pprz.cma -I $$PAPARAZZI_SRC/sw/ground_segment/tmtc $$PAPARAZZI_SRC/sw/ground_segment/tmtc/wind.cmo $$PAPARAZZI_SRC/sw/ground_segment/tmtc/receive.ml $$*' >> $@ @chmod a+x $@ diff --git a/sw/ground_segment/tmtc/receive.ml b/sw/ground_segment/tmtc/receive.ml index 51792d91bd..af8c53c11c 100644 --- a/sw/ground_segment/tmtc/receive.ml +++ b/sw/ground_segment/tmtc/receive.ml @@ -248,7 +248,9 @@ let log_and_parse = fun logging ac_name a msg values -> a.course <- norm_course ((Deg>>Rad)(fvalue "course" /. 10.)); a.alt <- fvalue "alt" /. 100.; a.climb <- fvalue "climb" /. 100.; - a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE" + a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE"; + if a.gspeed > 3. then + Wind.update ac_name a.gspeed a.course | "DESIRED" -> a.desired_east <- fvalue "desired_x"; a.desired_north <- fvalue "desired_y"; @@ -416,6 +418,21 @@ let send_horiz_status = fun a -> | UnknownHorizMode -> () +let send_wind = fun a -> + let id = a.id in + try + let (wind_cap_deg, wind_polar, mean, stddev, nb_sample) = Wind.get id in + let vs = + ["ac_id", Pprz.String id; + "dir", Pprz.Float wind_cap_deg; + "speed", Pprz.Float wind_polar; + "mean_aspeed", Pprz.Float mean; + "stddev", Pprz.Float stddev] in + Ground_Pprz.message_send my_id "WIND" vs + with + exc -> () + + let send_aircraft_msg = fun ac -> try let a = Hashtbl.find aircrafts ac in @@ -473,7 +490,8 @@ let send_aircraft_msg = fun ac -> send_fbw a; send_infrared a; send_svsinfo a; - send_horiz_status a + send_horiz_status a; + send_wind a with Not_found -> prerr_endline ac | x -> prerr_endline (Printexc.to_string x) @@ -509,11 +527,16 @@ let check_alerts = fun a -> if a.bat < 9. then send "CATASTROPHIC" else if a.bat < 10. then send "CRITIC" else if a.bat < 10.5 then send "WARNING" + +let wind_clear = fun _sender vs -> + Wind.clear (Pprz.string_assoc "ac_id" vs) let register_aircraft = fun name a -> Hashtbl.add aircrafts name a; ignore (Glib.Timeout.add aircraft_msg_period (fun () -> send_aircraft_msg name; true)); - ignore (Glib.Timeout.add aircraft_alerts_period (fun () -> check_alerts a; true)) + ignore (Glib.Timeout.add aircraft_alerts_period (fun () -> check_alerts a; true)); + Wind.new_ac name 1000; + ignore(Ground_Pprz.message_bind "WIND_CLEAR" wind_clear) (** Identifying message from a A/C *) diff --git a/sw/ground_segment/tmtc/wind.ml b/sw/ground_segment/tmtc/wind.ml new file mode 100644 index 0000000000..259e978bcd --- /dev/null +++ b/sw/ground_segment/tmtc/wind.ml @@ -0,0 +1,249 @@ +(* + * $Id$ + * + * Multi aircrafts receiver, logger and broadcaster + * + * Copyright (C) 2004 CENA/ENAC, 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. + * + *) + +(* + * + * Estimate wind by analysing aircrafts trajectories + * + * Author : Nicolas Barnier - barnier@recherche.enac.fr + * + *) + +let debug = false + +open Printf + +let (//) = Filename.concat +let conf_xml = Xml.parse_file (Env.paparazzi_home // "conf" // "conf.xml") + +open Geometry_2d + +type point_val = {p : pt_2D; f : float} + +type triangle = {a: point_val; b: point_val; c: point_val} + +let bary t = barycenter [t.a.p; t.b.p; t.c.p] + +let init w_init step f = + let pb = vect_add w_init {x2D = step; y2D = 0.} + and pc = vect_add w_init {x2D = 0.; y2D = step} in + {a = {p = w_init; f = f w_init}; + b = {p = pb; f = f pb}; + c = {p = pc; f = f pc}} + +let shift pa fa t = {a = {p = pa; f = fa}; b = t.a; c = t.b} + +let shiftpv pf t = shift pf.p pf.f t + +let calcnew p b alpha = vect_add_mul_scal alpha b (vect_make b p) + +let triangle_sort t = + let abc = [|t.a; t.b; t.c|] in + Array.sort (fun t1 t2 -> compare t2.f t1.f) abc; + {a = abc.(0); b = abc.(1); c = abc.(2)} + +let norme2 p = p.x2D *. p.x2D +. p.y2D *. p.y2D + +let simplex p fmax step max_iter precision = + let f x = -. (fmax x) in + + let rec loop num_iter vs = + if num_iter < max_iter && norme2 (vect_make vs.a.p vs.c.p) > precision then begin + begin if debug then + let pa = cart2polar vs.a.p in + Printf.printf "%f %f %f\n" pa.theta2D pa.r2D (-. vs.a.f) end; + + let vb = bary vs in + let vr = calcnew vs.c.p vb (-1.) in + let fvr = f vr in + let new_vs = + if fvr > vs.a.f then + let ve = calcnew vs.c.p vb (-2.) in + let fve = f ve in + if fve > fvr then shift ve fve vs + else shift vr fvr vs + else + let vc = calcnew vs.c.p vb 0.5 in + let fvc = f vc in + if fvc > vs.b.f || fvr > vs.b.f then + let v = if fvr > fvc then {p = vr; f = fvr} else {p = vc; f = fvc} in + if v.f <= vs.b.f then {vs with c = v} + else if v.f > vs.a.f then shiftpv v vs + else {vs with b = v; c = vs.b} + else + let vcb = calcnew vs.b.p vs.a.p 0.5 + and vcc = calcnew vs.c.p vs.a.p 0.5 in + triangle_sort {vs with b = {p = vcb; f = f vcb}; c = {p = vcc; f = f vcc}} in + + loop (num_iter + 1) new_vs end + else vs.a in + + if debug then Printf.printf "%f %f %f\n" p.x2D p.y2D (fmax p); + let vs = init p step f in + let vs = triangle_sort vs in + loop 0 vs + + +let isotropic_mean wind speeds = + let n = Array.length speeds in + let air_speeds = Array.map (fun speed -> cart2polar (vect_sub speed wind)) speeds in + let weights = + Array.map + (fun air -> + let sum = + Array.fold_left + (fun acc airj -> + acc +. norm_angle_rad (abs_float (air.theta2D -. airj.theta2D)) /. m_pi) + 0. air_speeds in + sum /. (float (n-1))) + air_speeds in + let mean = ref 0. in + for i = 0 to n-1 do + mean := !mean +. vect_norm (vect_sub speeds.(i) wind) *. weights.(i) done; + !mean /. float n + +let isotropic_wind wind_init speeds precision = + let n = Array.length speeds in + let mean wind = + let air_speeds = Array.map (fun speed -> cart2polar (vect_sub speed wind)) speeds in + let weights = + Array.mapi + (fun i airi -> + let sum = ref 0. in + for j = 0 to n-1 do + if j <> i then + sum := !sum +. + norm_angle_rad (abs_float (airi.theta2D -. air_speeds.(j).theta2D)) /. m_pi + done; + !sum /. (float (n-1))) + air_speeds in + let sum_weights = Array.fold_left (+.) 0. weights in + + let mean = ref 0. in + for i = 0 to n-1 do + mean := !mean +. vect_norm (vect_sub speeds.(i) wind) *. weights.(i) done; + (!mean /. sum_weights, sum_weights, weights) in + + let nb_calls = ref 0 in + let cost wind = + incr nb_calls; + let (m, sum_weights, weights) = mean wind in + let sum = ref 0. in + for i = 0 to n-1 do + let err = weights.(i) *. (vect_norm (vect_sub speeds.(i) wind) -. m) in + sum := !sum +. err *. err done; + !sum /. sum_weights in + + let step = 2. and max_iter = 100 in + let wind = simplex wind_init cost step max_iter precision in + if debug then Printf.printf "nb calls: %d\n" !nb_calls; + + let (mean, _, _) = mean wind.p in + (wind.p, mean, wind.f) + + +(* val wind : Geometry_2d.pt_2D -> Geometry_2d.pt_2D array -> float + -> (Geometry_2d.pt_2Dfloat * float * float) *) +(** [wind wind_init speeds precision] returns the wind and air speed mean and std dev. *) + +let wind wind_init speeds precision = + let mean wind = + let sum = + Array.fold_left (fun acc speed -> acc +. vect_norm (vect_sub speed wind)) 0. speeds in + sum /. float (Array.length speeds) in + + let nb_calls = ref 0 in + let cost wind = + incr nb_calls; + let m = mean wind in + let sum = + Array.fold_left + (fun acc speed -> + let err = vect_norm (vect_sub speed wind) -. m in + acc +. err *. err) + 0. speeds in + sum /. float (Array.length speeds) in + + let step = 2. and max_iter = 100 in + let wind = simplex wind_init cost step max_iter precision in + if debug then Printf.printf "nb calls: %d\n" !nb_calls; + + (wind.p, mean wind.p, wind.f) + +type wind_ac = + {mutable speeds : Geometry_2d.pt_2D array; + mutable index : int; + mutable length : int; + mutable wind_init : Geometry_2d.pt_2D} + +let h = Hashtbl.create 17 + +let create_wind_ac max_nb_sample = + {speeds = Array.create max_nb_sample {x2D=0.; y2D=0.} ; wind_init = null_vector; index = 0; length = 0} + +let new_ac = fun id max_nb_sample -> + Hashtbl.add h id (create_wind_ac max_nb_sample) + +let clear id = + let wind_ac = Hashtbl.find h id in + wind_ac.index <- 0; + wind_ac.length <- 0; + wind_ac.wind_init <- null_vector + +let precision = 1e-3 + +let update = fun id r course -> + let theta = heading_of_to_angle_rad course in + let speed = polar2cart {r2D = r; theta2D = theta} in + let wind_ac = Hashtbl.find h id in + let i = wind_ac.index in + wind_ac.speeds.(i) <- speed; + let i' = i + 1 in + wind_ac.length <- max wind_ac.length i'; + wind_ac.index <- i' mod (Array.length wind_ac.speeds) + + +let compute = fun compute_wind id -> + try + let wind_ac = Hashtbl.find h id in + let speeds = Array.sub wind_ac.speeds 0 wind_ac.length in + if Array.length speeds >= 3 then begin + let wind_init = wind_ac.wind_init in + let (wind, mean, stddev) = compute_wind wind_init speeds precision in + wind_ac.wind_init <- wind; + let wind_polar = cart2polar wind in + let wind_cap_deg = rad2deg (wind_dir_from_angle_rad wind_polar.theta2D) in + let nb_sample = Array.length speeds in + (wind_cap_deg, wind_polar.r2D, mean, stddev, nb_sample) + end else + failwith (Printf.sprintf "Wind.on_wind_compute: ac %s not enough data\n%!" id) + with Not_found -> + failwith (Printf.sprintf "Wind.on_wind_compute: ac %s unknown\n%!" id) + + +let get = fun id -> compute wind id +let get_iso = fun id -> compute isotropic_wind id + diff --git a/sw/simulator/gaia.ml b/sw/simulator/gaia.ml index 8f8fd664fb..b25d4f159f 100644 --- a/sw/simulator/gaia.ml +++ b/sw/simulator/gaia.ml @@ -56,8 +56,8 @@ let _ = 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 + 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;