mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 16:58:48 +08:00
Wind estimation
This commit is contained in:
Pascal Brisset
+3
-1
@@ -275,10 +275,12 @@
|
||||
<message name="SELECTED_REQ" ID="6">
|
||||
</message>
|
||||
|
||||
<message name="WIND_REQ" ID="7">
|
||||
<message name="WIND_CLEAR" ID="7">
|
||||
<field name="ac_id" type="string"/>
|
||||
</message>
|
||||
|
||||
<message name="WIND" ID="8">
|
||||
<field name="ac_id" type="string"/>
|
||||
<field name="dir" type="float" unit="deg_wind"/>
|
||||
<field name="speed" type="float" unit="m/s"/>
|
||||
<field name="mean_aspeed" type="float" unit="m/s"/>
|
||||
|
||||
@@ -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 $@
|
||||
|
||||
|
||||
|
||||
@@ -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 $@
|
||||
|
||||
|
||||
|
||||
@@ -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 *)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user