diff --git a/Makefile b/Makefile index 798d6f6f63..27d6a2c9e1 100644 --- a/Makefile +++ b/Makefile @@ -108,8 +108,6 @@ visu3d: lib wind: cd $(WIND); $(MAKE) -receive: tmtc - $(TMTC)/receive static_h : PAPARAZZI_HOME=`pwd` PAPARAZZI_SRC=`pwd` make -f Makefile.gen @@ -142,6 +140,7 @@ deb : dpkg-buildpackage -rfakeroot clean: + rm -fr dox find . -mindepth 2 -name Makefile -exec sh -c '$(MAKE) -C `dirname {}` $@' \; find . -name '*~' -exec rm -f {} \; diff --git a/conf/control_panel.xml.example b/conf/control_panel.xml.example index 9ce74408b2..58c29828dc 100644 --- a/conf/control_panel.xml.example +++ b/conf/control_panel.xml.example @@ -20,7 +20,7 @@
- + @@ -87,7 +87,7 @@ - + @@ -98,7 +98,7 @@ - + @@ -116,7 +116,7 @@ - + @@ -129,7 +129,7 @@ - + diff --git a/conf/control_panel.xml.sys b/conf/control_panel.xml.sys deleted file mode 100644 index 990ff0818a..0000000000 --- a/conf/control_panel.xml.sys +++ /dev/null @@ -1,84 +0,0 @@ - - - - -
- -
- -
- - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - - - - - - - - - -
- - diff --git a/sw/ground_segment/tmtc/Makefile b/sw/ground_segment/tmtc/Makefile index c8e6cd2d29..2e2ce2bf4c 100644 --- a/sw/ground_segment/tmtc/Makefile +++ b/sw/ground_segment/tmtc/Makefile @@ -24,11 +24,11 @@ include ../../../conf/Makefile.local CONF = ../../../conf VAR = ../../../var -all: hw_modem_listen receive messages wavecard $(VAR)/boa.conf +all: hw_modem_listen server messages wavecard $(VAR)/boa.conf # stereo_demod clean: - rm -f hw_modem_listen wavecard_connect receive messages *.bak *~ core *.o .depend *.opt *.out *.cm* + rm -f hw_modem_listen wavecard_connect server messages *.bak *~ core *.o .depend *.opt *.out *.cm* OCAMLC = ocamlc OCAMLOPT = ocamlopt @@ -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 : wind.cmo receive.ml ../../lib/ocaml/lib-pprz.cma +server : aircraft.cmo wind.cmo airprox.cmo server.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 -I $$PAPARAZZI_SRC/sw/ground_segment/tmtc $$PAPARAZZI_SRC/sw/ground_segment/tmtc/wind.cmo $$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/aircraft.cmo $$PAPARAZZI_SRC/sw/ground_segment/tmtc/wind.cmo $$PAPARAZZI_SRC/sw/ground_segment/tmtc/airprox.cmo $$PAPARAZZI_SRC/sw/ground_segment/tmtc/server.ml $$*' >> $@ @chmod a+x $@ wavecard : wavecard_connect.ml ../../lib/ocaml/lib-pprz.cma @@ -85,7 +85,8 @@ modem.cmo : modem.cmi %.cmi : %.mli $(OCAMLC) $(INCLUDES) $< - +aircraft.cmo : aircraft.cmi +airprox.cmo : airprox.cmi wavecard_c : wavecard_c.c wavecard_utils_c.c wavecard_foo_c.c gcc -Wall -o $@ $^ `pkg-config --cflags glib-2.0` `pkg-config --libs glib-2.0` diff --git a/sw/ground_segment/tmtc/aircraft.ml b/sw/ground_segment/tmtc/aircraft.ml new file mode 100644 index 0000000000..58fceb721d --- /dev/null +++ b/sw/ground_segment/tmtc/aircraft.ml @@ -0,0 +1,113 @@ +(* + * $Id$ + * + * Copyright (C) ENAC + * + * 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. + * + *) + +type ac_cam = { + mutable phi : float; (* Rad, right = >0 *) + mutable theta : float; (* Rad, front = >0 *) + } + +type inflight_calib = { + mutable if_mode : int; (* DOWN|OFF|UP *) + mutable if_val1 : float; + mutable if_val2 : float + } + +type contrast_status = string (** DEFAULT|WAITING|SET *) +type infrared = { + mutable gps_hybrid_mode : int; + mutable gps_hybrid_factor : float; + mutable contrast_status : contrast_status; + mutable contrast_value : int + } + +type rc_status = string (** OK, LOST, REALLY_LOST *) +type rc_mode = string (** MANUAL, AUTO, FAILSAFE *) +type fbw = { + mutable rc_status : rc_status; + mutable rc_mode : rc_mode; + } + +let gps_nb_channels = 16 +type svinfo = { + svid : int; + flags : int; + qi : int; + cno : int; + elev : int; + azim : int + } + +let svinfo_init = { + svid = 0 ; + flags = 0; + qi = 0; + cno = 0; + elev = 0; + azim = 0; + } + +type horiz_mode = + Circle of Latlong.utm * int + | Segment of Latlong.utm * Latlong.utm + | UnknownHorizMode + +type aircraft = { + id : string; + mutable pos : Latlong.utm; + mutable roll : float; + mutable pitch : float; + mutable nav_ref : Latlong.utm; + mutable desired_east : float; + mutable desired_north : float; + mutable desired_altitude : float; + mutable desired_course : float; + mutable desired_climb : float; + mutable gspeed : float; + mutable course : float; + mutable alt : float; + mutable climb : float; + mutable cur_block : int; + mutable cur_stage : int; + mutable throttle : float; + mutable throttle_accu : float; + mutable rpm : float; + mutable temp : float; + mutable bat : float; + mutable amp : float; + mutable energy : int; + mutable ap_mode : int; + mutable gaz_mode : int; + mutable lateral_mode : int; + mutable horizontal_mode : int; + cam : ac_cam; + mutable gps_mode : int; + inflight_calib : inflight_calib; + infrared : infrared; + fbw : fbw; + svinfo : svinfo array; + mutable flight_time : int; + mutable stage_time : int; + mutable block_time : int; + mutable horiz_mode : horiz_mode + } diff --git a/sw/ground_segment/tmtc/aircraft.mli b/sw/ground_segment/tmtc/aircraft.mli new file mode 100644 index 0000000000..1e4028db12 --- /dev/null +++ b/sw/ground_segment/tmtc/aircraft.mli @@ -0,0 +1,95 @@ +(* + * $Id$ + * + * Copyright (C) ENAC + * + * 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. + * + *) + +(** State of an A/C handled by the server *) + +type ac_cam = { mutable phi : float; mutable theta : float; } +type inflight_calib = { + mutable if_mode : int; + mutable if_val1 : float; + mutable if_val2 : float; +} +type contrast_status = string +type infrared = { + mutable gps_hybrid_mode : int; + mutable gps_hybrid_factor : float; + mutable contrast_status : contrast_status; + mutable contrast_value : int; +} +type rc_status = string +type rc_mode = string +type fbw = { mutable rc_status : rc_status; mutable rc_mode : rc_mode; } +val gps_nb_channels : int +type svinfo = { + svid : int; + flags : int; + qi : int; + cno : int; + elev : int; + azim : int; +} +val svinfo_init : svinfo +type horiz_mode = + Circle of Latlong.utm * int + | Segment of Latlong.utm * Latlong.utm + | UnknownHorizMode +type aircraft = { + id : string; + mutable pos : Latlong.utm; + mutable roll : float; + mutable pitch : float; + mutable nav_ref : Latlong.utm; + mutable desired_east : float; + mutable desired_north : float; + mutable desired_altitude : float; + mutable desired_course : float; + mutable desired_climb : float; + mutable gspeed : float; + mutable course : float; + mutable alt : float; + mutable climb : float; + mutable cur_block : int; + mutable cur_stage : int; + mutable throttle : float; + mutable throttle_accu : float; + mutable rpm : float; + mutable temp : float; + mutable bat : float; + mutable amp : float; + mutable energy : int; + mutable ap_mode : int; + mutable gaz_mode : int; + mutable lateral_mode : int; + mutable horizontal_mode : int; + cam : ac_cam; + mutable gps_mode : int; + inflight_calib : inflight_calib; + infrared : infrared; + fbw : fbw; + svinfo : svinfo array; + mutable flight_time : int; + mutable stage_time : int; + mutable block_time : int; + mutable horiz_mode : horiz_mode; +} diff --git a/sw/ground_segment/tmtc/airprox.ml b/sw/ground_segment/tmtc/airprox.ml new file mode 100644 index 0000000000..b87368e65f --- /dev/null +++ b/sw/ground_segment/tmtc/airprox.ml @@ -0,0 +1,73 @@ +(* + * $Id$ + * + * Air Proximity Alert Module + * + * Copyright (C) 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. + * + *) + +open Aircraft +open Latlong + +module Alerts_Pprz = Pprz.Protocol(struct let name = "alert" end) + +(** computes distance between 2d points *) +let distance = fun (x1, y1) (x2, y2) -> + sqrt (((x1 -. x2) **2.) +. ((y1 -. y2)**2.)) + + +(** check if there is an airprox between aircraft1 and aircraft2 *) +(** if the altitude difference is less than 10 meters *) +(** and if the horizontal distance is less than 100 meters (5s between *) +(** 2 aircraft at 10m/s *) +let airprox = fun aircraft1 aircraft2 -> + let x1 = aircraft1.pos.utm_x and x2 = aircraft2.pos.utm_x and + y1 = aircraft1.pos.utm_y and y2 = aircraft2.pos.utm_y and + z1 = aircraft1.alt and z2 = aircraft2.alt in + let alt_difference = abs_float (z1 -. z2) and + dist = distance (x1, y1) (x2, y2) in + ((alt_difference < 10.0) && (dist < 100.0)) + +(** return airprox level *) +(** level is warning if the distance between both aircraft is increasing *) +(** level is crictical otherwise *) +let airprox_level = fun aircraft1 aircraft2 -> + let x1 = aircraft1.pos.utm_x and x2 = aircraft2.pos.utm_x and + y1 = aircraft1.pos.utm_y and y2 = aircraft2.pos.utm_y in + let d0 = distance (x1, y1) (x2, y2) in + let course1 = aircraft1.course and course2 = aircraft2.course and + speed1 = aircraft1.gspeed and speed2 = aircraft2.gspeed in + (** FIXME: must have the real angle and not a course *) + let halfpi = Latlong.pi /. 2. in + let vx1 = speed1 *. (cos (halfpi -. course1)) and + vx2 = speed2 *. (cos (halfpi -. course2)) and + vy1 = speed1 *. (sin (halfpi -. course1)) and + vy2 = speed2 *. (sin (halfpi -. course2)) in + let d1 = distance + (x1+. vx1 *. 0.2, x2+. vx2 *. 0.2) + (y1+. vy1 *. 0.2, y2+. vy2 *. 0.2) in + if d1 < d0 then "CRITICAL" else "WARNING" + +(** send a airprox alert on ivy if there is an airprox between ac_name1 and *) +(** ac_name2 *) +type alert_level = string option +let check_airprox = fun ac1 ac2 -> + if airprox ac1 ac2 then Some (airprox_level ac1 ac2) else None diff --git a/sw/ground_segment/tmtc/airprox.mli b/sw/ground_segment/tmtc/airprox.mli new file mode 100644 index 0000000000..39d9bdfef9 --- /dev/null +++ b/sw/ground_segment/tmtc/airprox.mli @@ -0,0 +1,29 @@ +(* + * $Id$ + * + * Air Proximity Alert Module + * + * Copyright (C) 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. + * + *) + +type alert_level = string option +val check_airprox : Aircraft.aircraft -> Aircraft.aircraft -> alert_level +(** [check_airprox ac1 ac2] Returns airprox level *) diff --git a/sw/ground_segment/tmtc/receive.ml b/sw/ground_segment/tmtc/server.ml similarity index 89% rename from sw/ground_segment/tmtc/receive.ml rename to sw/ground_segment/tmtc/server.ml index 936ab3fe70..5cad1b1539 100644 --- a/sw/ground_segment/tmtc/receive.ml +++ b/sw/ground_segment/tmtc/server.ml @@ -29,6 +29,7 @@ let gps_mode_3D = 3 open Printf open Latlong +open Aircraft module U = Unix module Tele_Class = struct let name = "telemetry_ap" end @@ -67,97 +68,7 @@ let check_index = fun i t where -> i let get_indexed_value = fun t i -> - if i >= 0 then t.(i) else "UNK" - -type ac_cam = { - mutable phi : float; (* Rad, right = >0 *) - mutable theta : float; (* Rad, front = >0 *) - } - -type inflight_calib = { - mutable if_mode : int; (* DOWN|OFF|UP *) - mutable if_val1 : float; - mutable if_val2 : float - } - -type contrast_status = string (** DEFAULT|WAITING|SET *) -type infrared = { - mutable gps_hybrid_mode : int; - mutable gps_hybrid_factor : float; - mutable contrast_status : contrast_status; - mutable contrast_value : int - } - -type rc_status = string (** OK, LOST, REALLY_LOST *) -type rc_mode = string (** MANUAL, AUTO, FAILSAFE *) -type fbw = { - mutable rc_status : rc_status; - mutable rc_mode : rc_mode; - } - -let gps_nb_channels = 16 -type svinfo = { - svid : int; - flags : int; - qi : int; - cno : int; - elev : int; - azim : int - } - -let svinfo_init = { - svid = 0 ; - flags = 0; - qi = 0; - cno = 0; - elev = 0; - azim = 0; - } - -type horiz_mode = - Circle of utm * int - | Segment of utm * utm - | UnknownHorizMode - -type aircraft = { - id : string; - mutable pos : Latlong.utm; - mutable roll : float; - mutable pitch : float; - mutable nav_ref : Latlong.utm; - mutable desired_east : float; - mutable desired_north : float; - mutable desired_altitude : float; - mutable desired_course : float; - mutable desired_climb : float; - mutable gspeed : float; - mutable course : float; - mutable alt : float; - mutable climb : float; - mutable cur_block : int; - mutable cur_stage : int; - mutable throttle : float; - mutable throttle_accu : float; - mutable rpm : float; - mutable temp : float; - mutable bat : float; - mutable amp : float; - mutable energy : int; - mutable ap_mode : int; - mutable gaz_mode : int; - mutable lateral_mode : int; - mutable horizontal_mode : int; - cam : ac_cam; - mutable gps_mode : int; - inflight_calib : inflight_calib; - infrared : infrared; - fbw : fbw; - svinfo : svinfo array; - mutable flight_time : int; - mutable stage_time : int; - mutable block_time : int; - mutable horiz_mode : horiz_mode - } + if i >= 0 then t.(i) else "UNK" (** The aircrafts store *) let aircrafts = Hashtbl.create 3 @@ -541,10 +452,35 @@ let wind_clear = fun _sender vs -> let periodic = fun period cb -> ignore (Glib.Timeout.add period (fun () -> cb (); true)) +(** add the periodic airprox check for the aircraft (name) on all aircraft *) +(** already known *) +let periodic_airprox_check = fun name -> + let thisac = Hashtbl.find aircrafts name in + let aircrafts2list = fun () -> + let list = ref [] in + Hashtbl.iter (fun name a -> list := name :: !list) aircrafts; + !list in + let ac_names = List.filter (fun a -> a <> name) (aircrafts2list ()) in + let list_ac = List.map (fun name -> Hashtbl.find aircrafts name) ac_names in + + let check_airprox = fun ac -> + match Airprox.check_airprox thisac ac with + None -> () + | Some level -> + let vs = + ["ac_id", Pprz.String (thisac.id ^ "," ^ ac.id) ; "level", Pprz.String level; "value", Pprz.String ac.id] in + Alerts_Pprz.message_send my_id "AIR_PROX" vs in + + List.iter + (fun ac -> + periodic aircraft_alerts_period (fun () -> check_airprox ac) + ) list_ac + let register_aircraft = fun name a -> Hashtbl.add aircrafts name a; periodic aircraft_msg_period (fun () -> send_aircraft_msg name); periodic aircraft_alerts_period (fun () -> check_alerts a); + periodic_airprox_check name; periodic wind_msg_period (fun () -> send_wind a); Wind.new_ac name 1000; ignore(Ground_Pprz.message_bind "WIND_CLEAR" wind_clear)