diff --git a/sw/ground_segment/cockpit/Makefile b/sw/ground_segment/cockpit/Makefile index e20aeeea69..2c111a8bb9 100644 --- a/sw/ground_segment/cockpit/Makefile +++ b/sw/ground_segment/cockpit/Makefile @@ -29,20 +29,21 @@ INCLUDES=-I +lablgtk2 -I ../../lib/ocaml LIBS=glibivy-ocaml.cma lablgtk.cma lib-pprz.cma lablgnomecanvas.cma xlib-pprz.cma CMXA=$(LIBS:.cma=.cmxa) -ML=horizon.ml pages.ml map2d.ml +ML= speech.ml horizon.ml pages.ml strip.ml plugin.ml sectors.ml map2d.ml editFP.ml live.ml gcs.ml +MAIN=gcs CMO=$(ML:.ml=.cmo) CMX=$(ML:.ml=.cmx) -all : map2d +all : $(MAIN) -opt : map2d.opt +opt : $(MAIN).opt -map2d : $(CMO) +$(MAIN) : $(CMO) @echo OL $@ $(Q)$(OCAMLC) -custom $(INCLUDES) $(LIBS) threads.cma gtkThread.cmo gtkInit.cmo $(CMO) -o $@ -map2d.opt : $(CMX) +$(MAIN).opt : $(CMX) @echo OOL $@ $(Q)$(OCAMLOPT) $(INCLUDES) str.cmxa unix.cmxa xml-light.cmxa $(LIBS:.cma=.cmxa) threads.cmxa gtkThread.cmx gtkInit.cmx $(CMX) -o $@ @@ -50,15 +51,22 @@ map2d.opt : $(CMX) %.cmo: %.ml ../../lib/ocaml/lib-pprz.cma ../../lib/ocaml/xlib-pprz.cma @echo OC $< $(Q)$(OCAMLC) $(INCLUDES) -c $< -%.cmi: %.mli +%.cmi: %.mli ../../lib/ocaml/lib-pprz.cma ../../lib/ocaml/xlib-pprz.cma @echo OCI $< $(Q)$(OCAMLC) $(INCLUDES) -c $< %.cmx: %.ml @echo OOC $< $(Q)$(OCAMLOPT) $(INCLUDES) -c $< -map2d.cmo : pages.cmi +gcs.cmo : pages.cmi speech.cmi editFP.cmi sectors.cmi pages.cmo pages.cmx : pages.cmi horizon.cmi +live.cmo : plugin.cmi live.cmi strip.cmi speech.cmi +map2d.cmo : map2d.cmi +strip.cmo : strip.cmi +speech.cmo : speech.cmi +plugin.cmo : plugin.cmi +editFP.cmo : editFP.cmi +sectors.cmo : sectors.cmi clean: rm -f *~* *.cm* *.o *.out *.opt map2d diff --git a/sw/ground_segment/cockpit/editFP.ml b/sw/ground_segment/cockpit/editFP.ml new file mode 100644 index 0000000000..c5eddb7f8f --- /dev/null +++ b/sw/ground_segment/cockpit/editFP.ml @@ -0,0 +1,299 @@ +(***************** Editing ONE (single) flight plan **************************)open Printf +open Latlong + +module G2D = Geometry_2d + + +let (//) = Filename.concat +let fp_example = Env.flight_plans_path // "example.xml" +let default_path_maps = Env.paparazzi_home // "data" // "maps" + +(** Dummy flight plan (for map calibration) *) +let dummy_fp = fun latlong -> + Xml.Element("flight_plan", + ["lat0", string_of_float ((Rad>>Deg)latlong.posn_lat); + "lon0", string_of_float ((Rad>>Deg)latlong.posn_long); + "alt", "42."; + "MAX_DIST_FROM_HOME", "1000."], + [Xml.Element("waypoints", [],[])]) + + + +let current_fp = ref None + + (** Wrapper checking there is currently no flight plan loaded *) +let if_none = fun f -> + match !current_fp with + Some _ -> + GToolbox.message_box "Error" "Only one editable flight plan at a time" + | None -> + f () + + +let close_fp = fun () -> + match !current_fp with + None -> () (* Nothing to close *) + | Some (fp, _filename, window) -> + window#destroy (); + fp#destroy (); + current_fp := None + +let load_xml_fp = fun geomap accel_group ?(xml_file=Env.flight_plans_path) xml -> + Map2d.set_georef_if_none geomap (MapFP.georef_of_xml xml); + let fp = new MapFP.flight_plan ~show_moved:false geomap "red" Env.flight_plan_dtd xml in + let window = GWindow.window ~title:"Flight Plan" () in + window#set_default_size ~width:700 ~height:250; + window#add fp#window; + window#add_accel_group accel_group; + window#show(); + ignore (window#connect#destroy ~callback:close_fp); + current_fp := Some (fp,xml_file, window); + fp + +let labelled_entry = fun ?width_chars text value h -> + let _ = GMisc.label ~text ~packing:h#add () in + GEdit.entry ?width_chars ~text:value ~packing:h#add () + +let new_fp = fun geomap accel_group () -> + if_none (fun () -> + let dialog = GWindow.window ~border_width:10 ~title:"New flight plan" () in + let dvbx = GPack.box `VERTICAL ~packing:dialog#add () in + let h = GPack.hbox ~packing:dvbx#pack () in + let default_latlong = + match geomap#georef with + None -> "WGS84 37.21098 -113.45678" + | Some geo -> Latlong.string_of geo in + let latlong = labelled_entry ~width_chars:25 "Geographic Reference" default_latlong h in + let alt0 = labelled_entry ~width_chars:4 "Ground Alt" "380" h in + let h = GPack.hbox ~packing:dvbx#pack () in + let alt = labelled_entry ~width_chars:4 "Default Alt" "430" h in + let qfu = labelled_entry ~width_chars:4 "QFU" "270" h in + let mdfh = labelled_entry ~width_chars:4 "Max distance from HOME" "500" h in + + let h = GPack.hbox ~packing:dvbx#pack () in + let name = labelled_entry "Name" "Test flight" h in + + let h = GPack.hbox ~packing:dvbx#pack () in + let cancel = GButton.button ~stock:`CANCEL ~packing: h#add () in + ignore(cancel#connect#clicked ~callback:dialog#destroy); + + let createfp = GButton.button ~stock:`OK ~packing: h#add () in + createfp#grab_default (); + ignore(createfp#connect#clicked ~callback: + begin fun _ -> + let xml = Xml.parse_file fp_example in + let s = ExtXml.subst_attrib in + let wgs84 = Latlong.of_string latlong#text in + let xml = s "lat0" (deg_string_of_rad wgs84.posn_lat) xml in + let xml = s "lon0" (deg_string_of_rad wgs84.posn_long) xml in + let xml = s "ground_alt" alt0#text xml in + let xml = s "qfu" qfu#text xml in + let xml = s "alt" alt#text xml in + let xml = s "max_dist_from_home" mdfh#text xml in + let xml = s "name" name#text xml in + ignore (load_xml_fp geomap accel_group xml); + dialog#destroy () + end); + dialog#show ()) + + +(** Loading a flight plan for edition *) +let load_fp = fun geomap accel_group () -> + if_none (fun () -> + match GToolbox.select_file ~title:"Open flight plan" ~filename:(Env.flight_plans_path // "*.xml") () with + None -> () + | Some xml_file -> + try + let xml = Xml.parse_file xml_file in + ignore (load_xml_fp geomap accel_group ~xml_file xml) + with + Dtd.Check_error(e) -> + let m = sprintf "Error while loading %s:\n%s" xml_file (Dtd.check_error e) in + GToolbox.message_box "Error" m) + +let create_wp = fun geo -> + match !current_fp with + None -> + GToolbox.message_box "Error" "Load a flight plan first"; + failwith "create_wp" + | Some (fp,_,_) -> + fp#add_waypoint geo + + +let save_fp = fun () -> + match !current_fp with + None -> () (* Nothing to save *) + | Some (fp, filename,_) -> + match GToolbox.select_file ~title:"Save Flight Plan" ~filename () with + None -> () + | Some file -> + let f = open_out file in + fprintf f "%s\n" (Xml.to_string_fmt fp#xml); + close_out f + +let ref_point_of_waypoint = fun xml -> + Xml.Element("point", ["x",Xml.attrib xml "x"; + "y",Xml.attrib xml "y"; + "geo", Xml.attrib xml "name"],[]) + + +(** Calibration of chosen image (requires a dummy flight plan) *) +let calibrate_map = fun (geomap:MapCanvas.widget) accel_group () -> + match !current_fp with + | Some (_fp,_,_) -> GToolbox.message_box "Error" "Close current flight plan before calibration" + | None -> + match GToolbox.select_file ~filename:default_path_maps ~title:"Open Image" () with + None -> () + | Some image -> + (** Displaying the image in the NW corner *) + let pixbuf = GdkPixbuf.from_file image in + let pix = GnoCanvas.pixbuf ~pixbuf ~props:[`ANCHOR `NW] geomap#canvas#root in + let (x0, y0) = geomap#canvas#get_scroll_offsets in + let (x,y) = geomap#canvas#window_to_world (float x0) (float y0) in + pix#move x y; + + (** Open a dummy flight plan *) + let dummy_georef = + match geomap#georef with + None -> {posn_lat = (Deg>>Rad)43.; posn_long = (Deg>>Rad)1. } + | Some geo -> geo in + let fp_xml = dummy_fp dummy_georef in + let fp = load_xml_fp geomap accel_group fp_xml in + + (** Dialog to finish calibration *) + let dialog = GWindow.window ~border_width:10 ~title:"Map calibration" () in + let v = GPack.vbox ~packing:dialog#add () in + let _ = GMisc.label ~text:"Choose 2 (or more) waypoints (Ctrl-Left)\nRename the waypoints with their geographic coordinates\nFor example: 'WGS84 43.123456 1.234567' or 'UTM 530134 3987652 12' or 'LBT2e 123456 543210'\nClick the button below to save the XML result file\n" ~packing:v#add () in + let h = GPack.hbox ~packing:v#pack () in + let cancel = GButton.button ~stock:`CLOSE ~packing:h#add () in + let cal = GButton.button ~stock:`OK ~packing:h#add () in + let destroy = fun () -> + dialog#destroy (); + close_fp (); + pix#destroy () in + ignore(cancel#connect#clicked ~callback:destroy); + ignore(cal#connect#clicked ~callback:(fun _ -> + let points = List.map XmlEdit.xml_of_node fp#waypoints in + let points = List.map ref_point_of_waypoint points in + let xml = Xml.Element ("map", + ["file", Filename.basename image; + "projection", geomap#projection], + points) in + match GToolbox.select_file ~filename:(default_path_maps//".xml") ~title:"Save calibrated map" () with + None -> () + | Some xml_file -> + let f = open_out xml_file in + Printf.fprintf f "%s\n" (Xml.to_string_fmt xml); + close_out f)); + cal#grab_default (); + dialog#show () + +let radius = ref 30. +let path = ref (ref []) +let cur_arc = ref None +and cur_seg = ref None +and cur_f = ref { G2D.x2D = 0.; y2D = 0. } + +let set_segment = fun s p1 p2 -> + let x1 = p1.G2D.x2D and y1 = p1.G2D.y2D + and x2 = p2.G2D.x2D and y2 = p2.G2D.y2D in + s#set [`POINTS [|x1;y1;x2;y2|]] + +let arc_from_points = fun (geomap:MapCanvas.widget) c l f s -> + let cl = G2D.vect_make c l + and cf = G2D.vect_make c f in + let pol_cl = G2D.cart2polar cl in + let al = pol_cl.G2D.theta2D + and af = (G2D.cart2polar cf).G2D.theta2D in + let xc = c.G2D.x2D and yc = c.G2D.y2D in + let (a1, a2) = if s > 0. then (al, af) else (af, al) in + geomap#arc ~nb_points:10 ~fill_color:"blue" ~width:2 (xc,yc) pol_cl.G2D.r2D a1 a2 + + (** Update path after waypoint move *) +let update_path = fun (geomap:MapCanvas.widget) path waypoint -> + let n = waypoint#name in + let rec loop = function + [] -> failwith (sprintf "update_path: %s not found" n) + | [p] -> [p] + | (wp, Some arc, Some seg, _f, radius)::wps -> + let new_wps = if wp#name = waypoint#name then wps else loop wps in + begin + match new_wps with + [] -> failwith "unreachable" + | (wp1, _arc1, _seg1, f1, _r1)::new_wps' -> (* Previous *) + let wp1_2D = geomap#pt2D_of wp1#pos + and wp_2D = geomap#pt2D_of wp#pos in + match new_wps' with + [] -> (* wp is the second point: simple segment *) + set_segment seg wp1_2D wp_2D; + (wp, Some arc, Some seg, wp1_2D, radius)::new_wps + | _ -> (* At least 2 points before *) + let (c, f, s) = G2D.arc_segment f1 wp1_2D wp_2D radius in + set_segment seg f wp_2D; + arc#destroy (); + let new_arc = arc_from_points geomap c wp1_2D f s in + (wp, Some new_arc, Some seg, f, radius)::new_wps + end + | _ -> failwith "update_path" in + path := loop !path + + + +let path_button = fun (geomap:MapCanvas.widget) (xw, yw) -> + let geo = geomap#of_world (xw, yw) in + let wp = create_wp geo in + let cur_path = !path in + wp#connect (fun () -> update_path geomap cur_path wp); + if ! !path = [] then + cur_f := {G2D.x2D=xw; G2D.y2D=yw}; + cur_path := (wp, !cur_arc, !cur_seg, !cur_f, !radius) :: ! cur_path; + cur_arc := Some (GnoCanvas.line geomap#canvas#root); + cur_seg := Some (geomap#segment ~fill_color:"blue" ~width:2 geo geo) + +let path_notify (geomap:MapCanvas.widget) (xw, yw) = + match ! !path with + [] -> false (** Empty path: nothing to do *) + | (wp1, _, _, ll,_) :: p -> (** Last is wp1 *) + begin + match !cur_seg with + None -> failwith "path_notify" + | Some segment -> + let l = geomap#pt2D_of wp1#pos + and cur_2D = {G2D.x2D = xw; y2D = yw } in + match p with + [] -> (** Only 1 point in the current path:add a simple segment*) + set_segment segment l cur_2D; + | _::_ -> (** Already 2 points: add an arc and a segment *) + match !cur_arc with + None -> failwith "path_notify" + | Some arc -> + arc#destroy (); + let (c, f, s) = G2D.arc_segment ll l cur_2D !radius in + set_segment segment f cur_2D; + let arc = arc_from_points geomap c l f s in + cur_arc := Some arc; + cur_f := f + end; + true + + +let path_close = fun () -> + let destroy = fun ref -> + match !ref with + None -> () + | Some s -> s#destroy (); ref := None in + destroy cur_arc; + destroy cur_seg; + begin + match !current_fp with + None -> () + | Some (fp, _,_) -> + fp#insert_path (List.map (fun (wp,_,_,_,r) -> (wp, r)) (List.rev ! !path)); + end; + path := ref [] + +let path_change_radius = function + `UP -> radius := !radius *. 1.1 + | `DOWN -> radius := !radius /. 1.1 + | _ -> () + diff --git a/sw/ground_segment/cockpit/editFP.mli b/sw/ground_segment/cockpit/editFP.mli new file mode 100644 index 0000000000..6749e54519 --- /dev/null +++ b/sw/ground_segment/cockpit/editFP.mli @@ -0,0 +1,11 @@ +val path_button : + MapCanvas.widget -> MapCanvas.LL.fmeter * MapCanvas.LL.fmeter -> unit +val path_notify : MapCanvas.widget -> float * float -> bool +val path_close : unit -> unit +val path_change_radius : [> `DOWN | `UP ] -> unit +val create_wp : MapCanvas.LL.geographic -> MapWaypoints.waypoint +val calibrate_map : MapCanvas.widget -> Gtk.accel_group -> unit -> unit +val new_fp : MapCanvas.widget -> Gtk.accel_group -> unit -> unit +val load_fp : MapCanvas.widget -> Gtk.accel_group -> unit -> unit +val save_fp : unit -> unit +val close_fp : unit -> unit diff --git a/sw/ground_segment/cockpit/gcs.ml b/sw/ground_segment/cockpit/gcs.ml new file mode 100644 index 0000000000..8e7e327041 --- /dev/null +++ b/sw/ground_segment/cockpit/gcs.ml @@ -0,0 +1,552 @@ +(* +* $Id$ +* +* Multi aircrafts map display and flight plan editor +* +* Copyright (C) 2004-2006 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. +* +*) + +module G = MapCanvas +open Printf +open Latlong + +let soi = string_of_int + +let home = Env.paparazzi_home +let (//) = Filename.concat +let default_path_srtm = home // "data" // "srtm" +let default_path_maps = home // "data" // "maps" +let var_maps_path = home // "var" // "maps" +let _ = + ignore (Sys.command (sprintf "mkdir -p %s" var_maps_path)) + +let ign = ref false +let get_bdortho = ref "" +let auto_center_new_ac = ref false +let no_alarm = ref false + + + +(** Display a calibrated (XML) map *) +let display_map = fun (geomap:G.widget) xml_map -> + try + let dir = Filename.dirname xml_map in + let xml_map = Xml.parse_file xml_map in + let image = dir // ExtXml.attrib xml_map "file" in + let map_projection = Xml.attrib xml_map "projection" in + let opacity = try Some (int_of_string (Xml.attrib xml_map "opacity")) with _ -> None in + let current_projection = geomap#projection in + if map_projection <> current_projection then + GToolbox.message_box "Warning" (sprintf "You are loading a map in %s projection while the display use %s" map_projection current_projection); + + let pix_ref = fun p -> + truncate (ExtXml.float_attrib p "x"), truncate (ExtXml.float_attrib p "y") in + let geo_ref = fun p -> + try Latlong.of_string (Xml.attrib p "geo") with + _ -> (* Compatibility with the old UTM format *) + let utm_x = ExtXml.float_attrib p "utm_x" + and utm_y = ExtXml.float_attrib p "utm_y" in + let utm_zone = ExtXml.int_attrib xml_map "utm_zone" in + let utm = {utm_x = utm_x; utm_y = utm_y; utm_zone = utm_zone } in + Latlong.of_utm WGS84 utm in + + match Xml.children xml_map with + p1::p2::_ -> + let x1y1 = pix_ref p1 + and x2y2 = pix_ref p2 + and geo1 = geo_ref p1 + and geo2 = geo_ref p2 in + + (* Take this point as a reference for the display if none currently *) + Map2d.set_georef_if_none geomap geo1; + + ignore (geomap#display_pixbuf ?opacity ((x1y1),geo1) ((x2y2),geo2) (GdkPixbuf.from_file image)); + geomap#center geo1 + | _ -> failwith (sprintf "display_map: two ref points required") + with + Xml.File_not_found f -> + GToolbox.message_box "Error" (sprintf "File does not exist: %s" f) + | ExtXml.Error s -> + GToolbox.message_box "Error" (sprintf "Error in XML file: %s" s) + + + +let load_map = fun (geomap:G.widget) () -> + match GToolbox.select_file ~title:"Open Map" ~filename:(default_path_maps // "*.xml") () with + None -> () + | Some f -> display_map geomap f + + + +(** Save the given pixbuf calibrated with NW and SE corners *) +let save_map = fun geomap ?(projection=geomap#projection) pixbuf nw se -> + match GToolbox.select_file ~filename:(default_path_maps//".xml") ~title:"Save region map" () with + None -> () + | Some xml_file -> + let jpg = Filename.chop_extension xml_file ^ ".png" in + GdkPixbuf.save jpg "png" pixbuf; + let point = fun (x,y) wgs84 -> + Xml.Element ("point", ["x",soi x;"y",soi y;"geo", Latlong.string_of wgs84], []) in + let width = GdkPixbuf.get_width pixbuf + and height = GdkPixbuf.get_height pixbuf in + let points = [point (0, 0) nw; point (width, height) se] in + let xml = Xml.Element ("map", + ["file", Filename.basename jpg; + "projection", projection], + points) in + let f = open_out xml_file in + Printf.fprintf f "%s\n" (Xml.to_string_fmt xml); + close_out f + + + +(****** Creates a calibrated map from the bitmap (selected region) ***********) +let map_from_region = fun (geomap:G.widget) () -> + match geomap#region with + None -> GToolbox.message_box "Error" "Select a region (shift-left drag)" + | Some ((xw1,yw1), (xw2,yw2)) -> + let xw1, xw2 = min xw1 xw2, max xw1 xw2 + and yw1, yw2 = min yw1 yw2, max yw1 yw2 in + let (xc1, yc1) = geomap#canvas#w2c xw1 yw1 + and (xc2, yc2) = geomap#canvas#w2c xw2 yw2 in + let width = xc2-xc1 and height = yc2-yc1 in + let dest = GdkPixbuf.create width height () in + let (x0, y0) = geomap#canvas#get_scroll_offsets in + let src_x = xc1 - x0 and src_y = yc1 - y0 in + GdkPixbuf.get_from_drawable ~dest ~width ~height ~src_x ~src_y + geomap#canvas#misc#window; + let nw = geomap#of_world (xw1,yw1) + and se = geomap#of_world (xw2,yw2) in + save_map geomap dest nw se + + +(************ Google Maps handling *****************************************) +module GM = struct + let active_http = fun x -> + Gm.no_http := not x + + (** Fill the visible background with Google tiles *) + let fill_tiles = fun geomap -> + ignore (Thread.create MapGoogle.fill_window geomap) + + let auto = ref false + let update = fun geomap -> + if !auto then fill_tiles geomap + let active_auto = fun geomap x -> + auto := x; + update geomap + +(** Creates a calibrated map from the Google tiles (selected region) *) + let map_from_tiles = fun (geomap:G.widget) () -> + match geomap#region with + None -> GToolbox.message_box "Error" "Select a region (shift-left drag)" + | Some ((xw1,yw1), (xw2,yw2)) -> + let geo1 = geomap#of_world (xw1,yw1) + and geo2 = geomap#of_world (xw2,yw2) in + let sw = { posn_lat = min geo1.posn_lat geo2.posn_lat; + posn_long = min geo1.posn_long geo2.posn_long } + and ne = { posn_lat = max geo1.posn_lat geo2.posn_lat; + posn_long = max geo1.posn_long geo2.posn_long } in + let pix = MapGoogle.pixbuf sw ne in + let nw = { posn_lat = ne.posn_lat; posn_long = sw.posn_long } + and se = { posn_lat = sw.posn_lat; posn_long = ne.posn_long } in + save_map geomap ~projection:"Mercator" pix nw se +end (* GM module *) + +let bdortho_size = 400 +let bdortho_store = Hashtbl.create 97 +let display_bdortho = fun (geomap:G.widget) wgs84 () -> + let r = bdortho_size / 2 in + let { lbt_x = lx; lbt_y = ly} = lambertIIe_of wgs84 in + let lx = lx + r and ly = ly + 2 in + let lx = lx - (lx mod bdortho_size) + and ly = ly - (ly mod bdortho_size) in + let f = sprintf "ortho_%d_%d_%d.jpg" lx ly r in + let f = var_maps_path // f in + if not (Hashtbl.mem bdortho_store f) then begin + Hashtbl.add bdortho_store f true; + let display = fun _ -> + let nw = of_lambertIIe {lbt_x = lx - r; lbt_y = ly + r} + and se = of_lambertIIe {lbt_x = lx + r; lbt_y = ly - r} in + ignore (geomap#display_pixbuf ((0,0), nw) ((bdortho_size, bdortho_size), se) (GdkPixbuf.from_file f)); + + in + if Sys.file_exists f then + display f + else + ignore (Thread.create + (fun f -> + let c = sprintf "%s %d %d %d %s" !get_bdortho lx ly r f in + ignore (Sys.command c); + display f) + f) + end + + +let fill_ortho = fun (geomap:G.widget) -> + (** First estimate the coverage of the window *) + let width_c, height_c = Gdk.Drawable.get_size geomap#canvas#misc#window + and (xc0, yc0) = geomap#canvas#get_scroll_offsets in + let (xw0, yw0) = geomap#window_to_world (float xc0) (float (yc0+height_c)) + and (xw1, yw1) = geomap#window_to_world (float (xc0+width_c)) (float yc0) in + let sw = geomap#of_world (xw0, yw0) + and ne = geomap#of_world (xw1, yw1) in + let lbt2e_sw = lambertIIe_of sw + and lbt2e_ne = lambertIIe_of ne in + let w = lbt2e_ne.lbt_x - lbt2e_sw.lbt_x + and h = lbt2e_ne.lbt_y - lbt2e_sw.lbt_y in + for i = 0 to w / bdortho_size + 2 do + let lbt_x = lbt2e_sw.lbt_x + bdortho_size * i in + for j = 0 to h / bdortho_size + 2 do + let lbt_y = lbt2e_sw.lbt_y + bdortho_size * j in + let geo = of_lambertIIe {lbt_x = lbt_x; lbt_y = lbt_y } in + display_bdortho geomap geo () + done + done + + + + +(******* Mouse motion handling **********************************************) +let motion_notify = fun (geomap:G.widget) ev -> + let xc = GdkEvent.Motion.x ev + and yc = GdkEvent.Motion.y ev in + let xwyw = geomap#window_to_world xc yc in + EditFP.path_notify geomap xwyw + + +(******* Mouse wheel handling ***********************************************) +let any_event = fun (geomap:G.widget) ev -> + match GdkEvent.get_type ev with + `SCROLL -> + let state = GdkEvent.Scroll.state (GdkEvent.Scroll.cast ev) in + if Gdk.Convert.test_modifier `CONTROL state && Gdk.Convert.test_modifier `SHIFT state then + let scroll_event = GdkEvent.Scroll.cast ev in + EditFP.path_change_radius (GdkEvent.Scroll.direction scroll_event); + let xc = GdkEvent.Scroll.x scroll_event + and yc = GdkEvent.Scroll.y scroll_event in + let xwyw = geomap#window_to_world xc yc in + EditFP.path_notify geomap xwyw + else + false + | _ -> + false + + + +(******* Mouse buttons handling **********************************************) +let button_press = fun (geomap:G.widget) ev -> + let state = GdkEvent.Button.state ev in + if GdkEvent.Button.button ev = 3 && Gdk.Convert.test_modifier `CONTROL state && Gdk.Convert.test_modifier `SHIFT state then begin + EditFP.path_close (); + true + end else if GdkEvent.Button.button ev = 3 then begin + (** Display a tile from Google Maps or IGN *) + let xc = GdkEvent.Button.x ev + and yc = GdkEvent.Button.y ev in + let (xw,yw) = geomap#window_to_world xc yc in + + let thread = fun f x -> ignore (Thread.create f x) in + let wgs84 = geomap#of_world (xw,yw) in + let display_ign = fun () -> + thread (MapIGN.display_tile geomap) wgs84 + and display_gm = fun () -> + thread (fun () -> + try ignore (MapGoogle.display_tile geomap wgs84) with + Gm.Not_available -> ()) + () in + + let m = if !ign then [`I ("Load IGN tile", display_ign)] else [] in + let m = + if !get_bdortho <> "" then + (`I ("Load BDORTHO", display_bdortho geomap wgs84))::m + else + m in + + GToolbox.popup_menu ~entries:([`I ("Load Google tile", display_gm)]@m) + ~button:3 ~time:(Int32.of_int 00); + true; + end else if GdkEvent.Button.button ev = 1 && Gdk.Convert.test_modifier `CONTROL state then + if Gdk.Convert.test_modifier `SHIFT state then begin + let xc = GdkEvent.Button.x ev in + let yc = GdkEvent.Button.y ev in + let xyw = geomap#canvas#window_to_world xc yc in + EditFP.path_button geomap xyw; + true + end else begin + let xc = GdkEvent.Button.x ev in + let yc = GdkEvent.Button.y ev in + let xyw = geomap#canvas#window_to_world xc yc in + let geo = geomap#of_world xyw in + ignore (EditFP.create_wp geo); + true + end else + false + + + + + + +(******** Help ***************************************************************) +let keys_help = fun () -> + GToolbox.message_box ~title:"Keys" ~ok:"Close" + "Zoom: Mouse Wheel, PgUp, PgDown\n\ + Pan: Arrows\n\ + Load Map Tile: Right\n\ + Select Region: Left + Drag\n\ + Create Waypoint: Ctrl-Left\n\ + Create Path Point: Ctrl-Shift-Left\n\ + Change Path Radius: Wheel\n\ + Move Waypoint: Left drag\n\ + Edit Waypoint: Left click\n" + + + +(***************** MAIN ******************************************************) +let _main = + let ivy_bus = ref "127.255.255.255:2010" + and geo_ref = ref "" + and map_files = ref [] + and center = ref "" + and zoom = ref 1. + and maximize = ref false + and fullscreen = ref false + and projection = ref G.Mercator + and auto_ortho = ref false + and mplayer = ref "" + and plugin_window = ref "" in + let options = + [ "-b", Arg.String (fun x -> ivy_bus := x), "Bus\tDefault is 127.255.255.25:2010"; + "-maximize", Arg.Set maximize, "Maximize window"; + "-fullscreen", Arg.Set fullscreen, "Fullscreen window"; + "-ref", Arg.Set_string geo_ref, "Geographic ref (default '')"; + "-zoom", Arg.Set_float zoom, "Initial zoom"; + "-center", Arg.Set_string center, "Initial map center"; + "-center_ac", Arg.Set auto_center_new_ac, "Centers the map on any new A/C"; + "-plugin", Arg.Set_string plugin_window, "External X application (launched the id of the plugin window as argument)"; + "-mplayer", Arg.Set_string mplayer, "Launch mplayer with the given argument as X plugin"; + "-utm", Arg.Unit (fun () -> projection:=G.Mercator),"Switch to UTM local projection"; + "-mercator", Arg.Unit (fun () -> projection:=G.Mercator),"Switch to (Google Maps) Mercator projection, default"; + "-lambertIIe", Arg.Unit (fun () -> projection:=G.LambertIIe),"Switch to LambertIIe projection"; + "-ign", Arg.String (fun s -> ign:=true; IGN.data_path := s), "IGN tiles path"; + "-ortho", Arg.Set_string get_bdortho, "IGN tiles path"; + "-no_alarm", Arg.Set no_alarm, "Disables alarm page"; + "-auto_ortho", Arg.Set auto_ortho, "IGN tiles path"; + "-google_fill", Arg.Set GM.auto, "Google maps auto fill"; + "-speech", Arg.Set Speech.active, "Speech"; + "-m", Arg.String (fun x -> map_files := x :: !map_files), "Map description file"] in + Arg.parse (options) + (fun x -> Printf.fprintf stderr "Warning: Don't do anything with '%s'\n" x) + "Usage: "; + (* *) + Ivy.init "Paparazzi map 2D" "READY" (fun _ _ -> ()); + Ivy.start !ivy_bus; + + Srtm.add_path default_path_srtm; + Gm.cache_path := var_maps_path; + IGN.cache_path := var_maps_path; + + (** window for map2d **) + let window = GWindow.window ~title:"Paparazzi GCS" ~border_width:1 ~width:1024 ~height:750 () in + if !maximize then + window#maximize (); + if !fullscreen then + window#fullscreen (); + let vbox= GPack.vbox ~packing: window#add () in + + (** window for vertical situation *) + let vertical_situation = GWindow.window ~title: "Vertical" ~border_width:1 ~width:400 () in + let _vertical_vbox= GPack.vbox ~packing: vertical_situation#add () in + let quit = fun () -> GMain.Main.quit (); exit 0 in + + ignore (window#connect#destroy ~callback:quit); + ignore (vertical_situation#connect#destroy ~callback:quit); + + let geomap = new G.widget ~height:500 ~projection:!projection () in + + let menu_fact = new GMenu.factory geomap#file_menu in + let accel_group = menu_fact#accel_group in + + ignore (geomap#canvas#event#connect#button_press (button_press geomap)); + ignore (geomap#canvas#event#connect#motion_notify (motion_notify geomap)); + ignore (geomap#canvas#event#connect#any (any_event geomap)); + + (** widget displaying aircraft vertical position *) + + let _active_vertical = fun x -> + if x then vertical_situation#show () else vertical_situation#misc#hide () in + ignore (menu_fact#add_item "Redraw" ~key:GdkKeysyms._L ~callback:geomap#canvas#update_now); + let switch_fullscreen = fun x -> + if x then + window#fullscreen () + else + window#unfullscreen () in + ignore (menu_fact#add_check_item "Fullscreen" ~key:GdkKeysyms._F ~active: !fullscreen ~callback:switch_fullscreen); + ignore (menu_fact#add_item "Quit" ~key:GdkKeysyms._Q ~callback:quit); + + (* Maps handling *) + let map_menu = geomap#factory#add_submenu "Maps" in + let map_menu_fact = new GMenu.factory ~accel_group map_menu in + ignore (map_menu_fact#add_item "Load" ~key:GdkKeysyms._M ~callback:(load_map geomap)); + ignore (map_menu_fact#add_item "Calibrate" ~key:GdkKeysyms._C ~callback:(EditFP.calibrate_map geomap accel_group)); + ignore (map_menu_fact#add_item "GoogleMaps Fill" ~key:GdkKeysyms._G ~callback:(fun _ -> GM.fill_tiles geomap)); + ignore (map_menu_fact#add_check_item "GoogleMaps Http" ~key:GdkKeysyms._H ~active:true ~callback:GM.active_http); + ignore (map_menu_fact#add_check_item "GoogleMaps Auto" ~active:!GM.auto ~callback:(GM.active_auto geomap)); + ignore (map_menu_fact#add_item "Map of Region" ~key:GdkKeysyms._R ~callback:(map_from_region geomap)); + ignore (map_menu_fact#add_item "Map of Google Tiles" ~key:GdkKeysyms._T ~callback:(GM.map_from_tiles geomap)); + ignore (map_menu_fact#add_item "Load sector" ~callback:(Sectors.load geomap)); + + (** Connect Google Maps display to view change *) + geomap#connect_view (fun () -> GM.update geomap); + if !auto_ortho then + geomap#connect_view (fun () -> fill_ortho geomap); + + (** Flight plan editing *) + let fp_menu = geomap#factory#add_submenu "Edit" in + let fp_menu_fact = new GMenu.factory ~accel_group fp_menu in + ignore (fp_menu_fact#add_item "New flight plan" ~key:GdkKeysyms._N ~callback:(EditFP.new_fp geomap accel_group)); + ignore (fp_menu_fact#add_item "Open flight plan" ~key:GdkKeysyms._O ~callback:(EditFP.load_fp geomap accel_group)); + ignore (fp_menu_fact#add_item "Save flight plan" ~key:GdkKeysyms._S ~callback:(EditFP.save_fp)); + ignore (fp_menu_fact#add_item "Close flight plan" ~key:GdkKeysyms._W ~callback:(EditFP.close_fp)); + + (** Help pushed to the right *) + let mi = GMenu.menu_item ~label:"Help" ~right_justified:true ~packing:geomap#menubar#append () in + let help_menu = GMenu.menu () in + GToolbox.build_menu help_menu ~entries:[`I ("Keys", keys_help)]; + mi#set_submenu help_menu; + + (** Separate from A/C menus *) + ignore (geomap#factory#add_separator ()); + + let paned = GPack.paned ~show:true `VERTICAL ~packing:(vbox#pack ~expand:true) () in + let frame1 = GPack.vbox () in + paned#pack1 ~shrink:true (*** ~expand:true ***) frame1#coerce; + let hpaned = GPack.paned ~show:true `HORIZONTAL ~packing:paned#add2 () in + + (** Pack the canvas in the window *) + frame1#add geomap#frame#coerce; + (** Set the initial soom *) + geomap#zoom !zoom; + + (** Strips on the left *) + hpaned#add1 Strip.scrolled#coerce; + let hpaned2 = GPack.paned ~show:true `HORIZONTAL ~packing:hpaned#add2 () in + + (** Aircraft notebook *) + let fp_notebook = GPack.notebook ~tab_border:0 ~packing:(hpaned2#add1) () in + + let hpaned3 = GPack.paned ~show:true `HORIZONTAL ~packing:hpaned2#add2 () in + + (** Alerts text frame *) + let packing = if !no_alarm then fun _ -> () else hpaned3#add1 in + let alert_page = GBin.frame ~packing () in + let my_alert = new Pages.alert alert_page in + + if !mplayer <> "" then + plugin_window := sprintf "mplayer -nomouseinput '%s' -wid " !mplayer; + if !plugin_window <> "" then begin + let plugin_width = 400 and plugin_height = 300 in + let frame2 = GPack.vbox ~width:plugin_width () in + + hpaned3#pack2 frame2#coerce; + let frame = GBin.event_box ~packing:frame2#add ~width:plugin_width ~height:plugin_height () in + let s = GWindow.socket ~packing:frame#add () in + let com = sprintf "%s 0x%lx -geometry %dx%d" !plugin_window s#xwindow plugin_width plugin_height in + + let pid = ref None in + let restart = fun () -> + begin match !pid with + None -> () + | Some p -> try Unix.kill p 9 with _ -> () + end; + pid := Some (Unix.create_process "/bin/sh" [|"/bin/sh"; "-c"; com|] Unix.stdin Unix.stdout Unix.stderr) in + + restart (); + + ignore (menu_fact#add_item "Restart plugin" ~key:GdkKeysyms._P ~callback:restart); + + Plugin.frame := Some frame; + + let swap = fun _ -> + (** Keep the center of the geo canvas *) + let c = geomap#get_center () in + + let child1 = List.hd frame1#children in + let child2 = List.hd frame2#children in + child2#misc#reparent frame1#coerce; + child1#misc#reparent frame2#coerce; + + (* Strange: the centering does not work if done inside this callback. + It is postponed to be called by the mainloop(). *) + ignore (GMain.Idle.add (fun () -> geomap#center c; false)); + in + + let callback = fun ev -> + Printf.printf "%d\n%!" (GdkEvent.Button.button ev); + match GdkEvent.Button.button ev with + 1 -> swap (); true + | 3 -> restart (); true + | _ -> false in + + ignore (frame#event#connect#button_press ~callback) + end; + + + (** Periodically probe new A/Cs *) + ignore (Glib.Timeout.add 2000 (fun () -> Live.message_request "map2d" "AIRCRAFTS" [] (fun _sender vs -> Live.aircrafts_msg geomap fp_notebook vs); false)); + + (** New aircraft message *) + Live.safe_bind "NEW_AIRCRAFT" (fun _sender vs -> Live.one_new_ac geomap fp_notebook (Pprz.string_assoc "ac_id" vs)); + + (** Listen for all messages on ivy *) + Live.listen_flight_params geomap !auto_center_new_ac; + Live.listen_wind_msg (); + Live.listen_fbw_msg (); + Live.listen_engine_status_msg (); + Live.listen_if_calib_msg (); + Live.listen_waypoint_moved (); + Live.listen_infrared (); + Live.listen_svsinfo (); + Live.listen_alert my_alert; + + (** Display the window *) + window#add_accel_group accel_group; + window#show (); + + (** Loading an initial map *) + if !geo_ref <> "" then + Map2d.set_georef_if_none geomap (Latlong.of_string !geo_ref); + List.iter (fun map_file -> + let xml_map_file = if map_file.[0] <> '/' then default_path_maps // map_file else map_file in + display_map geomap xml_map_file) + !map_files; + + (** Center the map as required *) + if !center <> "" then begin + Map2d.set_georef_if_none geomap (Latlong.of_string !center); + geomap#center (Latlong.of_string !center) + end; + + Speech.say "Welcome to paparazzi"; + + (** Threaded main loop (map tiles loaded concurently) *) + GtkThread.main () diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml new file mode 100644 index 0000000000..36cd6aa054 --- /dev/null +++ b/sw/ground_segment/cockpit/live.ml @@ -0,0 +1,627 @@ +(******* Real time handling of flying A/Cs ***********************************) + +module G = MapCanvas +open Latlong +open Printf + +module Tele_Pprz = Pprz.Messages(struct let name = "telemetry" end) +module Ground_Pprz = Pprz.Messages(struct let name = "ground" end) +module Alert_Pprz = Pprz.Messages(struct let name = "alert" end) + +let rotate = fun a (x, y) -> + let cosa = cos a and sina = sin a in + (cosa *.x +. sina *.y, -. sina*.x +. cosa *. y) + +let rec list_casso x = function + [] -> raise Not_found + | (a,b)::abs -> if x = b then a else list_casso x abs + +let rec list_iter3 = fun f l1 l2 l3 -> + match l1, l2, l3 with + [], [], [] -> () + | x1::x1s, x2::x2s, x3::x3s -> + f x1 x2 x3; + list_iter3 f x1s x2s x3s + | _ -> invalid_arg "list_iter3" + + +type color = string +type aircraft = { + ac_name : string; + config : Pprz.values; + track : MapTrack.track; + color: color; + fp_group : MapFP.flight_plan; + fp : Xml.xml; + blocks : (int * string) list; + mutable last_ap_mode : string; + mutable last_stage : int * int; + ir_page : Pages.infrared; + gps_page : Pages.gps; + pfd_page : Pages.pfd; + misc_page : Pages.misc; + settings_page : Pages.settings option; + strip : Strip.t; + mutable first_pos : bool + } + +let live_aircrafts = Hashtbl.create 3 +let get_ac = fun vs -> + let ac_id = Pprz.string_assoc "ac_id" vs in + Hashtbl.find live_aircrafts ac_id + +let aircraft_pos_msg = fun track wgs84 heading altitude speed climb -> + let h = + try + Srtm.of_wgs84 wgs84 + with + _ -> truncate altitude in + track#move_icon wgs84 heading altitude (float_of_int h) speed climb + +let carrot_pos_msg = fun track wgs84 -> + track#move_carrot wgs84 + +let cam_pos_msg = fun track wgs84 target_wgs84 -> + track#move_cam wgs84 target_wgs84 + +let circle_status_msg = fun track wgs84 radius -> + track#draw_circle wgs84 radius + +let segment_status_msg = fun track geo1 geo2 -> + track#draw_segment geo1 geo2 + +let survey_status_msg = fun track geo1 geo2 -> + track#draw_zone geo1 geo2 + +let ap_status_msg = fun track flight_time -> + track#update_ap_status flight_time + + +let show_mission = fun ac on_off -> + let a = Hashtbl.find live_aircrafts ac in + if on_off then + a.fp_group#show () + else + a.fp_group#hide () + +let resize_track = fun ac track -> + match + GToolbox.input_string ~text:(string_of_int track#size) ~title:ac "Track size" + with + None -> () + | Some s -> track#resize (int_of_string s) + +let commit_changes = fun ac -> + let a = Hashtbl.find live_aircrafts ac in + List.iter + (fun w -> + let (i, w) = a.fp_group#index w in + if w#moved then + let wgs84 = w#pos in + let vs = ["ac_id", Pprz.String ac; + "wp_id", Pprz.Int i; + "lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat); + "long", Pprz.Float ((Rad>>Deg)wgs84.posn_long); + "alt", Pprz.Float w#alt + ] in + Ground_Pprz.message_send "map2d" "MOVE_WAYPOINT" vs) + a.fp_group#waypoints + +let send_event = fun ac e -> + Ground_Pprz.message_send "map2d" "SEND_EVENT" + ["ac_id", Pprz.String ac; "event_id", Pprz.Int e] + +let center = fun geomap track () -> + match track#last with + None -> () + | Some geo -> + geomap#center geo + + +let blocks_of_stages = fun stages -> + let blocks = ref [] in + List.iter (fun x -> + let name = ExtXml.attrib x "block_name" + and id = ExtXml.int_attrib x "block" in + if not (List.mem_assoc id !blocks) then + blocks := (id, name) :: !blocks) + (Xml.children stages); + List.sort compare !blocks + +let jump_to_block = fun ac_id id -> + Ground_Pprz.message_send "map2d" "JUMP_TO_BLOCK" + ["ac_id", Pprz.String ac_id; "block_id", Pprz.Int id] + +let menu_entry_of_block = fun ac_id (id, name) -> + let send_msg = fun () -> jump_to_block ac_id id in + `I (name, send_msg) + +let reset_waypoints = fun fp () -> + List.iter (fun w -> + let (_i, w) = fp#index w in + w#reset_moved ()) + fp#waypoints + +let icon = ref None +let show_snapshot = fun (geomap:G.widget) geo_FL geo_BR point pixbuf name ev -> + match ev with + | `BUTTON_PRESS ev -> + let image = GMisc.image ~pixbuf () in + let icon = image#coerce in + begin + match GToolbox.question_box ~title:name ~buttons:["Delete"; "Close"] ~icon "" with + 1 -> + point#destroy () + | _ -> () + end; + true + | `LEAVE_NOTIFY ev -> + begin + match !icon with + None -> () + | Some i -> i#destroy () + end; + false + | `ENTER_NOTIFY ev -> + let w = GdkPixbuf.get_width pixbuf + and h = GdkPixbuf.get_height pixbuf in + icon := Some (geomap#display_pixbuf ((0,0), geo_FL) ((w,h), geo_BR) pixbuf); + false + + | _ -> false + + +let mark = fun (geomap:G.widget) ac_id track plugin_frame -> + let i = ref 1 in fun () -> + match track#last with + Some geo -> + begin + let group = geomap#background in + let point = geomap#circle ~group ~fill_color:"blue" geo 5. in + point#raise_to_top (); + let lat = (Rad>>Deg)geo.posn_lat + and long = (Rad>>Deg)geo.posn_long in + Tele_Pprz.message_send ac_id "MARK" + ["ac_id", Pprz.String ac_id; + "lat", Pprz.Float lat; + "long", Pprz.Float long]; + match plugin_frame with + None -> () + | Some frame -> + let width, height = Gdk.Drawable.get_size frame#misc#window in + let dest = GdkPixbuf.create width height() in + GdkPixbuf.get_from_drawable ~dest ~width ~height frame#misc#window; + let name = sprintf "Snapshot-%s-%d_%f_%f_%f.png" ac_id !i lat long (track#last_heading) in + let png = sprintf "%s/var/logs/%s" Env.paparazzi_home name in + GdkPixbuf.save png "png" dest; + incr i; + + (* Computing the footprint: front_left and back_right *) + let cam_aperture = 2.4/.1.9 in (* width over distance FIXME *) + let alt = track#last_altitude -. float (Srtm.of_wgs84 geo) in + let width = cam_aperture *. alt in + let height = width *. 3. /. 4. in + let utm = utm_of WGS84 geo in + let a = (Deg>>Rad)track#last_heading in + let (xfl,yfl) = rotate a (-.width/.2., height/.2.) + and (xbr,ybr) = rotate a (width/.2., -.height/.2.) in + let geo_FL = of_utm WGS84 (utm_add utm (xfl,yfl)) + and geo_BR = of_utm WGS84 (utm_add utm (xbr,ybr)) in + ignore (point#connect#event (show_snapshot geomap geo_FL geo_BR point dest name)) + end + | None -> () + + +(** Load a mission. Returns the XML window *) +let load_mission = fun ?edit color geomap xml -> + Map2d.set_georef_if_none geomap (MapFP.georef_of_xml xml); + new MapFP.flight_plan ?edit ~show_moved:true geomap color Env.flight_plan_dtd xml + + + +let create_ac = fun (geomap:G.widget) (acs_notebook:GPack.notebook) ac_id config -> + let color = Pprz.string_assoc "default_gui_color" config + and name = Pprz.string_assoc "ac_name" config in + + (** Get the flight plan **) + let fp_url = Pprz.string_assoc "flight_plan" config in + let fp_file = Http.file_of_url fp_url in + let fp_xml_dump = Xml.parse_file fp_file in + let stages = ExtXml.child fp_xml_dump "stages" in + let blocks = blocks_of_stages stages in + + let label_box = GBin.event_box () in + let label = GPack.hbox ~packing:label_box#add ~spacing:3 () in + let eb = GBin.event_box ~width:10 ~height:10 ~packing:label#pack () in + eb#coerce#misc#modify_bg [`NORMAL, `NAME color]; + let _ac_label = GMisc.label ~text:name ~packing:label#pack () in + + let ac_mi = GMenu.image_menu_item ~image:label_box ~packing:geomap#menubar#append () in + let ac_menu = GMenu.menu () in + ac_mi#set_submenu ac_menu; + let ac_menu_fact = new GMenu.factory ac_menu in + let fp = ac_menu_fact#add_check_item "Fligh Plan" ~active:true in + ignore (fp#connect#toggled (fun () -> show_mission ac_id fp#active)); + + let track = new MapTrack.track ~name ~color:color geomap in + + let center_ac = center geomap track in + ignore (ac_menu_fact#add_item "Center A/C" ~callback:center_ac); + + ignore (ac_menu_fact#add_item "Clear Track" ~callback:(fun () -> track#clear_map2D)); + ignore (ac_menu_fact#add_item "Resize Track" ~callback:(fun () -> resize_track ac_id track)); + let reset_wp_menu = ac_menu_fact#add_item "Reset Waypoints" in + + let jump_block_entries = List.map (menu_entry_of_block ac_id) blocks in + + let commit_moves = fun () -> + commit_changes ac_id in + let sm = ac_menu_fact#add_submenu "Datalink" in + let dl_menu = [ + `M ("Jump to block", jump_block_entries); + `I ("Commit Moves", commit_moves); + `I ("Event 1", (fun () -> send_event ac_id 1)); + `I ("Event 2", (fun () -> send_event ac_id 2))] in + + GToolbox.build_menu sm ~entries:dl_menu; + + let cam = ac_menu_fact#add_check_item "Cam Display" ~active:false in + ignore (cam#connect#toggled (fun () -> track#set_cam_state cam#active)); + let params = ac_menu_fact#add_check_item "flight param. display" ~active:false in + ignore (params#connect#toggled (fun () -> track#set_params_state params#active)); + + (** Build the XML flight plan, connect then "jump_to_block" *) + let fp_xml = ExtXml.child fp_xml_dump "flight_plan" in + let fp = load_mission ~edit:false color geomap fp_xml in + fp#connect_activated (fun node -> + if XmlEdit.tag node = "block" then + let block = XmlEdit.attrib node "name" in + let id = list_casso block blocks in + jump_to_block ac_id id); + ignore (reset_wp_menu#connect#activate (reset_waypoints fp)); + + + (** Add a new tab in the A/Cs notebook, with a colored label *) + let eb = GBin.event_box () in + let _label = GMisc.label ~text:name ~packing:eb#add () in + eb#coerce#misc#modify_bg [`NORMAL, `NAME color;`ACTIVE, `NAME color]; + + (** Put a notebook for this A/C *) + let ac_frame = GBin.frame ~packing:(acs_notebook#append_page ~tab_label:eb#coerce) () in + let ac_notebook = GPack.notebook ~packing: ac_frame#add () in + let visible = fun w -> + ac_notebook#page_num w#coerce = ac_notebook#current_page in + + (** Insert the flight plan tab *) + let fp_label = GMisc.label ~text: "Flight Plan" () in + (ac_notebook:GPack.notebook)#append_page ~tab_label:fp_label#coerce fp#window#coerce; + + let infrared_label = GMisc.label ~text: "Infrared" () in + let infrared_frame = GBin.frame ~shadow_type: `NONE + ~packing: (ac_notebook#append_page ~tab_label: infrared_label#coerce) () in + let ir_page = new Pages.infrared infrared_frame in + + let gps_label = GMisc.label ~text: "GPS" () in + let gps_frame = GBin.frame ~shadow_type: `NONE + ~packing: (ac_notebook#append_page ~tab_label: gps_label#coerce) () in + let gps_page = new Pages.gps ~visible gps_frame in + + let pfd_label = GMisc.label ~text: "PFD" () in + let pfd_frame = GBin.frame ~shadow_type: `NONE + ~packing: (ac_notebook#append_page ~tab_label: pfd_label#coerce) () in + let pfd_page = new Pages.pfd pfd_frame + and _pfd_page_num = ac_notebook#page_num pfd_frame#coerce in + + let misc_label = GMisc.label ~text: "Misc" () in + let misc_frame = GBin.frame ~shadow_type: `NONE + ~packing: (ac_notebook#append_page ~tab_label:misc_label#coerce) () in + let misc_page = new Pages.misc ~packing:misc_frame#add misc_frame in + + let settings_page = + try + let xml_settings = Xml.children (ExtXml.child fp_xml "dl_settings") in + let callback = fun idx value -> + let vs = ["ac_id", Pprz.String ac_id; "index", Pprz.Int idx;"value", Pprz.Float value] in + Ground_Pprz.message_send "dl" "DL_SETTING" vs in + let settings_tab = new Pages.settings ~visible xml_settings callback in + let tab_label = (GMisc.label ~text:"Settings" ())#coerce in + ac_notebook#append_page ~tab_label settings_tab#widget; + Some settings_tab + with + _ -> None in + + (** Add a strip and connect it to the A/C notebook *) + let select_this_tab = + let n = acs_notebook#page_num ac_frame#coerce in + fun () -> acs_notebook#goto_page n in + let strip = Strip.add config color select_this_tab center_ac commit_moves (mark geomap ac_id track !Plugin.frame) in + + + Hashtbl.add live_aircrafts ac_id { track = track; color = color; + fp_group = fp ; config = config ; + fp = fp_xml; ac_name = name; + blocks = blocks; last_ap_mode= ""; + last_stage = (-1,-1); + ir_page = ir_page; + gps_page = gps_page; + pfd_page = pfd_page; + misc_page = misc_page; + settings_page = settings_page; + strip = strip; first_pos = true + } + + (** Bind to message while catching all the esceptions of the callback *) +let safe_bind = fun msg cb -> + let safe_cb = fun sender vs -> + try cb sender vs with _ -> () in + ignore (Ground_Pprz.message_bind msg safe_cb) + +let alert_bind = fun msg cb -> + let safe_cb = fun sender vs -> + try cb sender vs with _ -> () in + ignore (Alert_Pprz.message_bind msg safe_cb) + +let ask_config = fun geomap fp_notebook ac -> + let get_config = fun _sender values -> + create_ac geomap fp_notebook ac values + in + Ground_Pprz.message_req "map2d" "CONFIG" ["ac_id", Pprz.String ac] get_config + + + +let one_new_ac = fun (geomap:G.widget) fp_notebook ac -> + if not (Hashtbl.mem live_aircrafts ac) then begin + ask_config geomap fp_notebook ac + end + +let get_wind_msg = fun _sender vs -> + let ac = get_ac vs in + let value = fun field_name -> sprintf "%.1f" (Pprz.float_assoc field_name vs) in + ac.misc_page#set_wind_speed (value "wspeed"); + ac.misc_page#set_wind_dir (value "dir") + +let get_fbw_msg = fun _sender vs -> + let ac = get_ac vs in + let status = Pprz.string_assoc "rc_status" vs in + Strip.set_label ac.strip "RC" status; + Strip.set_color ac.strip "RC" + (match status with + "LOST" -> "orange" + | "REALLY_LOST" -> "red" + | _ -> "white") + + + +let get_engine_status_msg = fun _sender vs -> + let ac = get_ac vs in + Strip.set_label ac.strip "throttle" + (string_of_float (Pprz.float_assoc "throttle" vs)); + Strip.set_bat ac.strip (Pprz.float_assoc "bat" vs) + +let get_if_calib_msg = fun _sender vs -> + let ac = get_ac vs in + Strip.set_label ac.strip "settings" (Pprz.string_assoc "if_mode" vs) + +let listen_wind_msg = fun () -> + safe_bind "WIND" get_wind_msg + +let listen_fbw_msg = fun () -> + safe_bind "FLY_BY_WIRE" get_fbw_msg + +let listen_engine_status_msg = fun () -> + safe_bind "ENGINE_STATUS" get_engine_status_msg + +let listen_if_calib_msg = fun () -> + safe_bind "INFLIGH_CALIB" get_if_calib_msg + +let list_separator = Str.regexp "," + +let aircrafts_msg = fun (geomap:G.widget) fp_notebook acs -> + let acs = Pprz.string_assoc "ac_list" acs in + let acs = Str.split list_separator acs in + List.iter (one_new_ac geomap fp_notebook) acs + + +let listen_dl_value = fun () -> + let get_dl_value = fun _sender vs -> + let ac_id = Pprz.string_assoc "ac_id" vs in + let ac = Hashtbl.find live_aircrafts ac_id in + match ac.settings_page with + Some settings -> + let csv = Pprz.string_assoc "values" vs in + let values = Array.of_list (Str.split list_separator csv) in + for i = 0 to min (Array.length values) settings#length - 1 do + settings#set i (float_of_string values.(i)) + done + | None -> () in + safe_bind "DL_VALUES" get_dl_value + + +let highlight_fp = fun ac b s -> + if (b, s) <> ac.last_stage then begin + ac.last_stage <- (b, s); + ac.fp_group#highlight_stage b s + end + + +let listen_flight_params = fun geomap auto_center_new_ac -> + let get_fp = fun _sender vs -> + let ac = get_ac vs in + let pfd_page = ac.pfd_page in + + pfd_page#set_attitude (Pprz.float_assoc "roll" vs) (Pprz.float_assoc "pitch" vs); + pfd_page#set_alt (Pprz.float_assoc "alt" vs); + pfd_page#set_climb (Pprz.float_assoc "climb" vs); + pfd_page#set_speed (Pprz.float_assoc "speed" vs); + + let a = fun s -> Pprz.float_assoc s vs in + let wgs84 = { posn_lat = (Deg>>Rad)(a "lat"); posn_long = (Deg>>Rad)(a "long") } in + aircraft_pos_msg ac.track wgs84 (a "course") (a "alt") (a "speed") (a "climb"); + + if auto_center_new_ac && ac.first_pos then begin + center geomap ac.track (); + ac.first_pos <- false + end; + + let set_label lbl_name field_name = + let s = + if (a field_name) < 0. + then + "- "^(sprintf "%.1f" (abs_float (a field_name))) + else + sprintf "%.1f" (a field_name) + in + Strip.set_label ac.strip lbl_name s + in + set_label "alt" "alt"; + set_label "speed" "speed"; + set_label "climb" "climb" + in + safe_bind "FLIGHT_PARAM" get_fp; + + let get_ns = fun _sender vs -> + let ac = get_ac vs in + let a = fun s -> Pprz.float_assoc s vs in + let wgs84 = { posn_lat = (Deg>>Rad)(a "target_lat"); posn_long = (Deg>>Rad)(a "target_long") } in + carrot_pos_msg ac.track wgs84; + let cur_block = Pprz.int_assoc "cur_block" vs + and cur_stage = Pprz.int_assoc "cur_stage" vs in + let b = List.assoc cur_block ac.blocks in + let b = String.sub b 0 (min 10 (String.length b)) in + highlight_fp ac cur_block cur_stage; + let set_label = fun l f -> + Strip.set_label ac.strip l (sprintf "%.1f" (Pprz.float_assoc f vs)) in + set_label "->" "target_alt"; + set_label "/" "target_climb"; + Strip.set_label ac.strip "block_name" b + in + safe_bind "NAV_STATUS" get_ns; + + let get_cam_status = fun _sender vs -> + let ac_id = Pprz.string_assoc "ac_id" vs in + let ac = Hashtbl.find live_aircrafts ac_id in + let a = fun s -> Pprz.float_assoc s vs in + let wgs84 = { posn_lat = (Deg>>Rad)(a "cam_lat"); posn_long = (Deg>>Rad)(a "cam_long") } + and target_wgs84 = { posn_lat = (Deg>>Rad)(a "cam_target_lat"); posn_long = (Deg>>Rad)(a "cam_target_long") } in + + cam_pos_msg ac.track wgs84 target_wgs84 + in + safe_bind "CAM_STATUS" get_cam_status; + + let get_circle_status = fun _sender vs -> + let ac = get_ac vs in + let a = fun s -> Pprz.float_assoc s vs in + let wgs84 = { posn_lat = (Deg>>Rad)(a "circle_lat"); posn_long = (Deg>>Rad)(a "circle_long") } in + circle_status_msg ac.track wgs84 (float_of_string (Pprz.string_assoc "radius" vs)) + in + safe_bind "CIRCLE_STATUS" get_circle_status; + + let get_segment_status = fun _sender vs -> + let ac_id = Pprz.string_assoc "ac_id" vs in + let ac = Hashtbl.find live_aircrafts ac_id in + let a = fun s -> Pprz.float_assoc s vs in + let geo1 = { posn_lat = (Deg>>Rad)(a "segment1_lat"); posn_long = (Deg>>Rad)(a "segment1_long") } + and geo2 = { posn_lat = (Deg>>Rad)(a "segment2_lat"); posn_long = (Deg>>Rad)(a "segment2_long") } in + segment_status_msg ac.track geo1 geo2 + in + safe_bind "SEGMENT_STATUS" get_segment_status; + + + let get_survey_status = fun _sender vs -> + let ac = get_ac vs in + let a = fun s -> Pprz.float_assoc s vs in + let geo1 = { posn_lat = (Deg>>Rad)(a "south_lat"); posn_long = (Deg>>Rad)(a "west_long") } + and geo2 = { posn_lat = (Deg>>Rad)(a "north_lat"); posn_long = (Deg>>Rad)(a "east_long") } in + survey_status_msg ac.track geo1 geo2 + in + safe_bind "SURVEY_STATUS" get_survey_status; + + + let get_ap_status = fun _sender vs -> + let ac = get_ac vs in + ap_status_msg ac.track ( float_of_int (Pprz.int32_assoc "flight_time" vs )); + let ap_mode = Pprz.string_assoc "ap_mode" vs in + if ap_mode <> ac.last_ap_mode then begin + Speech.say (sprintf "%s, %s" ac.ac_name ap_mode); + ac.last_ap_mode <- ap_mode + end; + Strip.set_label ac.strip "AP" (Pprz.string_assoc "ap_mode" vs); + Strip.set_color ac.strip "AP" (if ap_mode="HOME" then "red" else "white"); + let gps_mode = Pprz.string_assoc "gps_mode" vs in + Strip.set_label ac.strip "GPS" gps_mode; + Strip.set_color ac.strip "GPS" (if gps_mode<>"3D" then "red" else "white"); + let ft = + let t = Int32.to_int (Int32.of_string (Pprz.string_assoc "flight_time" vs)) in + sprintf "%02d:%02d:%02d" (t / 3600) ((t mod 3600) / 60) ((t mod 3600) mod 60) in + Strip.set_label ac.strip "flight_time" ft + in + safe_bind "AP_STATUS" get_ap_status; + + listen_dl_value () + +let listen_waypoint_moved = fun () -> + let get_values = fun _sender vs -> + let ac = get_ac vs in + let wp_id = Pprz.int_assoc "wp_id" vs in + let a = fun s -> Pprz.float_assoc s vs in + let geo = { posn_lat = (Deg>>Rad)(a "lat"); posn_long = (Deg>>Rad)(a "long") } + and altitude = a "alt" in + + (** FIXME: No indexed access to waypoints: iter and compare: *) + List.iter (fun w -> + let (i, w) = ac.fp_group#index w in + if i = wp_id then begin + w#set ~if_not_moved:true ~altitude ~update:true geo; + raise Exit (** catched by safe_bind *) + end) + ac.fp_group#waypoints + in + safe_bind "WAYPOINT_MOVED" get_values + +let get_alert_bat_low = fun a _sender vs -> + let ac_id = Pprz.string_assoc "ac_id" vs in + let ac_name = ref "" in + let level = Pprz.string_assoc "level" vs in + let get_config = fun _sender config -> + ac_name := Pprz.string_assoc "ac_name" config; + a#add (!ac_name^" "^"BAT_LOW"^" "^level) + in + Ground_Pprz.message_req "map2d" "CONFIG" ["ac_id", Pprz.String ac_id] get_config + + +let listen_alert = fun a -> + alert_bind "BAT_LOW" (get_alert_bat_low a) + + +let get_infrared = fun _sender vs -> + let ac_id = Pprz.string_assoc "ac_id" vs in + let ac = Hashtbl.find live_aircrafts ac_id in + let ir_page = ac.ir_page in + let gps_hybrid_mode = Pprz.string_assoc "gps_hybrid_mode" vs in + let gps_hybrid_factor = Pprz.float_assoc "gps_hybrid_factor" vs in + let contrast_status = Pprz.string_assoc "contrast_status" vs in + let contrast_value = Pprz.int_assoc "contrast_value" vs in + + ir_page#set_gps_hybrid_mode gps_hybrid_mode; + ir_page#set_gps_hybrid_factor gps_hybrid_factor; + ir_page#set_contrast_status contrast_status; + ir_page#set_contrast_value contrast_value + +let listen_infrared = fun () -> safe_bind "INFRARED" get_infrared + +let get_svsinfo = fun _sender vs -> + let ac_id = Pprz.string_assoc "ac_id" vs in + let ac = Hashtbl.find live_aircrafts ac_id in + let gps_page = ac.gps_page in + let svid = Str.split list_separator (Pprz.string_assoc "svid" vs) + and cn0 = Str.split list_separator (Pprz.string_assoc "cno" vs) + and flags = Str.split list_separator (Pprz.string_assoc "flags" vs) in + + list_iter3 + (fun id cno flags -> + if id <> "0" then gps_page#svsinfo id cno (int_of_string flags)) + svid cn0 flags + +let listen_svsinfo = fun () -> safe_bind "SVSINFO" get_svsinfo + +let message_request = Ground_Pprz.message_req diff --git a/sw/ground_segment/cockpit/live.mli b/sw/ground_segment/cockpit/live.mli new file mode 100644 index 0000000000..734bb42813 --- /dev/null +++ b/sw/ground_segment/cockpit/live.mli @@ -0,0 +1,15 @@ +val message_request : string -> string -> Pprz.values -> (string -> Pprz.values -> unit) -> unit + +val aircrafts_msg : MapCanvas.widget -> GPack.notebook -> Pprz.values -> unit +val safe_bind : string -> (string -> Pprz.values -> unit) -> unit +val one_new_ac : MapCanvas.widget -> GPack.notebook -> string -> unit +val listen_flight_params : + < center : MapCanvas.LL.geographic -> unit; .. > -> bool -> unit +val listen_wind_msg : unit -> unit +val listen_fbw_msg : unit -> unit +val listen_engine_status_msg : unit -> unit +val listen_if_calib_msg : unit -> unit +val listen_waypoint_moved : unit -> unit +val listen_infrared : unit -> unit +val listen_svsinfo : unit -> unit +val listen_alert : < add : string -> unit; .. > -> unit diff --git a/sw/ground_segment/cockpit/map2d.ml b/sw/ground_segment/cockpit/map2d.ml index 1228ebec01..798beb4f87 100644 --- a/sw/ground_segment/cockpit/map2d.ml +++ b/sw/ground_segment/cockpit/map2d.ml @@ -1,8 +1,6 @@ (* * $Id$ * -* Multi aircrafts map display and flight plan editor -* * Copyright (C) 2004-2006 ENAC, Pascal Brisset, Antoine Drouin * * This file is part of paparazzi. @@ -24,1608 +22,7 @@ * *) -module G2D = Geometry_2d -module G = MapCanvas -open Printf -open Latlong - -let float_attr = fun xml a -> float_of_string (ExtXml.attrib xml a) -let int_attr = fun xml a -> int_of_string (ExtXml.attrib xml a) - -let rotate = fun a (x, y) -> - let cosa = cos a and sina = sin a in - (cosa *.x +. sina *.y, -. sina*.x +. cosa *. y) - -let rec list_iter3 = fun f l1 l2 l3 -> - match l1, l2, l3 with - [], [], [] -> () - | x1::x1s, x2::x2s, x3::x3s -> - f x1 x2 x3; - list_iter3 f x1s x2s x3s - | _ -> invalid_arg "list_iter3" - -let rec n_first n = function - [] -> [] - | x::xs -> if n > 0 then x::n_first (n-1) xs else [] - - -let rec list_casso x = function - [] -> raise Not_found - | (a,b)::abs -> if x = b then a else list_casso x abs - -let soi = string_of_int -let sof = string_of_float - -let bat_max = 13. -let bat_min = 8. - -let home = Env.paparazzi_home -let (//) = Filename.concat -let default_path_srtm = home // "data" // "srtm" -let default_path_maps = home // "data" // "maps" // "" -let path_fps = home // "conf" // "flight_plans" // "" -let fp_dtd = path_fps // "flight_plan.dtd" -let var_maps_path = home // "var" // "maps" -let _ = - ignore (Sys.command (sprintf "mkdir -p %s" var_maps_path)) -let fp_example = path_fps // "example.xml" - -let speech = ref false -let ign = ref false -let get_bdortho = ref "" -let auto_center_new_ac = ref false -let no_alarm = ref false - - - -let plugin_frame = ref None - -let say = fun s -> - if !speech then - ignore (Sys.command (sprintf "spd-say '%s'&" s)) - -(** window for the strip panel *) -let strip_scrolled = GBin.scrolled_window ~width:300 ~hpolicy: `AUTOMATIC ~vpolicy: `AUTOMATIC () -let strip_table = GPack.table ~rows: 1 ~columns: 1 ~row_spacings: 5 ~packing: (strip_scrolled#add_with_viewport) () - -(** Dummy flight plan (for map calibration) *) -let dummy_fp = fun latlong -> - Xml.Element("flight_plan", - ["lat0", sof ((Rad>>Deg)latlong.posn_lat); - "lon0", sof ((Rad>>Deg)latlong.posn_long); - "alt", "42."; - "MAX_DIST_FROM_HOME", "1000."], - [Xml.Element("waypoints", [],[])]) - - let set_georef_if_none = fun geomap wgs84 -> match geomap#georef with None -> geomap#set_georef wgs84 | Some _ -> () - - -(** Display a calibrated (XML) map *) -let display_map = fun (geomap:G.widget) xml_map -> - try - let dir = Filename.dirname xml_map in - let xml_map = Xml.parse_file xml_map in - let image = dir // ExtXml.attrib xml_map "file" in - let map_projection = Xml.attrib xml_map "projection" in - let opacity = try Some (int_of_string (Xml.attrib xml_map "opacity")) with _ -> None in - let current_projection = geomap#projection in - if map_projection <> current_projection then - GToolbox.message_box "Warning" (sprintf "You are loading a map in %s projection while the display use %s" map_projection current_projection); - - let pix_ref = fun p -> - truncate (float_attr p "x"), truncate (float_attr p "y") in - let geo_ref = fun p -> - try Latlong.of_string (Xml.attrib p "geo") with - _ -> (* Compatibility with the old UTM format *) - let utm_x = float_attr p "utm_x" - and utm_y = float_attr p "utm_y" in - let utm_zone = int_attr xml_map "utm_zone" in - let utm = {utm_x = utm_x; utm_y = utm_y; utm_zone = utm_zone } in - Latlong.of_utm WGS84 utm in - - match Xml.children xml_map with - p1::p2::_ -> - let x1y1 = pix_ref p1 - and x2y2 = pix_ref p2 - and geo1 = geo_ref p1 - and geo2 = geo_ref p2 in - - (* Take this point as a reference for the display if none currently *) - set_georef_if_none geomap geo1; - - ignore (geomap#display_pixbuf ?opacity ((x1y1),geo1) ((x2y2),geo2) (GdkPixbuf.from_file image)); - geomap#center geo1 - | _ -> failwith (sprintf "display_map: two ref points required") - with - Xml.File_not_found f -> - GToolbox.message_box "Error" (sprintf "File does not exist: %s" f) - | ExtXml.Error s -> - GToolbox.message_box "Error" (sprintf "Error in XML file: %s" s) - - - -let load_map = fun (geomap:G.widget) () -> - match GToolbox.select_file ~title:"Open Map" ~filename:(default_path_maps^"*.xml") () with - None -> () - | Some f -> display_map geomap f - - -(** Load a mission. Returns the XML window *) -let load_mission = fun ?edit color geomap xml -> - set_georef_if_none geomap (MapFP.georef_of_xml xml); - new MapFP.flight_plan ?edit ~show_moved:true geomap color fp_dtd xml - - -(** Save the given pixbuf calibrated with NW and SE corners *) -let save_map = fun geomap ?(projection=geomap#projection) pixbuf nw se -> - match GToolbox.select_file ~filename:(default_path_maps//".xml") ~title:"Save region map" () with - None -> () - | Some xml_file -> - let jpg = Filename.chop_extension xml_file ^ ".png" in - GdkPixbuf.save jpg "png" pixbuf; - let point = fun (x,y) wgs84 -> - Xml.Element ("point", ["x",soi x;"y",soi y;"geo", Latlong.string_of wgs84], []) in - let width = GdkPixbuf.get_width pixbuf - and height = GdkPixbuf.get_height pixbuf in - let points = [point (0, 0) nw; point (width, height) se] in - let xml = Xml.Element ("map", - ["file", Filename.basename jpg; - "projection", projection], - points) in - let f = open_out xml_file in - Printf.fprintf f "%s\n" (Xml.to_string_fmt xml); - close_out f - - - -(****** Creates a calibrated map from the bitmap (selected region) ***********) -let map_from_region = fun (geomap:G.widget) () -> - match geomap#region with - None -> GToolbox.message_box "Error" "Select a region (shift-left drag)" - | Some ((xw1,yw1), (xw2,yw2)) -> - let xw1, xw2 = min xw1 xw2, max xw1 xw2 - and yw1, yw2 = min yw1 yw2, max yw1 yw2 in - let (xc1, yc1) = geomap#canvas#w2c xw1 yw1 - and (xc2, yc2) = geomap#canvas#w2c xw2 yw2 in - let width = xc2-xc1 and height = yc2-yc1 in - let dest = GdkPixbuf.create width height () in - let (x0, y0) = geomap#canvas#get_scroll_offsets in - let src_x = xc1 - x0 and src_y = yc1 - y0 in - GdkPixbuf.get_from_drawable ~dest ~width ~height ~src_x ~src_y - geomap#canvas#misc#window; - let nw = geomap#of_world (xw1,yw1) - and se = geomap#of_world (xw2,yw2) in - save_map geomap dest nw se -(************ Strip handling ***********************************************) -module Strip = struct - type t = { - gauge: GRange.progress_bar ; - labels: (string * (GBin.event_box * GMisc.label)) list - } - - let labels_name = [| - [| "AP" ; "alt" ; "->" |]; [| "RC"; "climb"; "/" |]; [| "GPS"; "speed"; "" |]; - [| "settings" ; "throttle"; "" |] - |] - - let labels_print = [| - [| "AP" ; "alt" ; "->" |]; [| "RC"; "climb"; "->" |]; [| "GPS"; "speed"; "" |]; - [| "CAL" ; "throttle"; "" |] - |] - let gen_int = let i = ref (-1) in fun () -> incr i; !i - - let rows = Array.length labels_name + 1 - - - (** add a strip to the panel *) - let add (widget: GPack.table) config color select center_ac commit_moves mark = - (* number of the strip *) - let strip_number = gen_int () in - if strip_number > 1 then widget#set_rows strip_number; - - let strip_labels = ref [] in - let add_label = fun name value -> - strip_labels := (name, value) :: !strip_labels in - - let ac_name = Pprz.string_assoc "ac_name" config in - - (* frame of the strip *) - let frame = GBin.frame ~shadow_type: `IN ~packing: (widget#attach ~top: (strip_number) ~left: 0) () in - let strip = GPack.table ~rows: 2 ~columns: 2 ~col_spacings: 10 ~packing: frame#add () in - ignore (GMisc.label ~text: (ac_name) ~packing: (strip#attach ~top: 0 ~left: 0) ()); - - let plane_color = GBin.event_box ~width:10 ~height:10 ~packing:(strip#attach ~top:0 ~left: 1) () in - plane_color#coerce#misc#modify_bg [`NORMAL, `NAME color]; - ignore (plane_color#event#connect#button_press ~callback:(fun _ -> select (); true)); - let h = GPack.hbox ~packing:plane_color#add () in - let ft = GMisc.label ~text: "00:00:00" ~packing:h#add () in - ft#set_width_chars 8; - add_label ("flight_time_value") (plane_color, ft); - let block_name = GMisc.label ~text: "______" ~packing:h#add () in - add_label ("block_name_value") (plane_color, block_name); - - - (* battery and flight time *) - let pb = GRange.progress_bar ~orientation: `BOTTOM_TO_TOP ~packing:(strip#attach ~top:1 ~left:0) () in - pb#coerce#misc#modify_fg [`PRELIGHT, `NAME "green"]; - pb#coerce#misc#modify_font_by_name "sans 12"; - - let left_box = GPack.table ~rows ~columns: 6 ~col_spacings: 5 - ~packing: (strip#attach ~top: 1 ~left: 1) () in - - Array.iteri - (fun i a -> - Array.iteri - (fun j s -> - ignore (GMisc.label ~text: labels_print.(i).(j) ~justify:`RIGHT ~packing: (left_box#attach ~top: i ~left: (2*j)) ()); - let eb = GBin.event_box ~packing: (left_box#attach ~top: i ~left: (2*j+1)) () in - let lvalue = (GMisc.label ~text: "" ~justify: `RIGHT ~packing:eb#add ()) in - lvalue#set_width_chars 6; - add_label (s^"_value") (eb, lvalue); - ) a - ) labels_name; - let b = GButton.button ~label:"Center A/C" ~packing:(left_box#attach ~top:4 ~left:0 ~right:2) () in - ignore(b#connect#clicked ~callback:center_ac); - let b = GButton.button ~label:"Commit WPs" ~packing:(left_box#attach ~top:4 ~left:2 ~right:4) () in - ignore (b#connect#clicked ~callback:commit_moves); - let b = GButton.button ~label:"Mark" ~packing:(left_box#attach ~top:4 ~left:4 ~right:6) () in - ignore (b#connect#clicked ~callback:mark); - {gauge=pb ; labels= !strip_labels} - - - (** set a label *) - let set_label strip name value = - let _eb, l = List.assoc (name^"_value") strip.labels in - l#set_label value - - (** set a label *) - let set_color strip name color = - let eb, _l = List.assoc (name^"_value") strip.labels in - eb#coerce#misc#modify_bg [`NORMAL, `NAME color] - - (** set the battery *) - let set_bat strip value = - strip.gauge#set_text (sof value); - let f = (value -. bat_min) /. (bat_max -. bat_min) in - let f = max 0. (min 1. f) in - strip.gauge#set_fraction f - -end - - - -(************ Google Maps handling *****************************************) -module GM = struct - let active_http = fun x -> - Gm.no_http := not x - - (** Fill the visible background with Google tiles *) - let fill_tiles = fun geomap -> - ignore (Thread.create MapGoogle.fill_window geomap) - - let auto = ref false - let update = fun geomap -> - if !auto then fill_tiles geomap - let active_auto = fun geomap x -> - auto := x; - update geomap - -(** Creates a calibrated map from the Google tiles (selected region) *) - let map_from_tiles = fun (geomap:G.widget) () -> - match geomap#region with - None -> GToolbox.message_box "Error" "Select a region (shift-left drag)" - | Some ((xw1,yw1), (xw2,yw2)) -> - let geo1 = geomap#of_world (xw1,yw1) - and geo2 = geomap#of_world (xw2,yw2) in - let sw = { posn_lat = min geo1.posn_lat geo2.posn_lat; - posn_long = min geo1.posn_long geo2.posn_long } - and ne = { posn_lat = max geo1.posn_lat geo2.posn_lat; - posn_long = max geo1.posn_long geo2.posn_long } in - let pix = MapGoogle.pixbuf sw ne in - let nw = { posn_lat = ne.posn_lat; posn_long = sw.posn_long } - and se = { posn_lat = sw.posn_lat; posn_long = ne.posn_long } in - save_map geomap ~projection:"Mercator" pix nw se -end (* GM module *) - -let bdortho_size = 400 -let bdortho_store = Hashtbl.create 97 -let display_bdortho = fun (geomap:G.widget) wgs84 () -> - let r = bdortho_size / 2 in - let { lbt_x = lx; lbt_y = ly} = lambertIIe_of wgs84 in - let lx = lx + r and ly = ly + 2 in - let lx = lx - (lx mod bdortho_size) - and ly = ly - (ly mod bdortho_size) in - let f = sprintf "ortho_%d_%d_%d.jpg" lx ly r in - let f = var_maps_path // f in - if not (Hashtbl.mem bdortho_store f) then begin - Hashtbl.add bdortho_store f true; - let display = fun _ -> - let nw = of_lambertIIe {lbt_x = lx - r; lbt_y = ly + r} - and se = of_lambertIIe {lbt_x = lx + r; lbt_y = ly - r} in - ignore (geomap#display_pixbuf ((0,0), nw) ((bdortho_size, bdortho_size), se) (GdkPixbuf.from_file f)); - - in - if Sys.file_exists f then - display f - else - ignore (Thread.create - (fun f -> - let c = sprintf "%s %d %d %d %s" !get_bdortho lx ly r f in - ignore (Sys.command c); - display f) - f) - end - - -let fill_ortho = fun (geomap:MapCanvas.widget) -> - (** First estimate the coverage of the window *) - let width_c, height_c = Gdk.Drawable.get_size geomap#canvas#misc#window - and (xc0, yc0) = geomap#canvas#get_scroll_offsets in - let (xw0, yw0) = geomap#window_to_world (float xc0) (float (yc0+height_c)) - and (xw1, yw1) = geomap#window_to_world (float (xc0+width_c)) (float yc0) in - let sw = geomap#of_world (xw0, yw0) - and ne = geomap#of_world (xw1, yw1) in - let lbt2e_sw = lambertIIe_of sw - and lbt2e_ne = lambertIIe_of ne in - let w = lbt2e_ne.lbt_x - lbt2e_sw.lbt_x - and h = lbt2e_ne.lbt_y - lbt2e_sw.lbt_y in - for i = 0 to w / bdortho_size + 2 do - let lbt_x = lbt2e_sw.lbt_x + bdortho_size * i in - for j = 0 to h / bdortho_size + 2 do - let lbt_y = lbt2e_sw.lbt_y + bdortho_size * j in - let geo = of_lambertIIe {lbt_x = lbt_x; lbt_y = lbt_y } in - display_bdortho geomap geo () - done - done - - -(***************** Editing ONE (single) flight plan **************************) -module Edit = struct - let current_fp = ref None - - (** Wrapper checking there is currently no flight plan loaded *) - let if_none = fun f -> - match !current_fp with - Some _ -> - GToolbox.message_box "Error" "Only one editable flight plan at a time" - | None -> - f () - - - let close_fp = fun () -> - match !current_fp with - None -> () (* Nothing to close *) - | Some (_fp, _filename, window) -> - window#destroy (); - current_fp := None - - let load_xml_fp = fun geomap accel_group ?(xml_file=path_fps) xml -> - set_georef_if_none geomap (MapFP.georef_of_xml xml); - let fp = new MapFP.flight_plan ~show_moved:false geomap "red" fp_dtd xml in - let window = GWindow.window () in - window#set_default_size ~width:700 ~height:250; - window#add fp#window; - window#add_accel_group accel_group; - window#show(); - ignore (window#connect#destroy ~callback:close_fp); - current_fp := Some (fp,xml_file, window); - fp - - let labelled_entry = fun ?width_chars text value h -> - let _ = GMisc.label ~text ~packing:h#add () in - GEdit.entry ?width_chars ~text:value ~packing:h#add () - - let new_fp = fun geomap accel_group () -> - if_none (fun () -> - let dialog = GWindow.window ~border_width:10 ~title:"New flight plan" () in - let dvbx = GPack.box `VERTICAL ~packing:dialog#add () in - let h = GPack.hbox ~packing:dvbx#pack () in - let default_latlong = - match geomap#georef with - None -> "WGS84 37.21098 -113.45678" - | Some geo -> Latlong.string_of geo in - let latlong = labelled_entry ~width_chars:25 "Geographic Reference" default_latlong h in - let alt0 = labelled_entry ~width_chars:4 "Ground Alt" "380" h in - let h = GPack.hbox ~packing:dvbx#pack () in - let alt = labelled_entry ~width_chars:4 "Default Alt" "430" h in - let qfu = labelled_entry ~width_chars:4 "QFU" "270" h in - let mdfh = labelled_entry ~width_chars:4 "Max distance from HOME" "500" h in - - let h = GPack.hbox ~packing:dvbx#pack () in - let name = labelled_entry "Name" "Test flight" h in - - let h = GPack.hbox ~packing:dvbx#pack () in - let cancel = GButton.button ~stock:`CANCEL ~packing: h#add () in - ignore(cancel#connect#clicked ~callback:dialog#destroy); - - let createfp = GButton.button ~stock:`OK ~packing: h#add () in - createfp#grab_default (); - ignore(createfp#connect#clicked ~callback: - begin fun _ -> - let xml = Xml.parse_file fp_example in - let s = ExtXml.subst_attrib in - let wgs84 = Latlong.of_string latlong#text in - let xml = s "lat0" (deg_string_of_rad wgs84.posn_lat) xml in - let xml = s "lon0" (deg_string_of_rad wgs84.posn_long) xml in - let xml = s "ground_alt" alt0#text xml in - let xml = s "qfu" qfu#text xml in - let xml = s "alt" alt#text xml in - let xml = s "max_dist_from_home" mdfh#text xml in - let xml = s "name" name#text xml in - ignore (load_xml_fp geomap accel_group xml); - dialog#destroy () - end); - dialog#show ()) - - -(** Loading a flight plan for edition *) - let load_fp = fun geomap accel_group () -> - if_none (fun () -> - match GToolbox.select_file ~title:"Open flight plan" ~filename:(path_fps^"*.xml") () with - None -> () - | Some xml_file -> - try - let xml = Xml.parse_file xml_file in - ignore (load_xml_fp geomap accel_group ~xml_file xml) - with - Dtd.Check_error(e) -> - let m = sprintf "Error while loading %s:\n%s" xml_file (Dtd.check_error e) in - GToolbox.message_box "Error" m) - - let create_wp = fun geo -> - match !current_fp with - None -> - GToolbox.message_box "Error" "Load a flight plan first"; - failwith "create_wp" - | Some (fp,_,_) -> - fp#add_waypoint geo - - - let save_fp = fun () -> - match !current_fp with - None -> () (* Nothing to save *) - | Some (fp, filename,_) -> - match GToolbox.select_file ~title:"Save Flight Plan" ~filename () with - None -> () - | Some file -> - let f = open_out file in - fprintf f "%s\n" (Xml.to_string_fmt fp#xml); - close_out f - - let ref_point_of_waypoint = fun xml -> - Xml.Element("point", ["x",Xml.attrib xml "x"; - "y",Xml.attrib xml "y"; - "geo", Xml.attrib xml "name"],[]) - - -(** Calibration of chosen image (requires a dummy flight plan) *) - let calibrate_map = fun (geomap:G.widget) accel_group () -> - match !current_fp with - | Some (_fp,_,_) -> GToolbox.message_box "Error" "Close current flight plan before calibration" - | None -> - match GToolbox.select_file ~filename:default_path_maps ~title:"Open Image" () with - None -> () - | Some image -> - (** Displaying the image in the NW corner *) - let pixbuf = GdkPixbuf.from_file image in - let pix = GnoCanvas.pixbuf ~pixbuf ~props:[`ANCHOR `NW] geomap#canvas#root in - let (x0, y0) = geomap#canvas#get_scroll_offsets in - let (x,y) = geomap#canvas#window_to_world (float x0) (float y0) in - pix#move x y; - - (** Open a dummy flight plan *) - let dummy_georef = - match geomap#georef with - None -> {posn_lat = (Deg>>Rad)43.; posn_long = (Deg>>Rad)1. } - | Some geo -> geo in - let fp_xml = dummy_fp dummy_georef in - let fp = load_xml_fp geomap accel_group fp_xml in - - (** Dialog to finish calibration *) - let dialog = GWindow.window ~border_width:10 ~title:"Map calibration" () in - let v = GPack.vbox ~packing:dialog#add () in - let _ = GMisc.label ~text:"Choose 2 (or more) waypoints (Ctrl-Left)\nRename the waypoints with their geographic coordinates\nFor example: 'WGS84 43.123456 1.234567' or 'UTM 530134 3987652 12' or 'LBT2e 123456 543210'\nClick the button below to save the XML result file\n" ~packing:v#add () in - let h = GPack.hbox ~packing:v#pack () in - let cancel = GButton.button ~stock:`CLOSE ~packing:h#add () in - let cal = GButton.button ~stock:`OK ~packing:h#add () in - let destroy = fun () -> - dialog#destroy (); - close_fp (); - pix#destroy () in - ignore(cancel#connect#clicked ~callback:destroy); - ignore(cal#connect#clicked ~callback:(fun _ -> - let points = List.map XmlEdit.xml_of_node fp#waypoints in - let points = List.map ref_point_of_waypoint points in - let xml = Xml.Element ("map", - ["file", Filename.basename image; - "projection", geomap#projection], - points) in - match GToolbox.select_file ~filename:(default_path_maps//".xml") ~title:"Save calibrated map" () with - None -> () - | Some xml_file -> - let f = open_out xml_file in - Printf.fprintf f "%s\n" (Xml.to_string_fmt xml); - close_out f)); - cal#grab_default (); - dialog#show () - - let radius = ref 30. - let path = ref (ref []) - let cur_arc = ref None - and cur_seg = ref None - and cur_f = ref { G2D.x2D = 0.; y2D = 0. } - - let set_segment = fun s p1 p2 -> - let x1 = p1.G2D.x2D and y1 = p1.G2D.y2D - and x2 = p2.G2D.x2D and y2 = p2.G2D.y2D in - s#set [`POINTS [|x1;y1;x2;y2|]] - - let arc_from_points = fun (geomap:G.widget) c l f s -> - let cl = G2D.vect_make c l - and cf = G2D.vect_make c f in - let pol_cl = G2D.cart2polar cl in - let al = pol_cl.G2D.theta2D - and af = (G2D.cart2polar cf).G2D.theta2D in - let xc = c.G2D.x2D and yc = c.G2D.y2D in - let (a1, a2) = if s > 0. then (al, af) else (af, al) in - geomap#arc ~nb_points:10 ~fill_color:"blue" ~width:2 (xc,yc) pol_cl.G2D.r2D a1 a2 - - (** Update path after waypoint move *) - let update_path = fun (geomap:G.widget) path waypoint -> - let n = waypoint#name in - let rec loop = function - [] -> failwith (sprintf "update_path: %s not found" n) - | [p] -> [p] - | (wp, Some arc, Some seg, _f, radius)::wps -> - let new_wps = if wp#name = waypoint#name then wps else loop wps in - begin - match new_wps with - [] -> failwith "unreachable" - | (wp1, _arc1, _seg1, f1, _r1)::new_wps' -> (* Previous *) - let wp1_2D = geomap#pt2D_of wp1#pos - and wp_2D = geomap#pt2D_of wp#pos in - match new_wps' with - [] -> (* wp is the second point: simple segment *) - set_segment seg wp1_2D wp_2D; - (wp, Some arc, Some seg, wp1_2D, radius)::new_wps - | _ -> (* At least 2 points before *) - let (c, f, s) = G2D.arc_segment f1 wp1_2D wp_2D radius in - set_segment seg f wp_2D; - arc#destroy (); - let new_arc = arc_from_points geomap c wp1_2D f s in - (wp, Some new_arc, Some seg, f, radius)::new_wps - end - | _ -> failwith "update_path" in - path := loop !path - - - - let path_button = fun (geomap:G.widget) (xw, yw) -> - let geo = geomap#of_world (xw, yw) in - let wp = create_wp geo in - let cur_path = !path in - wp#connect (fun () -> update_path geomap cur_path wp); - if ! !path = [] then - cur_f := {G2D.x2D=xw; G2D.y2D=yw}; - cur_path := (wp, !cur_arc, !cur_seg, !cur_f, !radius) :: ! cur_path; - cur_arc := Some (GnoCanvas.line geomap#canvas#root); - cur_seg := Some (geomap#segment ~fill_color:"blue" ~width:2 geo geo) - - let path_notify (geomap:G.widget) (xw, yw) = - match ! !path with - [] -> false (** Empty path: nothing to do *) - | (wp1, _, _, ll,_) :: p -> (** Last is wp1 *) - begin - match !cur_seg with - None -> failwith "path_notify" - | Some segment -> - let l = geomap#pt2D_of wp1#pos - and cur_2D = {G2D.x2D = xw; y2D = yw } in - match p with - [] -> (** Only 1 point in the current path:add a simple segment*) - set_segment segment l cur_2D; - | _::_ -> (** Already 2 points: add an arc and a segment *) - match !cur_arc with - None -> failwith "path_notify" - | Some arc -> - arc#destroy (); - let (c, f, s) = G2D.arc_segment ll l cur_2D !radius in - set_segment segment f cur_2D; - let arc = arc_from_points geomap c l f s in - cur_arc := Some arc; - cur_f := f - end; - true - - - let path_close = fun () -> - let destroy = fun ref -> - match !ref with - None -> () - | Some s -> s#destroy (); ref := None in - destroy cur_arc; - destroy cur_seg; - begin - match !current_fp with - None -> () - | Some (fp, _,_) -> - fp#insert_path (List.map (fun (wp,_,_,_,r) -> (wp, r)) (List.rev ! !path)); - end; - path := ref [] - - let path_change_radius = function - `UP -> radius := !radius *. 1.1 - | `DOWN -> radius := !radius /. 1.1 - | _ -> () -end (** Edit module *) - - -(******* Mouse motion handling **********************************************) -let motion_notify = fun (geomap:G.widget) ev -> - let xc = GdkEvent.Motion.x ev - and yc = GdkEvent.Motion.y ev in - let xwyw = geomap#window_to_world xc yc in - Edit.path_notify geomap xwyw - - -(******* Mouse wheel handling ***********************************************) -let any_event = fun (geomap:G.widget) ev -> - match GdkEvent.get_type ev with - `SCROLL -> - let state = GdkEvent.Scroll.state (GdkEvent.Scroll.cast ev) in - if Gdk.Convert.test_modifier `CONTROL state && Gdk.Convert.test_modifier `SHIFT state then - let scroll_event = GdkEvent.Scroll.cast ev in - Edit.path_change_radius (GdkEvent.Scroll.direction scroll_event); - let xc = GdkEvent.Scroll.x scroll_event - and yc = GdkEvent.Scroll.y scroll_event in - let xwyw = geomap#window_to_world xc yc in - Edit.path_notify geomap xwyw - else - false - | _ -> - false - - - -(******* Mouse buttons handling **********************************************) -let button_press = fun (geomap:G.widget) ev -> - let state = GdkEvent.Button.state ev in - if GdkEvent.Button.button ev = 3 && Gdk.Convert.test_modifier `CONTROL state && Gdk.Convert.test_modifier `SHIFT state then begin - Edit.path_close (); - true - end else if GdkEvent.Button.button ev = 3 then begin - (** Display a tile from Google Maps or IGN *) - let xc = GdkEvent.Button.x ev - and yc = GdkEvent.Button.y ev in - let (xw,yw) = geomap#window_to_world xc yc in - - let thread = fun f x -> ignore (Thread.create f x) in - let wgs84 = geomap#of_world (xw,yw) in - let display_ign = fun () -> - thread (MapIGN.display_tile geomap) wgs84 - and display_gm = fun () -> - thread (fun () -> - try ignore (MapGoogle.display_tile geomap wgs84) with - Gm.Not_available -> ()) - () in - - let m = if !ign then [`I ("Load IGN tile", display_ign)] else [] in - let m = - if !get_bdortho <> "" then - (`I ("Load BDORTHO", display_bdortho geomap wgs84))::m - else - m in - - GToolbox.popup_menu ~entries:([`I ("Load Google tile", display_gm)]@m) - ~button:3 ~time:(Int32.of_int 00); - true; - end else if GdkEvent.Button.button ev = 1 && Gdk.Convert.test_modifier `CONTROL state then - if Gdk.Convert.test_modifier `SHIFT state then begin - let xc = GdkEvent.Button.x ev in - let yc = GdkEvent.Button.y ev in - let xyw = geomap#canvas#window_to_world xc yc in - Edit.path_button geomap xyw; - true - end else begin - let xc = GdkEvent.Button.x ev in - let yc = GdkEvent.Button.y ev in - let xyw = geomap#canvas#window_to_world xc yc in - let geo = geomap#of_world xyw in - ignore (Edit.create_wp geo); - true - end else - false - - -(******* Real time handling of flying A/Cs ***********************************) -module Live = struct - module Tele_Pprz = Pprz.Messages(struct let name = "telemetry" end) - module Ground_Pprz = Pprz.Messages(struct let name = "ground" end) - module Alert_Pprz = Pprz.Messages(struct let name = "alert" end) - - type color = string - type aircraft = { - ac_name : string; - config : Pprz.values; - track : MapTrack.track; - color: color; - fp_group : MapFP.flight_plan; - fp : Xml.xml; - blocks : (int * string) list; - mutable last_ap_mode : string; - mutable last_stage : int * int; - ir_page : Pages.infrared; - gps_page : Pages.gps; - pfd_page : Pages.pfd; - misc_page : Pages.misc; - settings_page : Pages.settings option; - strip : Strip.t; - mutable first_pos : bool - } - - let live_aircrafts = Hashtbl.create 3 - let get_ac = fun vs -> - let ac_id = Pprz.string_assoc "ac_id" vs in - Hashtbl.find live_aircrafts ac_id - - let aircraft_pos_msg = fun track wgs84 heading altitude speed climb -> - let h = - try - Srtm.of_wgs84 wgs84 - with - _ -> truncate altitude in - track#move_icon wgs84 heading altitude (float_of_int h) speed climb - - let carrot_pos_msg = fun track wgs84 -> - track#move_carrot wgs84 - - let cam_pos_msg = fun track wgs84 target_wgs84 -> - track#move_cam wgs84 target_wgs84 - - let circle_status_msg = fun track wgs84 radius -> - track#draw_circle wgs84 radius - - let segment_status_msg = fun track geo1 geo2 -> - track#draw_segment geo1 geo2 - - let survey_status_msg = fun track geo1 geo2 -> - track#draw_zone geo1 geo2 - - let ap_status_msg = fun track flight_time -> - track#update_ap_status flight_time - - - let show_mission = fun ac on_off -> - let a = Hashtbl.find live_aircrafts ac in - if on_off then - a.fp_group#show () - else - a.fp_group#hide () - - let resize_track = fun ac track -> - match - GToolbox.input_string ~text:(soi track#size) ~title:ac "Track size" - with - None -> () - | Some s -> track#resize (int_of_string s) - - let commit_changes = fun ac -> - let a = Hashtbl.find live_aircrafts ac in - List.iter - (fun w -> - let (i, w) = a.fp_group#index w in - if w#moved then - let wgs84 = w#pos in - let vs = ["ac_id", Pprz.String ac; - "wp_id", Pprz.Int i; - "lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat); - "long", Pprz.Float ((Rad>>Deg)wgs84.posn_long); - "alt", Pprz.Float w#alt - ] in - Ground_Pprz.message_send "map2d" "MOVE_WAYPOINT" vs) - a.fp_group#waypoints - - let send_event = fun ac e -> - Ground_Pprz.message_send "map2d" "SEND_EVENT" - ["ac_id", Pprz.String ac; "event_id", Pprz.Int e] - - let center = fun geomap track () -> - match track#last with - None -> () - | Some geo -> - geomap#center geo - - - let blocks_of_stages = fun stages -> - let blocks = ref [] in - List.iter (fun x -> - let name = ExtXml.attrib x "block_name" - and id = int_attr x "block" in - if not (List.mem_assoc id !blocks) then - blocks := (id, name) :: !blocks) - (Xml.children stages); - List.sort compare !blocks - - let jump_to_block = fun ac_id id -> - Ground_Pprz.message_send "map2d" "JUMP_TO_BLOCK" - ["ac_id", Pprz.String ac_id; "block_id", Pprz.Int id] - - let menu_entry_of_block = fun ac_id (id, name) -> - let send_msg = fun () -> jump_to_block ac_id id in - `I (name, send_msg) - - let reset_waypoints = fun fp () -> - List.iter (fun w -> - let (_i, w) = fp#index w in - w#reset_moved ()) - fp#waypoints - - let icon = ref None - let show_snapshot = fun (geomap:G.widget) geo_FL geo_BR point pixbuf name ev -> - match ev with - | `BUTTON_PRESS ev -> - let image = GMisc.image ~pixbuf () in - let icon = image#coerce in - begin - match GToolbox.question_box ~title:name ~buttons:["Delete"; "Close"] ~icon "" with - 1 -> - point#destroy () - | _ -> () - end; - true - | `LEAVE_NOTIFY ev -> - begin - match !icon with - None -> () - | Some i -> i#destroy () - end; - false - | `ENTER_NOTIFY ev -> - let w = GdkPixbuf.get_width pixbuf - and h = GdkPixbuf.get_height pixbuf in - icon := Some (geomap#display_pixbuf ((0,0), geo_FL) ((w,h), geo_BR) pixbuf); - false - - | _ -> false - - - let mark = fun (geomap:G.widget) ac_id track -> - let i = ref 1 in fun () -> - match track#last with - Some geo -> - begin - let group = geomap#background in - let point = geomap#circle ~group ~fill_color:"blue" geo 5. in - point#raise_to_top (); - let lat = (Rad>>Deg)geo.posn_lat - and long = (Rad>>Deg)geo.posn_long in - Tele_Pprz.message_send ac_id "MARK" - ["ac_id", Pprz.String ac_id; - "lat", Pprz.Float lat; - "long", Pprz.Float long]; - match !plugin_frame with - None -> () - | Some frame -> - let width, height = Gdk.Drawable.get_size frame#misc#window in - let dest = GdkPixbuf.create width height() in - GdkPixbuf.get_from_drawable ~dest ~width ~height frame#misc#window; - let name = sprintf "Snapshot-%s-%d_%f_%f_%f.png" ac_id !i lat long (track#last_heading) in - let png = sprintf "%s/var/logs/%s" home name in - GdkPixbuf.save png "png" dest; - incr i; - - (* Computing the footprint: front_left and back_right *) - let cam_aperture = 2.4/.1.9 in (* width over distance FIXME *) - let alt = track#last_altitude -. float (Srtm.of_wgs84 geo) in - let width = cam_aperture *. alt in - let height = width *. 3. /. 4. in - let utm = utm_of WGS84 geo in - let a = (Deg>>Rad)track#last_heading in - let (xfl,yfl) = rotate a (-.width/.2., height/.2.) - and (xbr,ybr) = rotate a (width/.2., -.height/.2.) in - let geo_FL = of_utm WGS84 (utm_add utm (xfl,yfl)) - and geo_BR = of_utm WGS84 (utm_add utm (xbr,ybr)) in - ignore (point#connect#event (show_snapshot geomap geo_FL geo_BR point dest name)) - end - | None -> () - - - - - let create_ac = fun (geomap:G.widget) (acs_notebook:GPack.notebook) ac_id config -> - let color = Pprz.string_assoc "default_gui_color" config - and name = Pprz.string_assoc "ac_name" config in - - (** Get the flight plan **) - let fp_url = Pprz.string_assoc "flight_plan" config in - let fp_file = Http.file_of_url fp_url in - let fp_xml_dump = Xml.parse_file fp_file in - let stages = ExtXml.child fp_xml_dump "stages" in - let blocks = blocks_of_stages stages in - - let label_box = GBin.event_box () in - let label = GPack.hbox ~packing:label_box#add ~spacing:3 () in - let eb = GBin.event_box ~width:10 ~height:10 ~packing:label#pack () in - eb#coerce#misc#modify_bg [`NORMAL, `NAME color]; - let _ac_label = GMisc.label ~text:name ~packing:label#pack () in - - let ac_mi = GMenu.image_menu_item ~image:label_box ~packing:geomap#menubar#append () in - let ac_menu = GMenu.menu () in - ac_mi#set_submenu ac_menu; - let ac_menu_fact = new GMenu.factory ac_menu in - let fp = ac_menu_fact#add_check_item "Fligh Plan" ~active:true in - ignore (fp#connect#toggled (fun () -> show_mission ac_id fp#active)); - - let track = new MapTrack.track ~name ~color:color geomap in - - let center_ac = center geomap track in - ignore (ac_menu_fact#add_item "Center A/C" ~callback:center_ac); - - ignore (ac_menu_fact#add_item "Clear Track" ~callback:(fun () -> track#clear_map2D)); - ignore (ac_menu_fact#add_item "Resize Track" ~callback:(fun () -> resize_track ac_id track)); - let reset_wp_menu = ac_menu_fact#add_item "Reset Waypoints" in - - let jump_block_entries = List.map (menu_entry_of_block ac_id) blocks in - - let commit_moves = fun () -> - commit_changes ac_id in - let sm = ac_menu_fact#add_submenu "Datalink" in - let dl_menu = [ - `M ("Jump to block", jump_block_entries); - `I ("Commit Moves", commit_moves); - `I ("Event 1", (fun () -> send_event ac_id 1)); - `I ("Event 2", (fun () -> send_event ac_id 2))] in - - GToolbox.build_menu sm ~entries:dl_menu; - - let cam = ac_menu_fact#add_check_item "Cam Display" ~active:false in - ignore (cam#connect#toggled (fun () -> track#set_cam_state cam#active)); - let params = ac_menu_fact#add_check_item "flight param. display" ~active:false in - ignore (params#connect#toggled (fun () -> track#set_params_state params#active)); - - (** Build the XML flight plan, connect then "jump_to_block" *) - let fp_xml = ExtXml.child fp_xml_dump "flight_plan" in - let fp = load_mission ~edit:false color geomap fp_xml in - fp#connect_activated (fun node -> - if XmlEdit.tag node = "block" then - let block = XmlEdit.attrib node "name" in - let id = list_casso block blocks in - jump_to_block ac_id id); - ignore (reset_wp_menu#connect#activate (reset_waypoints fp)); - - - (** Add a new tab in the A/Cs notebook, with a colored label *) - let eb = GBin.event_box () in - let _label = GMisc.label ~text:name ~packing:eb#add () in - eb#coerce#misc#modify_bg [`NORMAL, `NAME color;`ACTIVE, `NAME color]; - - (** Put a notebook for this A/C *) - let ac_frame = GBin.frame ~packing:(acs_notebook#append_page ~tab_label:eb#coerce) () in - let ac_notebook = GPack.notebook ~packing: ac_frame#add () in - let visible = fun w -> - ac_notebook#page_num w#coerce = ac_notebook#current_page in - - (** Insert the flight plan tab *) - let fp_label = GMisc.label ~text: "Flight Plan" () in - (ac_notebook:GPack.notebook)#append_page ~tab_label:fp_label#coerce fp#window#coerce; - - let infrared_label = GMisc.label ~text: "Infrared" () in - let infrared_frame = GBin.frame ~shadow_type: `NONE - ~packing: (ac_notebook#append_page ~tab_label: infrared_label#coerce) () in - let ir_page = new Pages.infrared infrared_frame in - - let gps_label = GMisc.label ~text: "GPS" () in - let gps_frame = GBin.frame ~shadow_type: `NONE - ~packing: (ac_notebook#append_page ~tab_label: gps_label#coerce) () in - let gps_page = new Pages.gps ~visible gps_frame in - - let pfd_label = GMisc.label ~text: "PFD" () in - let pfd_frame = GBin.frame ~shadow_type: `NONE - ~packing: (ac_notebook#append_page ~tab_label: pfd_label#coerce) () in - let pfd_page = new Pages.pfd pfd_frame - and _pfd_page_num = ac_notebook#page_num pfd_frame#coerce in - - let misc_label = GMisc.label ~text: "Misc" () in - let misc_frame = GBin.frame ~shadow_type: `NONE - ~packing: (ac_notebook#append_page ~tab_label:misc_label#coerce) () in - let misc_page = new Pages.misc ~packing:misc_frame#add misc_frame in - - let settings_page = - try - let xml_settings = Xml.children (ExtXml.child fp_xml "dl_settings") in - let callback = fun idx value -> - let vs = ["ac_id", Pprz.String ac_id; "index", Pprz.Int idx;"value", Pprz.Float value] in - Ground_Pprz.message_send "dl" "DL_SETTING" vs in - let settings_tab = new Pages.settings ~visible xml_settings callback in - let tab_label = (GMisc.label ~text:"Settings" ())#coerce in - ac_notebook#append_page ~tab_label settings_tab#widget; - Some settings_tab - with - _ -> None in - - (** Add a strip and connect it to the A/C notebook *) - let select_this_tab = - let n = acs_notebook#page_num ac_frame#coerce in - fun () -> acs_notebook#goto_page n in - let strip = Strip.add strip_table config color select_this_tab center_ac commit_moves (mark geomap ac_id track) in - - - Hashtbl.add live_aircrafts ac_id { track = track; color = color; - fp_group = fp ; config = config ; - fp = fp_xml; ac_name = name; - blocks = blocks; last_ap_mode= ""; - last_stage = (-1,-1); - ir_page = ir_page; - gps_page = gps_page; - pfd_page = pfd_page; - misc_page = misc_page; - settings_page = settings_page; - strip = strip; first_pos = true - } - - (** Bind to message while catching all the esceptions of the callback *) - let safe_bind = fun msg cb -> - let safe_cb = fun sender vs -> - try cb sender vs with _ -> () in - ignore (Ground_Pprz.message_bind msg safe_cb) - - let alert_bind = fun msg cb -> - let safe_cb = fun sender vs -> - try cb sender vs with _ -> () in - ignore (Alert_Pprz.message_bind msg safe_cb) - - let ask_config = fun geomap fp_notebook ac -> - let get_config = fun _sender values -> - create_ac geomap fp_notebook ac values - in - Ground_Pprz.message_req "map2d" "CONFIG" ["ac_id", Pprz.String ac] get_config - - - - let one_new_ac = fun (geomap:G.widget) fp_notebook ac -> - if not (Hashtbl.mem live_aircrafts ac) then begin - ask_config geomap fp_notebook ac - end - - let get_wind_msg = fun _sender vs -> - let ac = get_ac vs in - let value = fun field_name -> sprintf "%.1f" (Pprz.float_assoc field_name vs) in - ac.misc_page#set_wind_speed (value "wspeed"); - ac.misc_page#set_wind_dir (value "dir") - - let get_fbw_msg = fun _sender vs -> - let ac = get_ac vs in - let status = Pprz.string_assoc "rc_status" vs in - Strip.set_label ac.strip "RC" status; - Strip.set_color ac.strip "RC" - (match status with - "LOST" -> "orange" - | "REALLY_LOST" -> "red" - | _ -> "white") - - - - let get_engine_status_msg = fun _sender vs -> - let ac = get_ac vs in - Strip.set_label ac.strip "throttle" - (string_of_float (Pprz.float_assoc "throttle" vs)); - Strip.set_bat ac.strip (Pprz.float_assoc "bat" vs) - - let get_if_calib_msg = fun _sender vs -> - let ac = get_ac vs in - Strip.set_label ac.strip "settings" (Pprz.string_assoc "if_mode" vs) - - let listen_wind_msg = fun () -> - safe_bind "WIND" get_wind_msg - - let listen_fbw_msg = fun () -> - safe_bind "FLY_BY_WIRE" get_fbw_msg - - let listen_engine_status_msg = fun () -> - safe_bind "ENGINE_STATUS" get_engine_status_msg - - let listen_if_calib_msg = fun () -> - safe_bind "INFLIGH_CALIB" get_if_calib_msg - - let list_separator = Str.regexp "," - - let aircrafts_msg = fun (geomap:G.widget) fp_notebook acs -> - let acs = Pprz.string_assoc "ac_list" acs in - let acs = Str.split list_separator acs in - List.iter (one_new_ac geomap fp_notebook) acs - - - let listen_dl_value = fun () -> - let get_dl_value = fun _sender vs -> - let ac_id = Pprz.string_assoc "ac_id" vs in - let ac = Hashtbl.find live_aircrafts ac_id in - match ac.settings_page with - Some settings -> - let csv = Pprz.string_assoc "values" vs in - let values = Array.of_list (Str.split list_separator csv) in - for i = 0 to min (Array.length values) settings#length - 1 do - settings#set i (float_of_string values.(i)) - done - | None -> () in - safe_bind "DL_VALUES" get_dl_value - - - let highlight_fp = fun ac b s -> - if (b, s) <> ac.last_stage then begin - ac.last_stage <- (b, s); - ac.fp_group#highlight_stage b s - end - - - let listen_flight_params = fun geomap -> - let get_fp = fun _sender vs -> - let ac = get_ac vs in - let pfd_page = ac.pfd_page in - - pfd_page#set_attitude (Pprz.float_assoc "roll" vs) (Pprz.float_assoc "pitch" vs); - pfd_page#set_alt (Pprz.float_assoc "alt" vs); - pfd_page#set_climb (Pprz.float_assoc "climb" vs); - pfd_page#set_speed (Pprz.float_assoc "speed" vs); - - let a = fun s -> Pprz.float_assoc s vs in - let wgs84 = { posn_lat = (Deg>>Rad)(a "lat"); posn_long = (Deg>>Rad)(a "long") } in - aircraft_pos_msg ac.track wgs84 (a "course") (a "alt") (a "speed") (a "climb"); - - if !auto_center_new_ac && ac.first_pos then begin - center geomap ac.track (); - ac.first_pos <- false - end; - - let set_label lbl_name field_name = - let s = - if (a field_name) < 0. - then - "- "^(Printf.sprintf "%.1f" (abs_float (a field_name))) - else - Printf.sprintf "%.1f" (a field_name) - in - Strip.set_label ac.strip lbl_name s - in - set_label "alt" "alt"; - set_label "speed" "speed"; - set_label "climb" "climb" - in - safe_bind "FLIGHT_PARAM" get_fp; - - let get_ns = fun _sender vs -> - let ac = get_ac vs in - let a = fun s -> Pprz.float_assoc s vs in - let wgs84 = { posn_lat = (Deg>>Rad)(a "target_lat"); posn_long = (Deg>>Rad)(a "target_long") } in - carrot_pos_msg ac.track wgs84; - let cur_block = Pprz.int_assoc "cur_block" vs - and cur_stage = Pprz.int_assoc "cur_stage" vs in - let b = List.assoc cur_block ac.blocks in - let b = String.sub b 0 (min 10 (String.length b)) in - highlight_fp ac cur_block cur_stage; - let set_label = fun l f -> - Strip.set_label ac.strip l (Printf.sprintf "%.1f" (Pprz.float_assoc f vs)) in - set_label "->" "target_alt"; - set_label "/" "target_climb"; - Strip.set_label ac.strip "block_name" b - in - safe_bind "NAV_STATUS" get_ns; - - let get_cam_status = fun _sender vs -> - let ac_id = Pprz.string_assoc "ac_id" vs in - let ac = Hashtbl.find live_aircrafts ac_id in - let a = fun s -> Pprz.float_assoc s vs in - let wgs84 = { posn_lat = (Deg>>Rad)(a "cam_lat"); posn_long = (Deg>>Rad)(a "cam_long") } - and target_wgs84 = { posn_lat = (Deg>>Rad)(a "cam_target_lat"); posn_long = (Deg>>Rad)(a "cam_target_long") } in - - cam_pos_msg ac.track wgs84 target_wgs84 - in - safe_bind "CAM_STATUS" get_cam_status; - - let get_circle_status = fun _sender vs -> - let ac = get_ac vs in - let a = fun s -> Pprz.float_assoc s vs in - let wgs84 = { posn_lat = (Deg>>Rad)(a "circle_lat"); posn_long = (Deg>>Rad)(a "circle_long") } in - circle_status_msg ac.track wgs84 (float_of_string (Pprz.string_assoc "radius" vs)) - in - safe_bind "CIRCLE_STATUS" get_circle_status; - - let get_segment_status = fun _sender vs -> - let ac_id = Pprz.string_assoc "ac_id" vs in - let ac = Hashtbl.find live_aircrafts ac_id in - let a = fun s -> Pprz.float_assoc s vs in - let geo1 = { posn_lat = (Deg>>Rad)(a "segment1_lat"); posn_long = (Deg>>Rad)(a "segment1_long") } - and geo2 = { posn_lat = (Deg>>Rad)(a "segment2_lat"); posn_long = (Deg>>Rad)(a "segment2_long") } in - segment_status_msg ac.track geo1 geo2 - in - safe_bind "SEGMENT_STATUS" get_segment_status; - - - let get_survey_status = fun _sender vs -> - let ac = get_ac vs in - let a = fun s -> Pprz.float_assoc s vs in - let geo1 = { posn_lat = (Deg>>Rad)(a "south_lat"); posn_long = (Deg>>Rad)(a "west_long") } - and geo2 = { posn_lat = (Deg>>Rad)(a "north_lat"); posn_long = (Deg>>Rad)(a "east_long") } in - survey_status_msg ac.track geo1 geo2 - in - safe_bind "SURVEY_STATUS" get_survey_status; - - - let get_ap_status = fun _sender vs -> - let ac = get_ac vs in - ap_status_msg ac.track ( float_of_int (Pprz.int32_assoc "flight_time" vs )); - let ap_mode = Pprz.string_assoc "ap_mode" vs in - if ap_mode <> ac.last_ap_mode then begin - say (sprintf "%s, %s" ac.ac_name ap_mode); - ac.last_ap_mode <- ap_mode - end; - Strip.set_label ac.strip "AP" (Pprz.string_assoc "ap_mode" vs); - Strip.set_color ac.strip "AP" (if ap_mode="HOME" then "red" else "white"); - let gps_mode = Pprz.string_assoc "gps_mode" vs in - Strip.set_label ac.strip "GPS" gps_mode; - Strip.set_color ac.strip "GPS" (if gps_mode<>"3D" then "red" else "white"); - let ft = - let t = Int32.to_int (Int32.of_string (Pprz.string_assoc "flight_time" vs)) in - Printf.sprintf "%02d:%02d:%02d" (t / 3600) ((t mod 3600) / 60) ((t mod 3600) mod 60) in - Strip.set_label ac.strip "flight_time" ft - in - safe_bind "AP_STATUS" get_ap_status; - - listen_dl_value () - - let listen_waypoint_moved = fun () -> - let get_values = fun _sender vs -> - let ac = get_ac vs in - let wp_id = Pprz.int_assoc "wp_id" vs in - let a = fun s -> Pprz.float_assoc s vs in - let geo = { posn_lat = (Deg>>Rad)(a "lat"); posn_long = (Deg>>Rad)(a "long") } - and altitude = a "alt" in - - (** FIXME: No indexed access to waypoints: iter and compare: *) - List.iter (fun w -> - let (i, w) = ac.fp_group#index w in - if i = wp_id then begin - w#set ~if_not_moved:true ~altitude ~update:true geo; - raise Exit (** catched by safe_bind *) - end) - ac.fp_group#waypoints - in - safe_bind "WAYPOINT_MOVED" get_values - - let get_alert_bat_low = fun a _sender vs -> - let ac_id = Pprz.string_assoc "ac_id" vs in - let ac_name = ref "" in - let level = Pprz.string_assoc "level" vs in - let get_config = fun _sender config -> - ac_name := Pprz.string_assoc "ac_name" config; - a#add (!ac_name^" "^"BAT_LOW"^" "^level) - in - Ground_Pprz.message_req "map2d" "CONFIG" ["ac_id", Pprz.String ac_id] get_config - - - let listen_alert = fun a -> - alert_bind "BAT_LOW" (get_alert_bat_low a) - - - let get_infrared = fun _sender vs -> - let ac_id = Pprz.string_assoc "ac_id" vs in - let ac = Hashtbl.find live_aircrafts ac_id in - let ir_page = ac.ir_page in - let gps_hybrid_mode = Pprz.string_assoc "gps_hybrid_mode" vs in - let gps_hybrid_factor = Pprz.float_assoc "gps_hybrid_factor" vs in - let contrast_status = Pprz.string_assoc "contrast_status" vs in - let contrast_value = Pprz.int_assoc "contrast_value" vs in - - ir_page#set_gps_hybrid_mode gps_hybrid_mode; - ir_page#set_gps_hybrid_factor gps_hybrid_factor; - ir_page#set_contrast_status contrast_status; - ir_page#set_contrast_value contrast_value - - let listen_infrared = fun () -> safe_bind "INFRARED" get_infrared - - let get_svsinfo = fun _sender vs -> - let ac_id = Pprz.string_assoc "ac_id" vs in - let ac = Hashtbl.find live_aircrafts ac_id in - let gps_page = ac.gps_page in - let svid = Str.split list_separator (Pprz.string_assoc "svid" vs) - and cn0 = Str.split list_separator (Pprz.string_assoc "cno" vs) - and flags = Str.split list_separator (Pprz.string_assoc "flags" vs) in - - list_iter3 - (fun id cno flags -> - if id <> "0" then gps_page#svsinfo id cno (int_of_string flags)) - svid cn0 flags - - let listen_svsinfo = fun () -> safe_bind "SVSINFO" get_svsinfo -end (** module Live *) - - - - -(******** Help ***************************************************************) -let keys_help = fun () -> - GToolbox.message_box ~title:"Keys" ~ok:"Close" - "Zoom: Mouse Wheel, PgUp, PgDown\n\ - Pan: Left, Arrows\n\ - Load Map Tile: Right\n\ - Select Region: Shift-Left + Drag\n\ - Create Waypoint: Ctrl-Left\n\ - Move Waypoint: Left\n\ - Edit Waypoint: Middle\n" - - -(******** Sectors **********************************************************) - -module Sector = struct - let rec display = fun (geomap:G.widget) r -> - - match String.lowercase (Xml.tag r) with - "disc" -> - let rad = float_of_string (ExtXml.attrib r "radius") - and geo = Latlong.of_string (ExtXml.attrib (ExtXml.child r "point") "pos") in - ignore (geomap#circle ~width:5 ~color:"red" geo rad) - | "union" -> - List.iter (display geomap) (Xml.children r) - | "polygon" -> - let pts = List.map (fun x -> Latlong.of_string (ExtXml.attrib x "pos")) (Xml.children r) in - let pts = Array.of_list pts in - let n = Array.length pts in - for i = 0 to n - 1 do - ignore (geomap#segment ~width:5 ~fill_color:"red" pts.(i) pts.((i+1)mod n)) - done - |x -> fprintf stderr "Sector.display: '%s' not yet\n%!" x - - - let display_sector = fun (geomap:G.widget) sector -> - display geomap (ExtXml.child sector "0") - - - let load = fun geomap () -> - match GToolbox.select_file ~title:"Load sectors" ~filename:(path_fps^"*.xml") () with - None -> () - | Some f -> - try - let xml = Xml.parse_file f in - List.iter (display_sector geomap) (Xml.children xml) - with - Dtd.Prove_error(e) -> - let m = sprintf "Error while loading %s:\n%s" f (Dtd.prove_error e) in - GToolbox.message_box "Error" m -end - - - - - -(***************** MAIN ******************************************************) -let _main = - let ivy_bus = ref "127.255.255.255:2010" - and geo_ref = ref "" - and map_files = ref [] - and center = ref "" - and zoom = ref 1. - and maximize = ref false - and fullscreen = ref false - and projection = ref G.Mercator - and auto_ortho = ref false - and mplayer = ref "" - and plugin_window = ref "" in - let options = - [ "-b", Arg.String (fun x -> ivy_bus := x), "Bus\tDefault is 127.255.255.25:2010"; - "-maximize", Arg.Set maximize, "Maximize window"; - "-fullscreen", Arg.Set fullscreen, "Fullscreen window"; - "-ref", Arg.Set_string geo_ref, "Geographic ref (default '')"; - "-zoom", Arg.Set_float zoom, "Initial zoom"; - "-center", Arg.Set_string center, "Initial map center"; - "-center_ac", Arg.Set auto_center_new_ac, "Centers the map on any new A/C"; - "-plugin", Arg.Set_string plugin_window, "External X application (launched the id of the plugin window as argument)"; - "-mplayer", Arg.Set_string mplayer, "Launch mplayer with the given argument as X plugin"; - "-utm", Arg.Unit (fun () -> projection:=G.Mercator),"Switch to UTM local projection"; - "-mercator", Arg.Unit (fun () -> projection:=G.Mercator),"Switch to (Google Maps) Mercator projection, default"; - "-lambertIIe", Arg.Unit (fun () -> projection:=G.LambertIIe),"Switch to LambertIIe projection"; - "-ign", Arg.String (fun s -> ign:=true; IGN.data_path := s), "IGN tiles path"; - "-ortho", Arg.Set_string get_bdortho, "IGN tiles path"; - "-no_alarm", Arg.Set no_alarm, "Disables alarm page"; - "-auto_ortho", Arg.Set auto_ortho, "IGN tiles path"; - "-google_fill", Arg.Set GM.auto, "Google maps auto fill"; - "-speech", Arg.Set speech, "Speech"; - "-m", Arg.String (fun x -> map_files := x :: !map_files), "Map description file"] in - Arg.parse (options) - (fun x -> Printf.fprintf stderr "Warning: Don't do anything with '%s'\n" x) - "Usage: "; - (* *) - Ivy.init "Paparazzi map 2D" "READY" (fun _ _ -> ()); - Ivy.start !ivy_bus; - - Srtm.add_path default_path_srtm; - Gm.cache_path := var_maps_path; - IGN.cache_path := var_maps_path; - - (** window for map2d **) - let window = GWindow.window ~title:"Paparazzi GCS" ~border_width:1 ~width:1024 ~height:750 () in - if !maximize then - window#maximize (); - if !fullscreen then - window#fullscreen (); - let vbox= GPack.vbox ~packing: window#add () in - - (** window for vertical situation *) - let vertical_situation = GWindow.window ~title: "Vertical" ~border_width:1 ~width:400 () in - let _vertical_vbox= GPack.vbox ~packing: vertical_situation#add () in - let quit = fun () -> GMain.Main.quit (); exit 0 in - - ignore (window#connect#destroy ~callback:quit); - ignore (vertical_situation#connect#destroy ~callback:quit); - - let geomap = new G.widget ~height:500 ~projection:!projection () in - - let menu_fact = new GMenu.factory geomap#file_menu in - let accel_group = menu_fact#accel_group in - - ignore (geomap#canvas#event#connect#button_press (button_press geomap)); - ignore (geomap#canvas#event#connect#motion_notify (motion_notify geomap)); - ignore (geomap#canvas#event#connect#any (any_event geomap)); - - (** widget displaying aircraft vertical position *) - - let _active_vertical = fun x -> - if x then vertical_situation#show () else vertical_situation#misc#hide () in - ignore (menu_fact#add_item "Redraw" ~key:GdkKeysyms._L ~callback:geomap#canvas#update_now); - let switch_fullscreen = fun x -> - if x then - window#fullscreen () - else - window#unfullscreen () in - ignore (menu_fact#add_check_item "Fullscreen" ~key:GdkKeysyms._F ~active: !fullscreen ~callback:switch_fullscreen); - ignore (menu_fact#add_item "Quit" ~key:GdkKeysyms._Q ~callback:quit); - - (* Maps handling *) - let map_menu = geomap#factory#add_submenu "Maps" in - let map_menu_fact = new GMenu.factory ~accel_group map_menu in - ignore (map_menu_fact#add_item "Load" ~key:GdkKeysyms._M ~callback:(load_map geomap)); - ignore (map_menu_fact#add_item "Calibrate" ~key:GdkKeysyms._C ~callback:(Edit.calibrate_map geomap accel_group)); - ignore (map_menu_fact#add_item "GoogleMaps Fill" ~key:GdkKeysyms._G ~callback:(fun _ -> GM.fill_tiles geomap)); - ignore (map_menu_fact#add_check_item "GoogleMaps Http" ~key:GdkKeysyms._H ~active:true ~callback:GM.active_http); - ignore (map_menu_fact#add_check_item "GoogleMaps Auto" ~active:!GM.auto ~callback:(GM.active_auto geomap)); - ignore (map_menu_fact#add_item "Map of Region" ~key:GdkKeysyms._R ~callback:(map_from_region geomap)); - ignore (map_menu_fact#add_item "Map of Google Tiles" ~key:GdkKeysyms._T ~callback:(GM.map_from_tiles geomap)); - ignore (map_menu_fact#add_item "Load sector" ~callback:(Sector.load geomap)); - - (** Connect Google Maps display to view change *) - geomap#connect_view (fun () -> GM.update geomap); - if !auto_ortho then - geomap#connect_view (fun () -> fill_ortho geomap); - - (** Flight plan editing *) - let fp_menu = geomap#factory#add_submenu "Edit" in - let fp_menu_fact = new GMenu.factory ~accel_group fp_menu in - ignore (fp_menu_fact#add_item "New flight plan" ~key:GdkKeysyms._N ~callback:(Edit.new_fp geomap accel_group)); - ignore (fp_menu_fact#add_item "Open flight plan" ~key:GdkKeysyms._O ~callback:(Edit.load_fp geomap accel_group)); - ignore (fp_menu_fact#add_item "Save flight plan" ~key:GdkKeysyms._S ~callback:(Edit.save_fp)); - ignore (fp_menu_fact#add_item "Close flight plan" ~key:GdkKeysyms._W ~callback:(Edit.close_fp)); - - (** Help pushed to the right *) - let mi = GMenu.menu_item ~label:"Help" ~right_justified:true ~packing:geomap#menubar#append () in - let help_menu = GMenu.menu () in - GToolbox.build_menu help_menu ~entries:[`I ("Keys", keys_help)]; - mi#set_submenu help_menu; - - (** Separate from A/C menus *) - ignore (geomap#factory#add_separator ()); - - let paned = GPack.paned ~show:true `VERTICAL ~packing:(vbox#pack ~expand:true) () in - let frame1 = GPack.vbox () in - paned#pack1 ~shrink:true (*** ~expand:true ***) frame1#coerce; - let hpaned = GPack.paned ~show:true `HORIZONTAL ~packing:paned#add2 () in - - (** Pack the canvas in the window *) - frame1#add geomap#frame#coerce; - (** Set the initial soom *) - geomap#zoom !zoom; - - (** Strips on the left *) - hpaned#add1 strip_scrolled#coerce; - let hpaned2 = GPack.paned ~show:true `HORIZONTAL ~packing:hpaned#add2 () in - - (** Aircraft notebook *) - let fp_notebook = GPack.notebook ~tab_border:0 ~packing:(hpaned2#add1) () in - - let hpaned3 = GPack.paned ~show:true `HORIZONTAL ~packing:hpaned2#add2 () in - - (** Alerts text frame *) - let packing = if !no_alarm then fun _ -> () else hpaned3#add1 in - let alert_page = GBin.frame ~packing () in - let my_alert = new Pages.alert alert_page in - - if !mplayer <> "" then - plugin_window := sprintf "mplayer -nomouseinput '%s' -wid " !mplayer; - if !plugin_window <> "" then begin - let plugin_width = 400 and plugin_height = 300 in - let frame2 = GPack.vbox ~width:plugin_width () in - - hpaned3#pack2 frame2#coerce; - let frame = GBin.event_box ~packing:frame2#add ~width:plugin_width ~height:plugin_height () in - let s = GWindow.socket ~packing:frame#add () in - let com = sprintf "%s 0x%lx -geometry %dx%d" !plugin_window s#xwindow plugin_width plugin_height in - - let pid = ref None in - let restart = fun () -> - begin match !pid with - None -> () - | Some p -> try Unix.kill p 9 with _ -> () - end; - pid := Some (Unix.create_process "/bin/sh" [|"/bin/sh"; "-c"; com|] Unix.stdin Unix.stdout Unix.stderr) in - - restart (); - - ignore (menu_fact#add_item "Restart plugin" ~key:GdkKeysyms._P ~callback:restart); - - plugin_frame := Some frame; - - let swap = fun _ -> - (** Keep the center of the geo canvas *) - let c = geomap#get_center () in - - let child1 = List.hd frame1#children in - let child2 = List.hd frame2#children in - child2#misc#reparent frame1#coerce; - child1#misc#reparent frame2#coerce; - - (* Strange: the centering does not work if done inside this callback. - It is postponed to be called by the mainloop(). *) - ignore (GMain.Idle.add (fun () -> geomap#center c; false)); - in - - let callback = fun ev -> - Printf.printf "%d\n%!" (GdkEvent.Button.button ev); - match GdkEvent.Button.button ev with - 1 -> swap (); true - | 3 -> restart (); true - | _ -> false in - - ignore (frame#event#connect#button_press ~callback) - end; - - - (** Periodically probe new A/Cs *) - ignore (Glib.Timeout.add 2000 (fun () -> Live.Ground_Pprz.message_req "map2d" "AIRCRAFTS" [] (fun _sender vs -> Live.aircrafts_msg geomap fp_notebook vs); false)); - - (** New aircraft message *) - Live.safe_bind "NEW_AIRCRAFT" (fun _sender vs -> Live.one_new_ac geomap fp_notebook (Pprz.string_assoc "ac_id" vs)); - - (** Listen for all messages on ivy *) - Live.listen_flight_params geomap; - Live.listen_wind_msg (); - Live.listen_fbw_msg (); - Live.listen_engine_status_msg (); - Live.listen_if_calib_msg (); - Live.listen_waypoint_moved (); - Live.listen_infrared (); - Live.listen_svsinfo (); - Live.listen_alert my_alert; - - (** Display the window *) - window#add_accel_group accel_group; - window#show (); - - (** Loading an initial map *) - if !geo_ref <> "" then - set_georef_if_none geomap (Latlong.of_string !geo_ref); - List.iter (fun map_file -> - let xml_map_file = if map_file.[0] <> '/' then default_path_maps // map_file else map_file in - display_map geomap xml_map_file) - !map_files; - - (** Center the map as required *) - if !center <> "" then begin - set_georef_if_none geomap (Latlong.of_string !center); - geomap#center (Latlong.of_string !center) - end; - - say "Welcome to paparazzi"; - - (** Threaded main loop (map tiles loaded concurently) *) - GtkThread.main () diff --git a/sw/ground_segment/cockpit/map2d.mli b/sw/ground_segment/cockpit/map2d.mli new file mode 100644 index 0000000000..97fe40e325 --- /dev/null +++ b/sw/ground_segment/cockpit/map2d.mli @@ -0,0 +1,25 @@ +(* +* $Id$ +* +* Copyright (C) 2004-2006 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. +* +*) + +val set_georef_if_none : MapCanvas.widget -> Latlong.geographic -> unit diff --git a/sw/ground_segment/cockpit/plugin.ml b/sw/ground_segment/cockpit/plugin.ml new file mode 100644 index 0000000000..f695797ec6 --- /dev/null +++ b/sw/ground_segment/cockpit/plugin.ml @@ -0,0 +1 @@ +let frame = ref (None: GBin.event_box option) diff --git a/sw/ground_segment/cockpit/plugin.mli b/sw/ground_segment/cockpit/plugin.mli new file mode 100644 index 0000000000..9c549450e0 --- /dev/null +++ b/sw/ground_segment/cockpit/plugin.mli @@ -0,0 +1 @@ +val frame : GBin.event_box option ref diff --git a/sw/ground_segment/cockpit/sectors.ml b/sw/ground_segment/cockpit/sectors.ml new file mode 100644 index 0000000000..653127596a --- /dev/null +++ b/sw/ground_segment/cockpit/sectors.ml @@ -0,0 +1,41 @@ +(******** Sectors **********************************************************) + +open Printf + +let (//) = Filename.concat + +let rec display = fun (geomap:MapCanvas.widget) r -> + + match String.lowercase (Xml.tag r) with + "disc" -> + let rad = float_of_string (ExtXml.attrib r "radius") + and geo = Latlong.of_string (ExtXml.attrib (ExtXml.child r "point") "pos") in + ignore (geomap#circle ~width:5 ~color:"red" geo rad) + | "union" -> + List.iter (display geomap) (Xml.children r) + | "polygon" -> + let pts = List.map (fun x -> Latlong.of_string (ExtXml.attrib x "pos")) (Xml.children r) in + let pts = Array.of_list pts in + let n = Array.length pts in + for i = 0 to n - 1 do + ignore (geomap#segment ~width:5 ~fill_color:"red" pts.(i) pts.((i+1)mod n)) + done + |x -> fprintf stderr "Sector.display: '%s' not yet\n%!" x + + +let display_sector = fun (geomap:MapCanvas.widget) sector -> + display geomap (ExtXml.child sector "0") + + +let load = fun geomap () -> + match GToolbox.select_file ~title:"Load sectors" ~filename:(Env.flight_plans_path // "*.xml") () with + None -> () + | Some f -> + try + let xml = Xml.parse_file f in + List.iter (display_sector geomap) (Xml.children xml) + with + Dtd.Prove_error(e) -> + let m = sprintf "Error while loading %s:\n%s" f (Dtd.prove_error e) in + GToolbox.message_box "Error" m + diff --git a/sw/ground_segment/cockpit/sectors.mli b/sw/ground_segment/cockpit/sectors.mli new file mode 100644 index 0000000000..a52b340660 --- /dev/null +++ b/sw/ground_segment/cockpit/sectors.mli @@ -0,0 +1 @@ +val load : MapCanvas.widget -> unit -> unit diff --git a/sw/ground_segment/cockpit/speech.ml b/sw/ground_segment/cockpit/speech.ml new file mode 100644 index 0000000000..31afa67a66 --- /dev/null +++ b/sw/ground_segment/cockpit/speech.ml @@ -0,0 +1,5 @@ +let active = ref false + +let say = fun s -> + if !active then + ignore (Sys.command (Printf.sprintf "spd-say '%s'&" s)) diff --git a/sw/ground_segment/cockpit/speech.mli b/sw/ground_segment/cockpit/speech.mli new file mode 100644 index 0000000000..dff4aac28c --- /dev/null +++ b/sw/ground_segment/cockpit/speech.mli @@ -0,0 +1,2 @@ +val active : bool ref +val say : string -> unit diff --git a/sw/ground_segment/cockpit/strip.ml b/sw/ground_segment/cockpit/strip.ml new file mode 100644 index 0000000000..09c1cf4d13 --- /dev/null +++ b/sw/ground_segment/cockpit/strip.ml @@ -0,0 +1,103 @@ +(************ Strip handling ***********************************************) + +let bat_max = 13. +let bat_min = 8. + +(** window for the strip panel *) +let scrolled = GBin.scrolled_window ~width:300 ~hpolicy: `AUTOMATIC ~vpolicy: `AUTOMATIC () +let table = GPack.table ~rows: 1 ~columns: 1 ~row_spacings: 5 ~packing: (scrolled#add_with_viewport) () + + +type t = { + gauge: GRange.progress_bar ; + labels: (string * (GBin.event_box * GMisc.label)) list + } + +let labels_name = [| + [| "AP" ; "alt" ; "->" |]; [| "RC"; "climb"; "/" |]; [| "GPS"; "speed"; "" |]; + [| "settings" ; "throttle"; "" |] +|] + +let labels_print = [| + [| "AP" ; "alt" ; "->" |]; [| "RC"; "climb"; "->" |]; [| "GPS"; "speed"; "" |]; + [| "CAL" ; "throttle"; "" |] +|] +let gen_int = let i = ref (-1) in fun () -> incr i; !i + +let rows = Array.length labels_name + 1 + + + (** add a strip to the panel *) +let add config color select center_ac commit_moves mark = + let widget = table in + (* number of the strip *) + let strip_number = gen_int () in + if strip_number > 1 then widget#set_rows strip_number; + + let strip_labels = ref [] in + let add_label = fun name value -> + strip_labels := (name, value) :: !strip_labels in + + let ac_name = Pprz.string_assoc "ac_name" config in + + (* frame of the strip *) + let frame = GBin.frame ~shadow_type: `IN ~packing: (widget#attach ~top: (strip_number) ~left: 0) () in + let strip = GPack.table ~rows: 2 ~columns: 2 ~col_spacings: 10 ~packing: frame#add () in + ignore (GMisc.label ~text: (ac_name) ~packing: (strip#attach ~top: 0 ~left: 0) ()); + + let plane_color = GBin.event_box ~width:10 ~height:10 ~packing:(strip#attach ~top:0 ~left: 1) () in + plane_color#coerce#misc#modify_bg [`NORMAL, `NAME color]; + ignore (plane_color#event#connect#button_press ~callback:(fun _ -> select (); true)); + let h = GPack.hbox ~packing:plane_color#add () in + let ft = GMisc.label ~text: "00:00:00" ~packing:h#add () in + ft#set_width_chars 8; + add_label ("flight_time_value") (plane_color, ft); + let block_name = GMisc.label ~text: "______" ~packing:h#add () in + add_label ("block_name_value") (plane_color, block_name); + + + (* battery and flight time *) + let pb = GRange.progress_bar ~orientation: `BOTTOM_TO_TOP ~packing:(strip#attach ~top:1 ~left:0) () in + pb#coerce#misc#modify_fg [`PRELIGHT, `NAME "green"]; + pb#coerce#misc#modify_font_by_name "sans 12"; + + let left_box = GPack.table ~rows ~columns: 6 ~col_spacings: 5 + ~packing: (strip#attach ~top: 1 ~left: 1) () in + + Array.iteri + (fun i a -> + Array.iteri + (fun j s -> + ignore (GMisc.label ~text: labels_print.(i).(j) ~justify:`RIGHT ~packing: (left_box#attach ~top: i ~left: (2*j)) ()); + let eb = GBin.event_box ~packing: (left_box#attach ~top: i ~left: (2*j+1)) () in + let lvalue = (GMisc.label ~text: "" ~justify: `RIGHT ~packing:eb#add ()) in + lvalue#set_width_chars 6; + add_label (s^"_value") (eb, lvalue); + ) a + ) labels_name; + let b = GButton.button ~label:"Center A/C" ~packing:(left_box#attach ~top:4 ~left:0 ~right:2) () in + ignore(b#connect#clicked ~callback:center_ac); + let b = GButton.button ~label:"Commit WPs" ~packing:(left_box#attach ~top:4 ~left:2 ~right:4) () in + ignore (b#connect#clicked ~callback:commit_moves); + let b = GButton.button ~label:"Mark" ~packing:(left_box#attach ~top:4 ~left:4 ~right:6) () in + ignore (b#connect#clicked ~callback:mark); + {gauge=pb ; labels= !strip_labels} + + + (** set a label *) +let set_label strip name value = + let _eb, l = List.assoc (name^"_value") strip.labels in + l#set_label value + + (** set a label *) +let set_color strip name color = + let eb, _l = List.assoc (name^"_value") strip.labels in + eb#coerce#misc#modify_bg [`NORMAL, `NAME color] + + (** set the battery *) +let set_bat strip value = + strip.gauge#set_text (string_of_float value); + let f = (value -. bat_min) /. (bat_max -. bat_min) in + let f = max 0. (min 1. f) in + strip.gauge#set_fraction f + diff --git a/sw/ground_segment/cockpit/strip.mli b/sw/ground_segment/cockpit/strip.mli new file mode 100644 index 0000000000..fb94d1aeda --- /dev/null +++ b/sw/ground_segment/cockpit/strip.mli @@ -0,0 +1,9 @@ +type t +val add : + Pprz.values -> + string -> + (unit -> 'a) -> (unit -> unit) -> (unit -> unit) -> (unit -> unit) -> t +val set_label : t -> string -> string -> unit +val set_color : t -> string -> string -> unit +val set_bat : t -> float -> unit +val scrolled : GBin.scrolled_window