Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-26 08:22:43 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[ocaml] untabify, indentation

Browse Source
This commit is contained in:
Felix Ruess
2013-03-25 23:40:18 +01:00
parent e708b14f4a
commit 0bef5c0a6d
98 changed files with 7658 additions and 7660 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/ground_segment/cockpit/compass.ml
+36 -36
View File
@@ -1,26 +1,26 @@
(*
* Compass display for a manned vehicle
*
* Copyright (C) 2004-2009 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.
*
*)
* Compass display for a manned vehicle
*
* Copyright (C) 2004-2009 ENAC, Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*)
open Printf
open Latlong
@@ -45,9 +45,9 @@ let n = 100
let circle = fun (dr:GDraw.pixmap) (x,y) r ->
let r = float r in
let points = Array.init n
(fun i ->
let a = float i /. float n *. 2.*.pi in
(x + truncate (r*.cos a), y + truncate (r*.sin a))) in
(fun i ->
let a = float i /. float n *. 2.*.pi in
(x + truncate (r*.cos a), y + truncate (r*.sin a))) in
dr#polygon (Array.to_list points)
let draw = fun (da_object:Gtk_tools.pixmap_in_drawin_area) desired_course course_opt distance ->
@@ -79,14 +79,14 @@ let draw = fun (da_object:Gtk_tools.pixmap_in_drawin_area) desired_course course
(* Arrow *)
if distance > 5. then
match course_opt with
None ->
print_string (4*s) (4*s) "?"
| Some _ ->
let points = List.map (fun (x, y) -> translate (rotation (x*s/2,y*s/2))) arrow in
dr#set_foreground fore;
dr#polygon ~filled:true points;
circle dr (4*s,4*s) (2*s);
circle dr (4*s,4*s) (3*s)
None ->
print_string (4*s) (4*s) "?"
| Some _ ->
let points = List.map (fun (x, y) -> translate (rotation (x*s/2,y*s/2))) arrow in
dr#set_foreground fore;
dr#polygon ~filled:true points;
circle dr (4*s,4*s) (2*s);
circle dr (4*s,4*s) (3*s)
else
print_string (4*s) (4*s) "STOP";
@@ -101,7 +101,7 @@ let draw = fun (da_object:Gtk_tools.pixmap_in_drawin_area) desired_course course
(* Cardinal points *)
let rotation = rot (-. course) in
let cards = [(0, 10, "N"); (0, -10, "S"); (10, 0, "E"); (-10, 0, "W");
(7, 7, "NE"); (7, -7, "SE");(-7,-7,"SW");(-7,7,"NW")] in
(7, 7, "NE"); (7, -7, "SE");(-7,-7,"SW");(-7,7,"NW")] in
List.iter (fun (x,y,string)->
let (x,y) = translate (rotation ((x*5*s)/20, (y*5*s)/20)) in
print_string x y string)
@@ -141,9 +141,9 @@ let _ =
(* if speed < 1m/s, the course information is not relevant *)
course :=
if Pprz.int_assoc "speed" values > 100 then
Some (float (Pprz.int_assoc "course" values) /. 10.)
Some (float (Pprz.int_assoc "course" values) /. 10.)
else
None in
None in
ignore (Tm_Pprz.message_bind "GPS" get_gps);
let get_desired = fun _ values ->
desired_course := (Rad>>Deg) (Pprz.float_assoc "course" values) in
sw/ground_segment/cockpit/editFP.ml
+104 -104
View File
@@ -11,12 +11,12 @@ 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", [],[]);
Xml.Element("blocks", [],[])])
["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", [],[]);
Xml.Element("blocks", [],[])])
@@ -25,49 +25,49 @@ 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 ->
Some _ ->
GToolbox.message_box "Error" "Only one editable flight plan at a time"
| None ->
f ()
let set_window_title = fun geomap ->
let title_suffix =
match !current_fp with
None -> ""
| Some (_fp, xml_file) -> sprintf " (%s)" (Filename.basename xml_file) in
None -> ""
| Some (_fp, xml_file) -> sprintf " (%s)" (Filename.basename xml_file) in
match GWindow.toplevel geomap#canvas with
Some w ->
w#set_title (sprintf "Flight Plan Editor%s" title_suffix)
| None -> ()
Some w ->
w#set_title (sprintf "Flight Plan Editor%s" title_suffix)
| None -> ()
let save_fp = fun geomap ->
match !current_fp with
None -> () (* Nothing to save *)
| Some (fp, filename) ->
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 "<!DOCTYPE flight_plan SYSTEM \"flight_plan.dtd\">\n\n";
fprintf f "%s\n" (ExtXml.to_string_fmt fp#xml);
close_out f;
current_fp := Some (fp, file);
set_window_title geomap
None -> ()
| Some file ->
let f = open_out file in
fprintf f "<!DOCTYPE flight_plan SYSTEM \"flight_plan.dtd\">\n\n";
fprintf f "%s\n" (ExtXml.to_string_fmt fp#xml);
close_out f;
current_fp := Some (fp, file);
set_window_title geomap
let close_fp = fun geomap ->
match !current_fp with
None -> () (* Nothing to close *)
| Some (fp, _filename) ->
None -> () (* Nothing to close *)
| Some (fp, _filename) ->
let close = fun () ->
fp#destroy ();
current_fp := None in
fp#destroy ();
current_fp := None in
match GToolbox.question_box ~title:"Closing flight plan" ~buttons:["Close"; "Save&Close"; "Cancel"] "Do you want to save/close ?" with
2 -> save_fp geomap; close ()
| 1 -> close ()
| _ -> ()
2 -> save_fp geomap; close ()
| 1 -> close ()
| _ -> ()
let load_xml_fp = fun geomap editor_frame _accel_group ?(xml_file=Env.flight_plans_path) xml ->
Map2d.set_georef_if_none geomap (MapFP.georef_of_xml xml);
@@ -75,8 +75,8 @@ let load_xml_fp = fun geomap editor_frame _accel_group ?(xml_file=Env.flight_pla
editor_frame#add fp#window;
current_fp := Some (fp,xml_file);
(** Add waypoints as geo references *)
List.iter
(** Add waypoints as geo references *)
List.iter
(fun w ->
let (_i, w) = fp#index w in
geomap#add_info_georef (sprintf "%s" w#name) (w :> < pos : Latlong.geographic >))
@@ -95,8 +95,8 @@ let new_fp = fun geomap editor_frame accel_group () ->
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
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
@@ -114,20 +114,20 @@ let new_fp = fun geomap editor_frame accel_group () ->
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 editor_frame accel_group xml);
dialog#destroy ()
end);
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 editor_frame accel_group xml);
dialog#destroy ()
end);
dialog#show ())
@@ -144,9 +144,9 @@ let load_xml_file = fun geomap editor_frame accel_group xml_file ->
geomap#fit_to_window ();
set_window_title geomap
with
Dtd.Prove_error(e) -> loading_error xml_file (Dtd.prove_error e)
| Dtd.Check_error(e) -> loading_error xml_file (Dtd.check_error e)
| Xml.Error e -> loading_error xml_file (Xml.error e)
Dtd.Prove_error(e) -> loading_error xml_file (Dtd.prove_error e)
| Dtd.Check_error(e) -> loading_error xml_file (Dtd.check_error e)
| Xml.Error e -> loading_error xml_file (Xml.error e)
@@ -154,15 +154,15 @@ let load_xml_file = fun geomap editor_frame accel_group xml_file ->
let load_fp = fun geomap editor_frame accel_group () ->
if_none (fun () ->
match GToolbox.select_file ~title:"Open flight plan" ~filename:(Env.flight_plans_path // "*.xml") () with
None -> ()
| Some xml_file -> load_xml_file geomap editor_frame accel_group xml_file)
None -> ()
| Some xml_file -> load_xml_file geomap editor_frame accel_group xml_file)
let create_wp = fun geomap geo ->
match !current_fp with
None ->
GToolbox.message_box "Error" "Load a flight plan first";
failwith "create_wp"
| Some (fp,_) ->
None ->
GToolbox.message_box "Error" "Load a flight plan first";
failwith "create_wp"
| Some (fp,_) ->
let w = fp#add_waypoint geo in
geomap#add_info_georef (sprintf "%s" w#name) (w :> < pos : Latlong.geographic >);
w
@@ -171,57 +171,57 @@ let create_wp = fun geomap geo ->
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"],[])
"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) editor_frame accel_group () ->
match !current_fp with
| Some (_fp,_) -> GToolbox.message_box "Error" "Close current flight plan before calibration"
| None ->
| 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;
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 editor_frame accel_group fp_xml in
(** 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 editor_frame 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 geomap;
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 ()
(** 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 geomap;
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 ()
sw/ground_segment/cockpit/gcs.ml
+196 -196
View File
File diff suppressed because it is too large Load Diff
sw/ground_segment/cockpit/horizon.ml
+60 -60
View File
@@ -1,26 +1,26 @@
(*
* Multi aircrafts map display and flight plan editor
*
* Copyright (C) 2004-2006 ENAC, Pascal Brisset, Antoine Drouin / 2011 Tobias Muench, Rolf Noellenburg
*
* 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.
*
*)
* Multi aircrafts map display and flight plan editor
*
* Copyright (C) 2004-2006 ENAC, Pascal Brisset, Antoine Drouin / 2011 Tobias Muench, Rolf Noellenburg
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*)
open Printf
open Latlong
@@ -71,8 +71,8 @@ let ruler = fun ?(index_on_right=false) ~text_props ~max ~scale ~w ~index_width
let v = truncate v / step in
for i = Pervasives.max 0 (v - 5) to min (v + 5) (Array.length tab - 1) do (* FIXME *)
if not tab.(i) then begin
tab.(i) <- true;
draw i
tab.(i) <- true;
draw i
end
done in
@@ -116,7 +116,7 @@ class h = fun ?packing size ->
and _bottom = GnoCanvas.rect ~x1:(-.size) ~y1:0. ~x2:size ~y2:(size2*.5.) ~fill_color:"#986701" disc
and _line = GnoCanvas.line ~props:[`WIDTH_PIXELS 4] ~points:[|-.size;0.;size;0.|] ~fill_color:"white" disc
and _ = GnoCanvas.line ~points:[|0.;-.size;0.;size|] ~fill_color:"white" disc
in
in
let grads = fun ?(text=false) n s a b ->
for i = 0 to n do
let deg = float i *. a +. b in
@@ -124,13 +124,13 @@ class h = fun ?packing size ->
ignore (GnoCanvas.line ~points:[|-.s; y; s; y|] ~fill_color:"white" disc);
ignore (GnoCanvas.line ~points:[|-.s; -.y; s; -.y|] ~fill_color:"white" disc);
if text then
let text = Printf.sprintf "%d" (truncate deg)
and x = 2.*.s in
ignore (GnoCanvas.text ~props:text_props ~text ~y:(-.y) ~x disc);
ignore (GnoCanvas.text ~props:text_props ~text ~y:(-.y) ~x:(-.x) disc);
let text = "-"^text in
ignore (GnoCanvas.text ~props:text_props ~text ~y ~x disc);
ignore (GnoCanvas.text ~props:text_props ~text ~y ~x:(-.x) disc);
let text = Printf.sprintf "%d" (truncate deg)
and x = 2.*.s in
ignore (GnoCanvas.text ~props:text_props ~text ~y:(-.y) ~x disc);
ignore (GnoCanvas.text ~props:text_props ~text ~y:(-.y) ~x:(-.x) disc);
let text = "-"^text in
ignore (GnoCanvas.text ~props:text_props ~text ~y ~x disc);
ignore (GnoCanvas.text ~props:text_props ~text ~y ~x:(-.x) disc);
done in
let _ =
@@ -156,8 +156,8 @@ class h = fun ?packing size ->
ignore (GnoCanvas.line ~props:[`WIDTH_PIXELS 4] ~points:[|-.x;0.;-.x-.s;0.;-.x-.s;s|] ~fill_color:"black" mask);
ignore (GnoCanvas.line ~props:[`WIDTH_PIXELS 4] ~points:[|x;0.;x+.s;0.;x+.s;s|] ~fill_color:"black" mask);
(* Top and bottom graduations *)
let g = fun a ->
(* Top and bottom graduations *)
let g = fun a ->
let l = GnoCanvas.line~props:[`WIDTH_PIXELS 1] ~fill_color:"white" ~points:[|0.;-.size2;0.;-.1.07*.size2|] mask in
l#affine_relative (affine_pos_and_angle 0. 0. ((Deg>>Rad)a)) in
for i = 1 to 5 do
@@ -168,13 +168,13 @@ class h = fun ?packing size ->
let gg = fun a ->
let l = GnoCanvas.line~props:[`WIDTH_PIXELS 2] ~fill_color:"white" ~points:[|0.;-.size2;0.;-.1.15*.size2|] mask in
l#affine_relative (affine_pos_and_angle 0. 0. ((Deg>>Rad)a)) in
gg 30.; gg (-30.);
gg 0.; gg 0.;
gg 30.; gg (-30.);
gg 0.; gg 0.;
let _30 = fun a ->
let t = GnoCanvas.text ~text:"30" ~props:text_props ~x:0. ~y:(-1.28*.size2) mask in
t#affine_relative (affine_pos_and_angle 0. 0. ((Deg>>Rad)a)) in
_30 30.; _30 (-30.)
_30 30.; _30 (-30.)
in
@@ -197,31 +197,31 @@ class h = fun ?packing size ->
ruler ~text_props ~max:3000 ~scale:alt_scale ~w:alt_width ~step:10 ~index_width ~h:(0.75*.size2) g
in
object
method set_attitude = fun roll pitch ->
disc#affine_absolute (affine_pos_and_angle (xc+.((sin roll)*.(pitch_scale pitch))) (yc+.pitch_scale pitch*.(cos roll)) (-.roll))
val mutable max_speed = 0.
val mutable min_speed = max_float
method set_speed = fun (s:float) ->
speed#affine_absolute (affine_pos 0. 0.);
lazy_speed s;
speed#affine_absolute (affine_pos 0. (speed_scale*.s));
min_speed <- min min_speed s;
max_speed <- max max_speed s;
mi#set [`TEXT (sprintf "%.1f" min_speed)];
mx#set [`TEXT (sprintf "%.1f" max_speed)]
initializer
ignore (speed#connect#event (function
`BUTTON_PRESS _ev ->
max_speed <- 0.; min_speed <- max_float; true
| _ -> false))
object
method set_attitude = fun roll pitch ->
disc#affine_absolute (affine_pos_and_angle (xc+.((sin roll)*.(pitch_scale pitch))) (yc+.pitch_scale pitch*.(cos roll)) (-.roll))
val mutable max_speed = 0.
val mutable min_speed = max_float
method set_speed = fun (s:float) ->
speed#affine_absolute (affine_pos 0. 0.);
lazy_speed s;
speed#affine_absolute (affine_pos 0. (speed_scale*.s));
min_speed <- min min_speed s;
max_speed <- max max_speed s;
mi#set [`TEXT (sprintf "%.1f" min_speed)];
mx#set [`TEXT (sprintf "%.1f" max_speed)]
initializer
ignore (speed#connect#event (function
`BUTTON_PRESS _ev ->
max_speed <- 0.; min_speed <- max_float; true
| _ -> false))
method set_alt = fun (s:float) ->
alt#affine_absolute (affine_pos 0. 0.);
lazy_alt s;
alt#affine_absolute (affine_pos 0. (alt_scale*.s))
method set_alt = fun (s:float) ->
alt#affine_absolute (affine_pos 0. 0.);
lazy_alt s;
alt#affine_absolute (affine_pos 0. (alt_scale*.s))
end
end
(*****************************************************************************)
(* pfd page *)
sw/ground_segment/cockpit/live.ml
+349 -349
View File
File diff suppressed because it is too large Load Diff
sw/ground_segment/cockpit/map2d.ml
+24 -24
View File
@@ -1,28 +1,28 @@
(*
* 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.
*
*)
* 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.
*
*)
let set_georef_if_none = fun geomap wgs84 ->
match geomap#georef with
None ->
geomap#set_georef wgs84;
geomap#center wgs84
| Some _ -> ()
None ->
geomap#set_georef wgs84;
geomap#center wgs84
| Some _ -> ()
sw/ground_segment/cockpit/page_settings.ml
+26 -26
View File
@@ -1,26 +1,26 @@
(*
* Widget to pack settings buttons
*
* Copyright (C) 2004-2009 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.
*
*)
* Widget to pack settings buttons
*
* Copyright (C) 2004-2009 ENAC, Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*)
open Printf
@@ -122,14 +122,14 @@ let one_setting = fun (i:int) (do_change:int -> float -> unit) packing dl_settin
let group = (GButton.radio_button ())#group in (* Group shared by the buttons *)
let buttons = Array.init (iupper-ilower+1)
(fun j ->
(* Build the button *)
(* Build the button *)
let label =
if Array.length values = 0
then Printf.sprintf "%d" (ilower + j)
else values.(j) in
let b = GButton.radio_button ~group ~label ~packing:hbox#add () in
(* Connect the event *)
(* Connect the event *)
ignore (b#connect#pressed (fun () -> update_value (ilower + j)));
b) in
(callback, fun j -> try buttons.(truncate j - ilower)#set_active true with _ -> ())
@@ -196,14 +196,14 @@ let one_setting = fun (i:int) (do_change:int -> float -> unit) packing dl_settin
let pixbuf = GdkPixbuf.from_file (Env.gcs_icons_path // icon) in
ignore (GMisc.image ~pixbuf ~packing:b#add ());
(* Drag for Drop *)
(* Drag for Drop *)
let papget = Papget_common.xml "variable_setting" "button"
["variable", varname;
"value", ExtXml.attrib x "value";
"icon", icon] in
Papget_common.dnd_source b#coerce papget;
(* Associates the label as a tooltip *)
(* Associates the label as a tooltip *)
tooltips#set_tip b#coerce ~text:label;
b
with
sw/ground_segment/cockpit/pages.ml
+110 -110
View File
@@ -32,42 +32,42 @@ open Printf
(** alert page *)
class alert (widget: GBin.frame) =
let scrolled = GBin.scrolled_window
~hpolicy: `AUTOMATIC
~vpolicy: `AUTOMATIC
~packing: widget#add
()
~hpolicy: `AUTOMATIC
~vpolicy: `AUTOMATIC
~packing: widget#add
()
in
let view = GText.view ~editable:false ~packing: scrolled#add () in
(* the object itselft *)
object
val mutable last = ""
method add text =
if text <> last then begin
let l = Unix.localtime (Unix.gettimeofday ()) in
view#buffer#insert (sprintf "%02d:%02d:%02d " l.Unix.tm_hour l.Unix.tm_min l.Unix.tm_sec);
view#buffer#insert text;
view#buffer#insert "\n";
(* the object itselft *)
object
val mutable last = ""
method add text =
if text <> last then begin
let l = Unix.localtime (Unix.gettimeofday ()) in
view#buffer#insert (sprintf "%02d:%02d:%02d " l.Unix.tm_hour l.Unix.tm_min l.Unix.tm_sec);
view#buffer#insert text;
view#buffer#insert "\n";
(* Scroll to the bottom line *)
let end_iter = view#buffer#end_iter in
let end_mark = view#buffer#create_mark end_iter in
view#scroll_mark_onscreen (`MARK end_mark);
(* Scroll to the bottom line *)
let end_iter = view#buffer#end_iter in
let end_mark = view#buffer#create_mark end_iter in
view#scroll_mark_onscreen (`MARK end_mark);
last <- text
end
end
last <- text
end
end
(*****************************************************************************)
(* infrared page *)
(*****************************************************************************)
class infrared (widget: GBin.frame) =
let table = GPack.table
~rows: 4
~columns: 2
~row_spacings: 5
~col_spacings: 5
~packing: widget#add
()
~rows: 4
~columns: 2
~row_spacings: 5
~col_spacings: 5
~packing: widget#add
()
in
let contrast_status =
GMisc.label ~text: "" ~packing: (table#attach ~top:0 ~left: 1) ()
@@ -87,24 +87,24 @@ class infrared (widget: GBin.frame) =
ignore (GMisc.label ~text: "gps hybrid mode" ~packing: (table#attach ~top:2 ~left: 0) ());
ignore (GMisc.label ~text: "gps hybrid factor" ~packing: (table#attach ~top:3 ~left: 0) ())
in
object
val parent = widget
val table = table
object
val parent = widget
val table = table
val contrast_status = contrast_status
val contrast_value = contrast_value
val gps_hybrid_mode = gps_hybrid_mode
val gps_hybrid_factor = gps_hybrid_factor
val contrast_status = contrast_status
val contrast_value = contrast_value
val gps_hybrid_mode = gps_hybrid_mode
val gps_hybrid_factor = gps_hybrid_factor
method set_contrast_status (s:string) =
contrast_status#set_label s
method set_contrast_value (s:int) =
contrast_value#set_label (Printf.sprintf "%d" s)
method set_gps_hybrid_mode (s:string) =
gps_hybrid_mode#set_label s
method set_gps_hybrid_factor (s:float) =
gps_hybrid_factor#set_label (Printf.sprintf "%.8f" s)
end
method set_contrast_status (s:string) =
contrast_status#set_label s
method set_contrast_value (s:int) =
contrast_value#set_label (Printf.sprintf "%d" s)
method set_gps_hybrid_mode (s:string) =
gps_hybrid_mode#set_label s
method set_gps_hybrid_factor (s:float) =
gps_hybrid_factor#set_label (Printf.sprintf "%.8f" s)
end
(*****************************************************************************)
(* gps page *)
@@ -123,71 +123,71 @@ class gps ?(visible = fun _ -> true) (widget: GBin.frame) =
let warm = GButton.button ~label:"Warmstart" ~packing:hbox#add () in
let cold = GButton.button ~label:"Coldstart" ~packing:hbox#add () in
object
val mutable active_cno = []
val mutable active_flags = []
object
val mutable active_cno = []
val mutable active_flags = []
method connect_reset = fun (callback:int -> unit) ->
hbox#misc#show ();
ignore (hot#connect#clicked (fun () -> callback 0));
ignore (warm#connect#clicked (fun () -> callback 1));
ignore (cold#connect#clicked (fun () -> callback 2))
method connect_reset = fun (callback:int -> unit) ->
hbox#misc#show ();
ignore (hot#connect#clicked (fun () -> callback 0));
ignore (warm#connect#clicked (fun () -> callback 1));
ignore (cold#connect#clicked (fun () -> callback 2))
method svsinfo pacc a =
if visible widget then
let da = da_object#drawing_area in
let {Gtk.width=width; height=height} = da#misc#allocation in
method svsinfo pacc a =
if visible widget then
let da = da_object#drawing_area in
let {Gtk.width=width; height=height} = da#misc#allocation in
(* Background *)
let dr = da_object#get_pixmap () in
dr#set_foreground (`NAME "white");
dr#rectangle ~x:0 ~y:0 ~width ~height ~filled:true ();
(* Background *)
let dr = da_object#get_pixmap () in
dr#set_foreground (`NAME "white");
dr#rectangle ~x:0 ~y:0 ~width ~height ~filled:true ();
let context = da#misc#create_pango_context in
context#set_font_by_name ("sans " ^ string_of_int 10);
let layout = context#create_layout in
let context = da#misc#create_pango_context in
context#set_font_by_name ("sans " ^ string_of_int 10);
let layout = context#create_layout in
let n = Array.length a in
let sep_size = 3 in
let indic_size = min 25 ((width-(n+1)*sep_size)/n) in
let max_cn0 = 50 in
let n = Array.length a in
let sep_size = 3 in
let indic_size = min 25 ((width-(n+1)*sep_size)/n) in
let max_cn0 = 50 in
Pango.Layout.set_text layout "Dummy";
let (_, h) = Pango.Layout.get_pixel_size layout in
Pango.Layout.set_text layout "Dummy";
let (_, h) = Pango.Layout.get_pixel_size layout in
let size = fun cn0 -> (max 20 cn0 - 20) * 2 in
let size = fun cn0 -> (max 20 cn0 - 20) * 2 in
let y = sep_size + h + (size max_cn0) in
for i = 0 to n - 1 do
let (id, cn0, flags, age) = a.(i) in
if age < 60 then
let x = sep_size + i * (sep_size+indic_size) in
let y = sep_size + h + (size max_cn0) in
for i = 0 to n - 1 do
let (id, cn0, flags, age) = a.(i) in
if age < 60 then
let x = sep_size + i * (sep_size+indic_size) in
(* level *)
Pango.Layout.set_text layout (sprintf "% 2d" cn0);
dr#put_layout ~x ~y:0 ~fore:`BLACK layout;
(* level *)
Pango.Layout.set_text layout (sprintf "% 2d" cn0);
dr#put_layout ~x ~y:0 ~fore:`BLACK layout;
(* bar *)
let color = if age > 5 then "grey" else if flags land 0x01 = 1 then "green" else "red" in
dr#set_foreground (`NAME color);
let height = size cn0 in
dr#rectangle ~filled:true ~x ~y:(y-height) ~width:indic_size ~height ();
(* SV id *)
Pango.Layout.set_text layout (sprintf "% 2d" id);
dr#put_layout ~x ~y ~fore:`BLACK layout
done;
(* bar *)
let color = if age > 5 then "grey" else if flags land 0x01 = 1 then "green" else "red" in
dr#set_foreground (`NAME color);
let height = size cn0 in
dr#rectangle ~filled:true ~x ~y:(y-height) ~width:indic_size ~height ();
(* SV id *)
Pango.Layout.set_text layout (sprintf "% 2d" id);
dr#put_layout ~x ~y ~fore:`BLACK layout
done;
(* Pacc *)
let max_pacc = 2000 in
dr#set_foreground (`NAME "red");
let w = min width ((pacc*width)/max_pacc) in
dr#rectangle ~filled:true ~x:0 ~y:(y+h) ~width:w ~height:h ();
Pango.Layout.set_text layout (if pacc = 0 then "Pos accuracy: N/A" else sprintf "Pos accuracy: %.1fm" (float pacc/.100.));
let (_, h) = Pango.Layout.get_pixel_size layout in
dr#put_layout ~x:((width-w)/2) ~y:(y+h) ~fore:`BLACK layout;
(* Pacc *)
let max_pacc = 2000 in
dr#set_foreground (`NAME "red");
let w = min width ((pacc*width)/max_pacc) in
dr#rectangle ~filled:true ~x:0 ~y:(y+h) ~width:w ~height:h ();
Pango.Layout.set_text layout (if pacc = 0 then "Pos accuracy: N/A" else sprintf "Pos accuracy: %.1fm" (float pacc/.100.));
let (_, h) = Pango.Layout.get_pixel_size layout in
dr#put_layout ~x:((width-w)/2) ~y:(y+h) ~fore:`BLACK layout;
(new GDraw.drawable da#misc#window)#put_pixmap ~x:0 ~y:0 dr#pixmap
end
(new GDraw.drawable da#misc#window)#put_pixmap ~x:0 ~y:0 dr#pixmap
end
(*****************************************************************************)
(* Misc page *)
@@ -215,27 +215,27 @@ class misc ~packing (widget: GBin.frame) =
let top = index_of "Send periodically" in
values.(top)#destroy ();
GButton.check_button ~active:true ~packing:(table#attach ~top ~left:1) () in
object
method set_value label s = values.(index_of label)#set_text s
method periodic_send = periodic_send#active
end
object
method set_value label s = values.(index_of label)#set_text s
method periodic_send = periodic_send#active
end
type rc_mode = string
type rc_setting_mode = string
let rc_setting_index = function
"gain_1_up" -> 0, 0
"gain_1_up" -> 0, 0
| "gain_1_down" -> 1, 0
| "gain_2_up" -> 0, 1
| "gain_2_down" -> 1, 1
| x -> failwith (sprintf "Unknown rc_setting: %s" x)
let rc_mode_index = function
"AUTO1" -> 0 | "AUTO2" -> 1
"AUTO1" -> 0 | "AUTO2" -> 1
| _x -> -1
let rc_setting_mode_index = function
"UP" -> 0 | "DOWN" -> 1
"UP" -> 0 | "DOWN" -> 1
| _x -> -1
let one_rc_mode = fun (table:GPack.table) rc_mode ->
@@ -245,7 +245,7 @@ let one_rc_mode = fun (table:GPack.table) rc_mode ->
and text = ExtXml.attrib rc_setting "var" in
let (j, k) = rc_setting_index name in
ignore (GMisc.label ~text ~packing:(table#attach ~top:(1+2*j+k) ~left:(1+2*i)) ())
)
)
(Xml.children rc_mode)
@@ -283,13 +283,13 @@ class rc_settings = fun ?(visible = fun _ -> true) xmls ->
method widget = sw#coerce
method set = fun v1 v2 ->
if visible self#widget then
let i = rc_mode_index rc_mode
and j = rc_setting_mode_index rc_setting_mode in
if i >= 0 && j >= 0 then
let s1 = string_of_float v1 in
let s2 = string_of_float v2 in
let i = rc_mode_index rc_mode
and j = rc_setting_mode_index rc_setting_mode in
if i >= 0 && j >= 0 then
let s1 = string_of_float v1 in
let s2 = string_of_float v2 in
values.(i).(j).(0)#set_text s1;
values.(i).(j).(1)#set_text s2
values.(i).(j).(0)#set_text s1;
values.(i).(j).(1)#set_text s2
end
sw/ground_segment/cockpit/papgets.ml
+110 -110
View File
@@ -31,9 +31,9 @@ let dump_store = fun () ->
Hashtbl.fold
(fun _ p r ->
if not p#deleted then
p#config ()::r
p#config ()::r
else
r)
r)
papgets
[]
@@ -43,26 +43,26 @@ let papget_listener =
try
let field = Papget_common.get_property "field" papget in
match Str.split sep field with
[msg_name; field_name] ->
(new Papget.message_field msg_name field_name)
| _ -> failwith (sprintf "Unexpected field spec: %s" field)
[msg_name; field_name] ->
(new Papget.message_field msg_name field_name)
| _ -> failwith (sprintf "Unexpected field spec: %s" field)
with
_ -> failwith (sprintf "field attr expected in '%s" (Xml.to_string papget))
_ -> failwith (sprintf "field attr expected in '%s" (Xml.to_string papget))
let block_name_of_index = function
[ i ] ->
let i = sprintf "%.0f" (float_of_string i) in
if Hashtbl.length Live.aircrafts = 1 then
Hashtbl.fold
(fun ac_id ac _r ->
let blocks = ExtXml.child ac.Live.fp "blocks" in
let block = ExtXml.child blocks i in
ExtXml.attrib block "name")
Live.aircrafts
"N/A"
else
"N/A"
[ i ] ->
let i = sprintf "%.0f" (float_of_string i) in
if Hashtbl.length Live.aircrafts = 1 then
Hashtbl.fold
(fun ac_id ac _r ->
let blocks = ExtXml.child ac.Live.fp "blocks" in
let block = ExtXml.child blocks i in
ExtXml.attrib block "name")
Live.aircrafts
"N/A"
else
"N/A"
| _ -> failwith "Papgets.block_name_of_index"
let extra_functions =
@@ -79,15 +79,15 @@ let expression_listener = fun papget ->
let display_float_papget = fun canvas_group config display x y listener ->
let renderer =
match display with
"text" ->
(new Papget_renderer.canvas_text ~config canvas_group x y :> Papget_renderer.t)
| "ruler" ->
(new Papget_renderer.canvas_ruler canvas_group ~config x y :> Papget_renderer.t)
| "gauge" ->
(new Papget_renderer.canvas_gauge ~config canvas_group x y :> Papget_renderer.t)
| "led" ->
(new Papget_renderer.canvas_led ~config canvas_group x y :> Papget_renderer.t)
| _ -> failwith (sprintf "Unexpected papget display: %s" display) in
"text" ->
(new Papget_renderer.canvas_text ~config canvas_group x y :> Papget_renderer.t)
| "ruler" ->
(new Papget_renderer.canvas_ruler canvas_group ~config x y :> Papget_renderer.t)
| "gauge" ->
(new Papget_renderer.canvas_gauge ~config canvas_group x y :> Papget_renderer.t)
| "led" ->
(new Papget_renderer.canvas_led ~config canvas_group x y :> Papget_renderer.t)
| _ -> failwith (sprintf "Unexpected papget display: %s" display) in
let p = new Papget.canvas_display_float_item ~config listener renderer in
let p = (p :> Papget.item) in
@@ -108,84 +108,84 @@ let create = fun canvas_group papget ->
and y = ExtXml.float_attrib papget "y"
and config = Xml.children papget in
match type_ with
"expression" ->
let expr_listener = expression_listener papget in
display_float_papget canvas_group config display x y expr_listener
"expression" ->
let expr_listener = expression_listener papget in
display_float_papget canvas_group config display x y expr_listener
| "message_field" ->
let msg_listener = papget_listener papget in
display_float_papget canvas_group config display x y msg_listener
| "message_field" ->
let msg_listener = papget_listener papget in
display_float_papget canvas_group config display x y msg_listener
| "goto_block" ->
let renderer =
match display with
"button" ->
(new Papget_renderer.canvas_button canvas_group ~config x y :> Papget_renderer.t)
| _ -> failwith (sprintf "Unexpected papget display: %s" display) in
let block_name = Papget_common.get_property "block_name" papget in
let clicked = fun () ->
prerr_endline "Warning: goto_block papget sends to all A/C";
Hashtbl.iter
(fun ac_id ac ->
let blocks = ExtXml.child ac.Live.fp "blocks" in
let block = ExtXml.child ~select:(fun x -> ExtXml.attrib x "name" = block_name) blocks "block" in
let block_id = ExtXml.int_attrib block "no" in
Live.jump_to_block ac_id block_id
)
Live.aircrafts
in
let properties =
[ Papget_common.property "block_name" block_name ] @ locked papget in
| "goto_block" ->
let renderer =
match display with
"button" ->
(new Papget_renderer.canvas_button canvas_group ~config x y :> Papget_renderer.t)
| _ -> failwith (sprintf "Unexpected papget display: %s" display) in
let block_name = Papget_common.get_property "block_name" papget in
let clicked = fun () ->
prerr_endline "Warning: goto_block papget sends to all A/C";
Hashtbl.iter
(fun ac_id ac ->
let blocks = ExtXml.child ac.Live.fp "blocks" in
let block = ExtXml.child ~select:(fun x -> ExtXml.attrib x "name" = block_name) blocks "block" in
let block_id = ExtXml.int_attrib block "no" in
Live.jump_to_block ac_id block_id
)
Live.aircrafts
in
let properties =
[ Papget_common.property "block_name" block_name ] @ locked papget in
let p = new Papget.canvas_goto_block_item properties clicked renderer in
let p = (p :> Papget.item) in
register_papget p
| "variable_setting" ->
let renderer =
match display with
"button" ->
(new Papget_renderer.canvas_button canvas_group ~config x y :> Papget_renderer.t)
| _ -> failwith (sprintf "Unexpected papget display: %s" display) in
let p = new Papget.canvas_goto_block_item properties clicked renderer in
let p = (p :> Papget.item) in
register_papget p
| "variable_setting" ->
let renderer =
match display with
"button" ->
(new Papget_renderer.canvas_button canvas_group ~config x y :> Papget_renderer.t)
| _ -> failwith (sprintf "Unexpected papget display: %s" display) in
let varname = Papget_common.get_property "variable" papget
and value = float_of_string (Papget_common.get_property "value" papget) in
let varname = Papget_common.get_property "variable" papget
and value = float_of_string (Papget_common.get_property "value" papget) in
let clicked = fun () ->
prerr_endline "Warning: variable_setting papget sending to all active A/C";
Hashtbl.iter
(fun ac_id ac ->
match ac.Live.dl_settings_page with
None -> ()
| Some settings ->
let var_id = settings#assoc varname in
Live.dl_setting ac_id var_id value)
Live.aircrafts
in
let properties =
[ Papget_common.property "variable" varname;
Papget_common.float_property "value" value ]
@ locked papget in
let p = new Papget.canvas_variable_setting_item properties clicked renderer in
let p = (p :> Papget.item) in
register_papget p
let clicked = fun () ->
prerr_endline "Warning: variable_setting papget sending to all active A/C";
Hashtbl.iter
(fun ac_id ac ->
match ac.Live.dl_settings_page with
None -> ()
| Some settings ->
let var_id = settings#assoc varname in
Live.dl_setting ac_id var_id value)
Live.aircrafts
in
let properties =
[ Papget_common.property "variable" varname;
Papget_common.float_property "value" value ]
@ locked papget in
let p = new Papget.canvas_variable_setting_item properties clicked renderer in
let p = (p :> Papget.item) in
register_papget p
| "video_plugin" ->
let renderer =
match display with
"mplayer" ->
(new Papget_renderer.canvas_mplayer canvas_group ~config x y :> Papget_renderer.t)
| "plugin" ->
(new Papget_renderer.canvas_plugin canvas_group ~config x y :> Papget_renderer.t)
| _ -> failwith (sprintf "Unexpected papget display: %s" display) in
| "video_plugin" ->
let renderer =
match display with
"mplayer" ->
(new Papget_renderer.canvas_mplayer canvas_group ~config x y :> Papget_renderer.t)
| "plugin" ->
(new Papget_renderer.canvas_plugin canvas_group ~config x y :> Papget_renderer.t)
| _ -> failwith (sprintf "Unexpected papget display: %s" display) in
let properties = locked papget in
let p = new Papget.canvas_video_plugin_item properties renderer in
let p = (p :> Papget.item) in
register_papget p
let properties = locked papget in
let p = new Papget.canvas_video_plugin_item properties renderer in
let p = (p :> Papget.item) in
register_papget p
| _ -> failwith (sprintf "Unexpected papget type: %s" type_)
| _ -> failwith (sprintf "Unexpected papget type: %s" type_)
with
exc -> fprintf stderr "Papgets.create: %s\n%!" (Printexc.to_string exc)
exc -> fprintf stderr "Papgets.create: %s\n%!" (Printexc.to_string exc)
exception Parse_message_dnd of string
@@ -194,27 +194,27 @@ let parse_message_dnd =
let sep = Str.regexp ":" in
fun s ->
match Str.split sep s with
[s; c; m; f;scale] -> (s, c, m, f,scale)
| _ -> raise (Parse_message_dnd (Printf.sprintf "parse_dnd: %s" s))
[s; c; m; f;scale] -> (s, c, m, f,scale)
| _ -> raise (Parse_message_dnd (Printf.sprintf "parse_dnd: %s" s))
let dnd_data_received = fun canvas_group _context ~x ~y data ~info ~time ->
try (* With the format sent by Messages *)
let (_sender, _class_name, msg_name, field_name,scale) = parse_message_dnd data#data in
let attrs =
[ "type", "message_field";
"display", "text";
"x", sprintf "%d" x; "y", sprintf "%d" y ]
"display", "text";
"x", sprintf "%d" x; "y", sprintf "%d" y ]
and props =
[ Papget_common.property "field" (sprintf "%s:%s" msg_name field_name);
Papget_common.property "scale" scale ] in
Papget_common.property "scale" scale ] in
let papget_xml = Xml.Element ("papget", attrs, props) in
create canvas_group papget_xml
with
Parse_message_dnd _ ->
try (* XML spec *)
let xml = Xml.parse_string data#data in
(* Add x and y attributes *)
let attrs = Xml.attribs xml @ ["x", string_of_int x; "y", string_of_int y] in
let papget_xml = Xml.Element (Xml.tag xml,attrs,Xml.children xml) in
create canvas_group papget_xml
with
exc -> prerr_endline (Printexc.to_string exc)
Parse_message_dnd _ ->
try (* XML spec *)
let xml = Xml.parse_string data#data in
(* Add x and y attributes *)
let attrs = Xml.attribs xml @ ["x", string_of_int x; "y", string_of_int y] in
let papget_xml = Xml.Element (Xml.tag xml,attrs,Xml.children xml) in
create canvas_group papget_xml
with
exc -> prerr_endline (Printexc.to_string exc)
sw/ground_segment/cockpit/particules.ml
+13 -13
View File
@@ -20,12 +20,12 @@ let move_particule = fun (geomap:MapCanvas.widget) id geo value ->
geomap#move_item item geo;
item#set [`FILL_COLOR fill_color]
with
Not_found ->
let group = geomap#background in
let p = GnoCanvas.ellipse ~fill_color ~props:[`WIDTH_PIXELS 1; `OUTLINE_COLOR "black"] ~x1:(-3.) ~y1:(-3.) ~x2:3. ~y2:3. group in
(* geomap#circle ~group ~fill_color:"red" geo 10. in *)
p#raise_to_top ();
Hashtbl.add particules id (p:>GnomeCanvas.re_p GnoCanvas.item)
Not_found ->
let group = geomap#background in
let p = GnoCanvas.ellipse ~fill_color ~props:[`WIDTH_PIXELS 1; `OUTLINE_COLOR "black"] ~x1:(-3.) ~y1:(-3.) ~x2:3. ~y2:3. group in
(* geomap#circle ~group ~fill_color:"red" geo 10. in *)
p#raise_to_top ();
Hashtbl.add particules id (p:>GnomeCanvas.re_p GnoCanvas.item)
let list_separator = Str.regexp ","
@@ -41,13 +41,13 @@ let listen = fun (geomap:MapCanvas.widget) ->
let rec loop = fun ids xs ys vs ->
match ids, xs, ys, vs with
[], [], [], [] -> ()
| id::ids, x::xs, y::ys, v::vs ->
let id = int_of_string id
and wgs84 = {posn_lat=(Deg>>Rad)(fos x); posn_long=(Deg>>Rad)(fos y)} in
move_particule geomap id wgs84 (ios v);
loop ids xs ys vs
| _ -> failwith "Particules.listen loop"
[], [], [], [] -> ()
| id::ids, x::xs, y::ys, v::vs ->
let id = int_of_string id
and wgs84 = {posn_lat=(Deg>>Rad)(fos x); posn_long=(Deg>>Rad)(fos y)} in
move_particule geomap id wgs84 (ios v);
loop ids xs ys vs
| _ -> failwith "Particules.listen loop"
in
loop ids xs ys vs
in
sw/ground_segment/cockpit/saveSettings.ml
+5 -5
View File
@@ -134,15 +134,15 @@ let fill_data = fun (model:GTree.tree_store) settings airframe_xml ->
try
let unit_code =
match code_unit, unit_setting with
| Some uc, Some us ->
| Some uc, Some us ->
if uc = us then uc
else invalid_arg (Printf.sprintf "Warning: code unit in airframe (%s) and setting file (%s) are not matching for param %s\n" uc us param) (* raise Invalid_argument *)
| Some u, None | None, Some u -> u
| None, None -> ""
| Some u, None | None, Some u -> u
| None, None -> ""
and unit_airframe =
match airframe_unit with
| Some u -> u
| None -> ""
| Some u -> u
| None -> ""
in
(* Printf.fprintf stderr "param %s: unit_code=%s unit_airframe=%s\n" param unit_code unit_airframe; flush stderr; *)
Pprz.scale_of_units unit_airframe unit_code
sw/ground_segment/cockpit/sectors.ml
+15 -15
View File
@@ -7,20 +7,20 @@ 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" ->
"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" ->
| "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))
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
|x -> fprintf stderr "Sector.display: '%s' not yet\n%!" x
let display_sector = fun (geomap:MapCanvas.widget) sector ->
@@ -29,13 +29,13 @@ let display_sector = fun (geomap:MapCanvas.widget) sector ->
let load = fun geomap () ->
match GToolbox.select_file ~title:"Load sectors" ~filename:(Env.flight_plans_path // "*.xml") () with
None -> ()
| Some f ->
None -> ()
| Some f ->
try
let xml = Xml.parse_file f in
List.iter (display_sector geomap) (Xml.children xml)
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
Dtd.Prove_error(e) ->
let m = sprintf "Error while loading %s:\n%s" f (Dtd.prove_error e) in
GToolbox.message_box "Error" m
sw/ground_segment/cockpit/speech.ml
+2 -2
View File
@@ -30,8 +30,8 @@ let say = fun s ->
match os with
(* If the os is Darwin, then use "say" *)
"Linux" -> ignore (Sys.command (Printf.sprintf "spd-say '%s'&" s))
(* If the os is Linux, use "spd-say" *)
(* If the os is Linux, use "spd-say" *)
| "Darwin" -> ignore (Sys.command (Printf.sprintf "say '%s'&" s))
(* Add more cases here to enhance support *)
(* Add more cases here to enhance support *)
| _ -> ignore (Sys.command (Printf.sprintf "echo Current OS not supported by -speech option"))
)
sw/ground_segment/cockpit/strip.ml
+220 -220
View File
File diff suppressed because it is too large Load Diff
sw/ground_segment/joystick/input2ivy.ml
+136 -136
View File
@@ -23,14 +23,14 @@
*)
(* 1/26/2011 - Additional functionality added by jpeverill:
Joystick xml config files now loaded from PAPARAZZI_HOME/conf/joystick/
Exponential output setting (per channel)
Limit output setting (per channel)
Per channel trim, controlled through joystick buttons
Trim can also be saved into auxilliary file, based on aircraft, and loaded at runtime if it exists
File will be called <xml joystick profile name>.<A/C name>.trim and is saved in the conf/joystick directory as well
Division in channel mapping
Interactive keyboard trim control (primitive)
Joystick xml config files now loaded from PAPARAZZI_HOME/conf/joystick/
Exponential output setting (per channel)
Limit output setting (per channel)
Per channel trim, controlled through joystick buttons
Trim can also be saved into auxilliary file, based on aircraft, and loaded at runtime if it exists
File will be called <xml joystick profile name>.<A/C name>.trim and is saved in the conf/joystick directory as well
Division in channel mapping
Interactive keyboard trim control (primitive)
*)
open Printf
@@ -64,7 +64,7 @@ let index_of_blocks = Hashtbl.create 13
(** External C functions to access the input device *)
external stick_init : int -> int = "ml_stick_init"
(** [stick_init device] Return 0 on success. Search for a device if [device]
is the empty string *)
is the empty string *)
external stick_read : unit -> int * int * int array = "ml_stick_read"
(** Return the number of buttons, an integer of bits for the buttons values
@@ -82,13 +82,13 @@ type input =
(** Description of a message *)
type msg = {
msg_name : string;
msg_class : string;
fields : (string * Syntax.expression) list;
on_event : Syntax.expression option;
send_always : bool;
has_ac_id : bool
}
msg_name : string;
msg_class : string;
fields : (string * Syntax.expression) list;
on_event : Syntax.expression option;
send_always : bool;
has_ac_id : bool
}
(** Representation of a variable *)
type var = {
@@ -98,11 +98,11 @@ type var = {
(** Represenation of an input device, the messages to send and the variables *)
type actions = {
period_ms : int;
inputs : (string*input) list;
messages : msg list;
variables : (string*var) list;
}
period_ms : int;
inputs : (string*input) list;
messages : msg list;
variables : (string*var) list;
}
(** adjust the trim on an axis given its name *)
let trim_adjust = fun axis_name adjustment ->
@@ -111,20 +111,20 @@ let trim_adjust = fun axis_name adjustment ->
(** Get message class type *)
let get_message_type = fun class_name ->
match class_name with
"datalink" -> "Message"
| "ground" -> "Message"
| "trim_plus" -> "Trim"
| "trim_minus" -> "Trim"
| "trim_save" -> "Trim"
| _ -> failwith class_name
"datalink" -> "Message"
| "ground" -> "Message"
| "trim_plus" -> "Trim"
| "trim_minus" -> "Trim"
| "trim_save" -> "Trim"
| _ -> failwith class_name
(** Get a message description from its name (and class name) *)
(** class_names with entries above as "Message" should be listed here *)
let get_message = fun class_name msg_name ->
match class_name with
"datalink" -> snd (DL.message_of_name msg_name)
| "ground" -> snd (G.message_of_name msg_name)
| _ -> failwith class_name
"datalink" -> snd (DL.message_of_name msg_name)
| "ground" -> snd (G.message_of_name msg_name)
| _ -> failwith class_name
(** Get the A/C id from its name in conf/conf.xml *)
let ac_id_of_name = fun ac_name ->
@@ -133,8 +133,8 @@ let ac_id_of_name = fun ac_name ->
let aircraft = ExtXml.child ~select:(fun x -> Xml.attrib x "name" = ac_name) conf_xml "aircraft" in
ExtXml.int_attrib aircraft "ac_id"
with
Not_found ->
failwith (sprintf "A/C '%s' not found" ac_name)
Not_found ->
failwith (sprintf "A/C '%s' not found" ac_name)
(** Fill the index_of_settings table from var/AC/settings.xml *)
let hash_index_of_settings = fun ac_name ->
@@ -168,29 +168,29 @@ let hash_index_of_blocks = fun ac_name ->
(* Return the rank of an element in a list, first is 0 *)
let rank = fun x l ->
let rec loop i = function
[] -> raise Not_found
[] -> raise Not_found
| y::ys -> if x = y then i else loop (i+1) ys in
loop 0 l
(** Eval IndexOfEnum, IndexOfSetting and IndexOfBlock built-in functions
in an expression *)
in an expression *)
let eval_settings_and_blocks = fun field_descr expr ->
let rec loop = function
Syntax.Call ("IndexOfEnum", [Syntax.Ident enum]) -> begin
try Syntax.Int (rank enum field_descr.Pprz.enum) with
Not_found -> failwith (sprintf "IndexOfEnum: unknown value '%s'" enum)
end
| Syntax.Call ("IndexOfSetting", [Syntax.Ident var]) -> begin
Syntax.Call ("IndexOfEnum", [Syntax.Ident enum]) -> begin
try Syntax.Int (rank enum field_descr.Pprz.enum) with
Not_found -> failwith (sprintf "IndexOfEnum: unknown value '%s'" enum)
end
| Syntax.Call ("IndexOfSetting", [Syntax.Ident var]) -> begin
try Syntax.Int (Hashtbl.find index_of_settings var) with
Not_found -> failwith (sprintf "IndexOfSetting: unknown var '%s'" var)
Not_found -> failwith (sprintf "IndexOfSetting: unknown var '%s'" var)
end
| Syntax.Call ("IndexOfBlock", [Syntax.Ident name]) -> begin
| Syntax.Call ("IndexOfBlock", [Syntax.Ident name]) -> begin
try Syntax.Int (Hashtbl.find index_of_blocks name) with
Not_found -> failwith (sprintf "IndexOfBlock: unknown block '%s'" name)
Not_found -> failwith (sprintf "IndexOfBlock: unknown block '%s'" name)
end
| Syntax.Call (ident, exprs) | Syntax.CallOperator (ident, exprs) ->
| Syntax.Call (ident, exprs) | Syntax.CallOperator (ident, exprs) ->
Syntax.Call (ident, List.map loop exprs)
| e -> e in
| e -> e in
loop expr
(** Parse an XML list of input channels *)
@@ -200,16 +200,16 @@ let parse_input = fun input ->
and index = ExtXml.int_attrib x "index" in
let value =
match Xml.tag x with
"axis" ->
let trim = try ExtXml.float_attrib x "trim" with _ -> 0.0 in
let exponent = try ExtXml.float_attrib x "exponent" with _ -> 0.0 in
let limit = try ExtXml.float_attrib x "limit" with _ -> 1.0 in
let deadband = try ExtXml.int_attrib x "deadband" with _ -> 0 in
Axis (index, deadband, limit, exponent, ref trim)
| "button" -> Button index
| _ -> failwith "parse_input: unexepcted tag" in
"axis" ->
let trim = try ExtXml.float_attrib x "trim" with _ -> 0.0 in
let exponent = try ExtXml.float_attrib x "exponent" with _ -> 0.0 in
let limit = try ExtXml.float_attrib x "limit" with _ -> 1.0 in
let deadband = try ExtXml.int_attrib x "deadband" with _ -> 0 in
Axis (index, deadband, limit, exponent, ref trim)
| "button" -> Button index
| _ -> failwith "parse_input: unexepcted tag" in
(name, value))
(Xml.children input)
(Xml.children input)
(** Parse a 'à la C' expression *)
let parse_value = fun s ->
@@ -231,12 +231,12 @@ let parse_msg = fun msg ->
let fields, has_ac_id =
match get_message_type msg_class with
"Message" ->
let msg_descr = get_message msg_class msg_name in
(List.map (parse_msg_field msg_descr) (Xml.children msg),
List.mem_assoc "ac_id" msg_descr.Pprz.fields)
| "Trim" -> ([], false)
| _ -> failwith ("Unknown message class type") in
"Message" ->
let msg_descr = get_message msg_class msg_name in
(List.map (parse_msg_field msg_descr) (Xml.children msg),
List.mem_assoc "ac_id" msg_descr.Pprz.fields)
| "Trim" -> ([], false)
| _ -> failwith ("Unknown message class type") in
let on_event =
try Some (parse_value (Xml.attrib msg "on_event")) with _ -> None in
@@ -254,23 +254,23 @@ let parse_variables = fun variables ->
let l = ref [] in
List.iter (fun x ->
match Xml.tag x with
"var" ->
let name = Xml.attrib x "name"
and default = ExtXml.int_attrib x "default" in
if List.mem_assoc name !l then failwith (sprintf "Variable %s already declared" name);
"var" ->
let name = Xml.attrib x "name"
and default = ExtXml.int_attrib x "default" in
if List.mem_assoc name !l then failwith (sprintf "Variable %s already declared" name);
(* filter all "set" node for this variable *)
let set = List.filter (fun vs ->
let set = List.filter (fun vs ->
(ExtXml.tag_is vs "set") &&
(compare (ExtXml.attrib_or_default vs "var" "") name) = 0)
(Xml.children variables) in
let var_event = List.map (fun s ->
let value = ExtXml.int_attrib s "value"
and on_event = parse_value (Xml.attrib s "on_event") in
(value, on_event)
(compare (ExtXml.attrib_or_default vs "var" "") name) = 0)
(Xml.children variables) in
let var_event = List.map (fun s ->
let value = ExtXml.int_attrib s "value"
and on_event = parse_value (Xml.attrib s "on_event") in
(value, on_event)
) set in
l := (name, { value = default; var_event = var_event; }) :: !l;
()
| _ -> ()
l := (name, { value = default; var_event = var_event; }) :: !l;
()
| _ -> ()
) (Xml.children variables);
!l
@@ -288,8 +288,8 @@ let second_of_two (_,x) = x
let trim_set = fun inputs value ->
let input = my_assoc (first_of_two value) inputs in
match input with
Axis (i, deadband, limit, exponent, trim) -> trim := (second_of_two value)
| Button i -> failwith "No trim for buttons"
Axis (i, deadband, limit, exponent, trim) -> trim := (second_of_two value)
| Button i -> failwith "No trim for buttons"
(** Input the trim file if it exists *)
@@ -298,10 +298,10 @@ let parse_trim_file = fun trim_file_name inputs ->
let trim = Xml.parse_file trim_file_name in
let trim_values = List.map
(fun x ->
let axis = ExtXml.attrib x "axis"
and trimval = ExtXml.float_attrib x "value" in
(axis, trimval))
(Xml.children trim) in
let axis = ExtXml.attrib x "axis"
and trimval = ExtXml.float_attrib x "value" in
(axis, trimval))
(Xml.children trim) in
List.iter (trim_set inputs) trim_values;
end
@@ -343,8 +343,8 @@ let apply_trim = fun x trim ->
(** Access to an input value, button or axis *)
let eval_input = fun buttons axis input ->
match input with
Axis (i, deadband, limit, exponent, trim) -> (apply_trim (apply_limit (apply_exponent (apply_deadband axis.(i) deadband) exponent) limit) trim.contents)
| Button i -> (buttons lsr i) land 0x1
Axis (i, deadband, limit, exponent, trim) -> (apply_trim (apply_limit (apply_exponent (apply_deadband axis.(i) deadband) exponent) limit) trim.contents)
| Button i -> (buttons lsr i) land 0x1
(** Scale a value in the given bounds *)
let scale = fun x min max ->
@@ -360,10 +360,10 @@ let fit = fun x min max min_input max_input ->
bound v min_input max_input
(** Return a pprz RC mode
* mode > max -> 2
* mode < min -> 0
* else 1
*)
* mode > max -> 2
* mode < min -> 0
* else 1
*)
let pprz_threshold = max_input / 2
let pprz_mode = fun mode ->
if mode > pprz_threshold then 2
@@ -373,39 +373,39 @@ let pprz_mode = fun mode ->
(** Eval a function call (TO BE COMPLETED) *)
let eval_call = fun f args ->
match f, args with
"-", [a] -> - a
| "-", [a1; a2] -> a1 - a2
| "+", [a1; a2] -> a1 + a2
| "*", [a1; a2] -> a1 * a2
| "%", [a1; a2] -> a1 / a2
| "&&", [a1; a2] -> a1 land a2
| "||", [a1; a2] -> a1 lor a2
| "<", [a1; a2] -> if a1 < a2 then 1 else 0
| ">", [a1; a2] -> if a1 > a2 then 1 else 0
| "Scale", [x; min; max] -> scale (x) (min) (max)
| "Fit", [x; min; max; min_input; max_input] -> fit (x) (min) (max) (min_input) (max_input)
| "Bound", [x; min; max] -> bound (x) (min) (max)
| "PprzMode", [x] -> pprz_mode (x)
| "JoystickID", [] -> !joystick_id
| f, args -> failwith (sprintf "eval_call: unknown function '%s'" f)
"-", [a] -> - a
| "-", [a1; a2] -> a1 - a2
| "+", [a1; a2] -> a1 + a2
| "*", [a1; a2] -> a1 * a2
| "%", [a1; a2] -> a1 / a2
| "&&", [a1; a2] -> a1 land a2
| "||", [a1; a2] -> a1 lor a2
| "<", [a1; a2] -> if a1 < a2 then 1 else 0
| ">", [a1; a2] -> if a1 > a2 then 1 else 0
| "Scale", [x; min; max] -> scale (x) (min) (max)
| "Fit", [x; min; max; min_input; max_input] -> fit (x) (min) (max) (min_input) (max_input)
| "Bound", [x; min; max] -> bound (x) (min) (max)
| "PprzMode", [x] -> pprz_mode (x)
| "JoystickID", [] -> !joystick_id
| f, args -> failwith (sprintf "eval_call: unknown function '%s'" f)
(** Eval an expression *)
let eval_expr = fun buttons axis inputs variables expr ->
let rec eval = function
Syntax.Ident ident ->
Syntax.Ident ident ->
(* try input first, then variables *)
let i = match (List.mem_assoc ident inputs, List.mem_assoc ident variables) with
(true, _) -> eval_input buttons axis (List.assoc ident inputs)
| (false, true) ->
let v = List.assoc ident variables in
v.value
| (false, false) -> failwith (sprintf "eval_expr: %s not found" ident)
in
i
let i = match (List.mem_assoc ident inputs, List.mem_assoc ident variables) with
(true, _) -> eval_input buttons axis (List.assoc ident inputs)
| (false, true) ->
let v = List.assoc ident variables in
v.value
| (false, false) -> failwith (sprintf "eval_expr: %s not found" ident)
in
i
| Syntax.Int int -> int
| Syntax.Float float -> failwith "eval_expr: float"
| Syntax.Call (ident, exprs) | Syntax.CallOperator (ident, exprs) ->
eval_call ident (List.map eval exprs)
eval_call ident (List.map eval exprs)
| Syntax.Index _ -> failwith "eval_expr: index"
| Syntax.Field _ -> failwith "eval_expr: Field"
| Syntax.Deref _ -> failwith "eval_expr: deref" in
@@ -431,8 +431,8 @@ let trim_save_add_leaf = fun x channel_pair ->
let chan_name = first_list channel_pair in
let channel = second_list channel_pair in
match channel with
Axis (i, deadband, limit, exponent, trim) -> x := x.contents ^ (Printf.sprintf "<trim axis='%s' value = '%f'/>" chan_name trim.contents)
| Button i -> Printf.printf "%d" i
Axis (i, deadband, limit, exponent, trim) -> x := x.contents ^ (Printf.sprintf "<trim axis='%s' value = '%f'/>" chan_name trim.contents)
| Button i -> Printf.printf "%d" i
(** save trim settings to file *)
let trim_save = fun inputs ->
@@ -449,8 +449,8 @@ let trim_save = fun inputs ->
let trim_adjust = fun axis_name adjustment inputs ->
let input = my_assoc axis_name inputs in
match input with
Axis (i, deadband, limit, exponent, trim) -> trim := trim.contents +. adjustment
| Button i -> failwith "No trim for buttons"
Axis (i, deadband, limit, exponent, trim) -> trim := trim.contents +. adjustment
| Button i -> failwith "No trim for buttons"
(** Update variables state *)
let update_variables = fun inputs buttons axis variables ->
@@ -473,20 +473,20 @@ let execute_action = fun ac_id inputs buttons axis variables message ->
and on_event =
match message.on_event with
None -> true
| Some expr -> eval_expr buttons axis inputs variables expr <> 0 in
None -> true
| Some expr -> eval_expr buttons axis inputs variables expr <> 0 in
let previous_values = get_previous_values message.msg_name in
(* FIXME ((value <> previous) && on_event) || send_always ??? *)
if ( ( (on_event, values) <> previous_values ) || message.send_always ) && on_event then begin
let vs = if message.has_ac_id then ("ac_id", Pprz.Int ac_id) :: values else values in
match message.msg_class with
"datalink" -> DL.message_send "input2ivy" message.msg_name vs
| "ground" -> G.message_send "input2ivy" message.msg_name vs
| "trim_plus" -> trim_adjust message.msg_name trim_step inputs
| "trim_minus" -> trim_adjust message.msg_name (-.trim_step) inputs
| "trim_save" -> trim_save inputs
| c -> failwith (sprintf "execute_action: unknown class '%s'" c)
"datalink" -> DL.message_send "input2ivy" message.msg_name vs
| "ground" -> G.message_send "input2ivy" message.msg_name vs
| "trim_plus" -> trim_adjust message.msg_name trim_step inputs
| "trim_minus" -> trim_adjust message.msg_name (-.trim_step) inputs
| "trim_save" -> trim_save inputs
| c -> failwith (sprintf "execute_action: unknown class '%s'" c)
end;
record_values message.msg_name (on_event, values)
@@ -505,7 +505,7 @@ let print_inputs = fun nb_buttons buttons axis ->
(** Get the values from the input values and send messages
This is called at a rate programmed in the xml *)
This is called at a rate programmed in the xml *)
let execute_actions = fun actions ac_id ->
try
let (nb_buttons, buttons, axis) = stick_read () in
@@ -517,7 +517,7 @@ let execute_actions = fun actions ac_id ->
update_variables actions.inputs buttons axis actions.variables;
List.iter (execute_action ac_id actions.inputs buttons axis actions.variables) actions.messages
with
exc -> prerr_endline (Printexc.to_string exc)
exc -> prerr_endline (Printexc.to_string exc)
(** process keyboard commands *)
@@ -530,16 +530,16 @@ let execute_kb_action = fun actions conditions ->
ijkm for right *)
if true then begin
match ch with
101 -> trim_adjust "ly" 1.0 actions.inputs
| 115 -> trim_adjust "lx" (-1.0) actions.inputs
| 100 -> trim_adjust "lx" 1.0 actions.inputs
| 120 -> trim_adjust "ly" (-1.0) actions.inputs
| 105 -> trim_adjust "ry" 1.0 actions.inputs
| 106 -> trim_adjust "rx" (-1.0) actions.inputs
| 107 -> trim_adjust "rx" 1.0 actions.inputs
| 109 -> trim_adjust "ry" (-1.0) actions.inputs
| _ -> trim_adjust "ly" 0.0 actions.inputs
match ch with
101 -> trim_adjust "ly" 1.0 actions.inputs
| 115 -> trim_adjust "lx" (-1.0) actions.inputs
| 100 -> trim_adjust "lx" 1.0 actions.inputs
| 120 -> trim_adjust "ly" (-1.0) actions.inputs
| 105 -> trim_adjust "ry" 1.0 actions.inputs
| 106 -> trim_adjust "rx" (-1.0) actions.inputs
| 107 -> trim_adjust "rx" 1.0 actions.inputs
| 109 -> trim_adjust "ry" (-1.0) actions.inputs
| _ -> trim_adjust "ly" 0.0 actions.inputs
end;
true
@@ -593,8 +593,8 @@ let () =
(** setup stdin *) (* TODO find a better way to change trim, use a GUI ? *)
(*let tstatus = (Unix.tcgetattr Unix.stdin) in
tstatus.c_icanon <- false;
Unix.tcsetattr Unix.stdin Unix.TCSANOW tstatus;*)
tstatus.c_icanon <- false;
Unix.tcsetattr Unix.stdin Unix.TCSANOW tstatus;*)
ignore (Glib.Timeout.add actions.period_ms (fun () -> execute_actions actions ac_id; true));
(*ignore (Glib.Io.add_watch ~cond:[`IN] ~callback:(fun x -> execute_kb_action actions x) (Glib.Io.channel_of_descr Unix.stdin));*)
sw/ground_segment/multimon/test_gen_hdlc.ml
+2 -2
View File
@@ -13,14 +13,14 @@ let _ =
let i = ref 0 in
let cb = fun _ ->
incr i;
(***)
(***)
Hdlc.write_data
(let s = Printf.sprintf "coucou [%d]" !i in prerr_endline s; s);
true in
(***) ignore (Glib.Timeout.add 1000 cb); (***)
ignore (Glib.Timeout.add 90 (fun _ -> Hdlc.write_to_dsp (); true));
(** ignore (Glib.Timeout.add 100 (fun _ -> prerr_endline "x"; true)); **)
(** ignore (Glib.Timeout.add 100 (fun _ -> prerr_endline "x"; true)); **)
(** Threaded main loop (blocking write) *)
GtkThread.main ()
sw/ground_segment/tmtc/150m.ml
+9 -9
View File
@@ -1,7 +1,7 @@
(** Code example on the Ivy bus.
Agent which monitors the altitude from the ground and set the A/C in HOME mode
if it reaches 150m. Also displays this altitude and a HOME button to allow the user
to force the HOME mode *)
Agent which monitors the altitude from the ground and set the A/C in HOME mode
if it reaches 150m. Also displays this altitude and a HOME button to allow the user
to force the HOME mode *)
let (//) = Filename.concat
@@ -27,13 +27,13 @@ module Datalink_Pprz = Pprz.Messages(struct let name = "datalink" end)
(******************************* Send the message to the A/C to set it in HOME mode *)
let set_to_HOME = fun () ->
let vs = ["ac_id", Pprz.String ac_id;
"index", Pprz.Int index_pprz_mode;
"value", Pprz.Float (float autopilot_HOME_mode_value)] in
"index", Pprz.Int index_pprz_mode;
"value", Pprz.Float (float autopilot_HOME_mode_value)] in
Datalink_Pprz.message_send "dl" "SETTING" vs
(******************************* Get GPS message, display the altitude from the SRTM
model, and set to HOME if higher than 150m *)
model, and set to HOME if higher than 150m *)
let get_gps_message = fun label _sender vs ->
(* Extract data from the message *)
let alt_m = Pprz.int_assoc "alt" vs / 100
@@ -43,8 +43,8 @@ let get_gps_message = fun label _sender vs ->
(* Build the geographic position *)
let utm = { Latlong.utm_x = float utm_east;
Latlong.utm_y = float utm_north;
Latlong.utm_zone = utm_zone } in
Latlong.utm_y = float utm_north;
Latlong.utm_zone = utm_zone } in
(* Get the ground altitude from the SRTM model *)
let srtm_alt_m = Srtm.of_utm utm in
@@ -61,7 +61,7 @@ let get_gps_message = fun label _sender vs ->
(********************************* Main *********************************************)
let () =
let ivy_bus = Defivybus.default_ivy_bus in
let ivy_bus = Defivybus.default_ivy_bus in
(** Connect to the Ivy bus *)
Ivy.init "Paparazzi 150m" "READY" (fun _ _ -> ());
sw/ground_segment/tmtc/aircraft.ml
+91 -91
View File
@@ -23,47 +23,47 @@
open Latlong
type ac_cam = {
mutable phi : float; (* Rad, right = >0 *)
mutable theta : float; (* Rad, front = >0 *)
mutable target : (float * float) (* meter*meter relative *)
}
mutable phi : float; (* Rad, right = >0 *)
mutable theta : float; (* Rad, front = >0 *)
mutable target : (float * float) (* meter*meter relative *)
}
type rc_status = string (** OK, LOST, REALLY_LOST *)
type rc_mode = string (** MANUAL, AUTO, FAILSAFE *)
type fbw = {
mutable rc_status : rc_status;
mutable rc_mode : rc_mode;
mutable rc_rate : int;
mutable pprz_mode_msgs_since_last_fbw_status_msg : int;
}
mutable rc_status : rc_status;
mutable rc_mode : rc_mode;
mutable rc_rate : int;
mutable pprz_mode_msgs_since_last_fbw_status_msg : int;
}
let gps_nb_channels = 16
type svinfo = {
svid : int;
flags : int;
qi : int;
cno : int;
elev : int;
azim : int;
mutable age : int
}
svid : int;
flags : int;
qi : int;
cno : int;
elev : int;
azim : int;
mutable age : int
}
let svinfo_init = fun () ->
{
svid = 0 ;
flags = 0;
qi = 0;
cno = 0;
elev = 0;
azim = 0;
age = 0
}
svid = 0 ;
flags = 0;
qi = 0;
cno = 0;
elev = 0;
azim = 0;
age = 0
}
type inflight_calib = {
mutable if_mode : int; (* DOWN|OFF|UP *)
mutable if_val1 : float;
mutable if_val2 : float
}
mutable if_mode : int; (* DOWN|OFF|UP *)
mutable if_val1 : float;
mutable if_val2 : float
}
type horiz_mode =
Circle of Latlong.geographic * int
@@ -88,13 +88,13 @@ let add_pos_to_nav_ref = fun nav_ref ?(z = 0.) (x, y) ->
lat
in
match nav_ref with
Geo geo ->
let m_to_rad = 0.0005399568034557235 *. 0.00029088820866572159 in
let lat = lat_of_xy (geo.posn_lat +. asin (y*.m_to_rad)) 0. geo (x*.m_to_rad, y *.m_to_rad) 10 1.e-7 in
Latlong.make_geo lat (geo.posn_long +. asin (x*.m_to_rad /. cos lat))
| Utm utm ->
Geo geo ->
let m_to_rad = 0.0005399568034557235 *. 0.00029088820866572159 in
let lat = lat_of_xy (geo.posn_lat +. asin (y*.m_to_rad)) 0. geo (x*.m_to_rad, y *.m_to_rad) 10 1.e-7 in
Latlong.make_geo lat (geo.posn_long +. asin (x*.m_to_rad /. cos lat))
| Utm utm ->
Latlong.of_utm Latlong.WGS84 (Latlong.utm_add utm (x, y))
| Ltp ecef ->
| Ltp ecef ->
let ned = Latlong.make_ned [| y; x; 0. |] in (* FIXME z=0 *)
let (geo, _) = Latlong.geo_of_ecef Latlong.WGS84 (Latlong.ecef_of_ned ecef ned) in
geo
@@ -102,62 +102,62 @@ let add_pos_to_nav_ref = fun nav_ref ?(z = 0.) (x, y) ->
type waypoint = { altitude : float; wp_geo : Latlong.geographic }
type aircraft = {
mutable vehicle_type : vehicle_type;
id : string;
name : string;
flight_plan : Xml.xml;
airframe : Xml.xml;
mutable pos : Latlong.geographic;
mutable unix_time : float;
mutable itow : int32; (* ms *)
mutable roll : float;
mutable pitch : float;
mutable heading : float; (* rad, CW 0=N *)
mutable gspeed : float; (* m/s *)
mutable course : float; (* rad *)
mutable alt : float;
mutable agl : float;
mutable climb : float;
mutable nav_ref : nav_ref option;
mutable d_hmsl : float;
mutable desired_pos : Latlong.geographic;
mutable desired_altitude : float;
mutable desired_course : float;
mutable desired_climb : float;
mutable cur_block : int;
mutable cur_stage : int;
mutable throttle : float;
mutable kill_mode : bool;
mutable throttle_accu : float;
mutable rpm : float;
mutable temp : float;
mutable bat : float;
mutable amp : float;
mutable energy : int;
mutable ap_mode : int;
mutable gaz_mode : int;
mutable lateral_mode : int;
mutable horizontal_mode : int;
mutable periodic_callbacks : Glib.Timeout.id list;
cam : ac_cam;
mutable gps_mode : int;
mutable gps_Pacc : int;
mutable state_filter_mode : int;
fbw : fbw;
svinfo : svinfo array;
waypoints : (int, waypoint) Hashtbl.t;
mutable flight_time : int;
mutable stage_time : int;
mutable block_time : int;
mutable horiz_mode : horiz_mode;
dl_setting_values : float array;
mutable nb_dl_setting_values : int;
mutable survey : (Latlong.geographic * Latlong.geographic) option;
mutable last_msg_date : float;
mutable time_since_last_survey_msg : float;
mutable dist_to_wp : float;
inflight_calib : inflight_calib
}
mutable vehicle_type : vehicle_type;
id : string;
name : string;
flight_plan : Xml.xml;
airframe : Xml.xml;
mutable pos : Latlong.geographic;
mutable unix_time : float;
mutable itow : int32; (* ms *)
mutable roll : float;
mutable pitch : float;
mutable heading : float; (* rad, CW 0=N *)
mutable gspeed : float; (* m/s *)
mutable course : float; (* rad *)
mutable alt : float;
mutable agl : float;
mutable climb : float;
mutable nav_ref : nav_ref option;
mutable d_hmsl : float;
mutable desired_pos : Latlong.geographic;
mutable desired_altitude : float;
mutable desired_course : float;
mutable desired_climb : float;
mutable cur_block : int;
mutable cur_stage : int;
mutable throttle : float;
mutable kill_mode : bool;
mutable throttle_accu : float;
mutable rpm : float;
mutable temp : float;
mutable bat : float;
mutable amp : float;
mutable energy : int;
mutable ap_mode : int;
mutable gaz_mode : int;
mutable lateral_mode : int;
mutable horizontal_mode : int;
mutable periodic_callbacks : Glib.Timeout.id list;
cam : ac_cam;
mutable gps_mode : int;
mutable gps_Pacc : int;
mutable state_filter_mode : int;
fbw : fbw;
svinfo : svinfo array;
waypoints : (int, waypoint) Hashtbl.t;
mutable flight_time : int;
mutable stage_time : int;
mutable block_time : int;
mutable horiz_mode : horiz_mode;
dl_setting_values : float array;
mutable nb_dl_setting_values : int;
mutable survey : (Latlong.geographic * Latlong.geographic) option;
mutable last_msg_date : float;
mutable time_since_last_survey_msg : float;
mutable dist_to_wp : float;
inflight_calib : inflight_calib
}
let max_nb_dl_setting_values = 256 (** indexed iwth an uint8 (messages.xml) *)
sw/ground_segment/tmtc/airprox.ml
+3 -3
View File
@@ -44,7 +44,7 @@ let airprox = fun aircraft1 aircraft2 ->
z1 = aircraft1.alt and z2 = aircraft2.alt in
let alt_difference = abs_float (z1 -. z2) and
dist = distance (x1, y1) (x2, y2) in
((alt_difference < 10.0) && (dist < 100.0))
((alt_difference < 10.0) && (dist < 100.0))
(** return airprox level *)
(** level is warning if the distance between both aircraft is increasing *)
@@ -64,8 +64,8 @@ let airprox_level = fun aircraft1 aircraft2 ->
vy1 = speed1 *. (sin (halfpi -. course1)) and
vy2 = speed2 *. (sin (halfpi -. course2)) in
let d1 = distance
(x1+. vx1 *. 0.2, x2+. vx2 *. 0.2)
(y1+. vy1 *. 0.2, y2+. vy2 *. 0.2) in
(x1+. vx1 *. 0.2, x2+. vx2 *. 0.2)
(y1+. vy1 *. 0.2, y2+. vy2 *. 0.2) in
if d1 < d0 then "CRITICAL" else "WARNING"
(** send a airprox alert on ivy if there is an airprox between ac_name1 and *)
sw/ground_segment/tmtc/broadcaster.ml
+8 -8
View File
@@ -27,15 +27,15 @@ let () =
let buffer = String.create buffer_size in
let get_tcp = fun _ ->
begin
try
let n = input i buffer 0 buffer_size in
let data = String.sub buffer 0 n in
try
let n = input i buffer 0 buffer_size in
let data = String.sub buffer 0 n in
Ivy.send (sprintf "%s %s" !ivy_to (Base64.encode_string data))
with
exc -> prerr_endline (Printexc.to_string exc)
Ivy.send (sprintf "%s %s" !ivy_to (Base64.encode_string data))
with
exc -> prerr_endline (Printexc.to_string exc)
end;
true in
true in
let ginput = GMain.Io.channel_of_descr (Unix.descr_of_in_channel i) in
ignore (Glib.Io.add_watch [`IN] get_tcp ginput);
@@ -45,7 +45,7 @@ let () =
(* Forward datalink on Tcp *)
let get_ivy = fun _ args ->
try fprintf o "%s%!" (Base64.decode_string args.(0)) with
exc -> prerr_endline (Printexc.to_string exc) in
exc -> prerr_endline (Printexc.to_string exc) in
ignore (Ivy.bind get_ivy (sprintf "^%s (.*)" !ivy_from));
(* Main Loop *)
sw/ground_segment/tmtc/dia.ml
+15 -15
View File
@@ -21,8 +21,8 @@
*)
(** Encode telemetry messages in an audio stream (to be mixed with a
video stream). Listen messages from the "ground" class (a server
must be running) and write message(s) of the "DIA" class.
video stream). Listen messages from the "ground" class (a server
must be running) and write message(s) of the "DIA" class.
*)
open Printf
@@ -34,16 +34,16 @@ module Ground_Pprz = Pprz.Messages(struct let name = "ground" end)
module Sub_Pprz = Pprz.Messages(struct let name = "DIA" end)
type state = {
mutable lat : float;
mutable long : float;
mutable alt : int;
mutable lat : float;
mutable long : float;
mutable alt : int;
mutable course : int;
mutable speed : int;
mutable course : int;
mutable speed : int;
mutable cam_roll : int;
mutable cam_pitch : int;
}
mutable cam_roll : int;
mutable cam_pitch : int;
}
let state = {
lat = 0.; long = 0.; alt = 0;
@@ -115,8 +115,8 @@ let _ =
ignore (Glib.Timeout.add msg_period (fun () -> send_msg (); true));
(* Main Loop *)
let loop = Glib.Main.create true in
while Glib.Main.is_running loop do
ignore (Glib.Main.iteration true)
done
(* Main Loop *)
let loop = Glib.Main.create true in
while Glib.Main.is_running loop do
ignore (Glib.Main.iteration true)
done
sw/ground_segment/tmtc/diadec.ml
+2 -2
View File
@@ -36,8 +36,8 @@ let use_tele_message = fun buf ->
let msg = Sub_Pprz.message_of_id msg_id in
printf "%d %s\n%!" ac_id (Sub_Pprz.string_of_message msg values)
with
_ ->
Debug.call 'W' (fun f -> fprintf f "Warning, cannot use: %s\n" (Debug.xprint buf))
_ ->
Debug.call 'W' (fun f -> fprintf f "Warning, cannot use: %s\n" (Debug.xprint buf))
let _ =
sw/ground_segment/tmtc/fw_server.ml
+97 -97
View File
@@ -42,7 +42,7 @@ let rec norm_course =
let fvalue = fun x ->
match x with
Pprz.Float x -> x
Pprz.Float x -> x
| Pprz.Int32 x -> Int32.to_float x
| Pprz.Int x -> float_of_int x
| _ -> failwith (sprintf "Receive.log_and_parse: float expected, got '%s'" (Pprz.string_of_value x))
@@ -50,16 +50,16 @@ let fvalue = fun x ->
let ivalue = fun x ->
match x with
Pprz.Int x -> x
| Pprz.Int32 x -> Int32.to_int x
| _ -> failwith "Receive.log_and_parse: int expected"
Pprz.Int x -> x
| Pprz.Int32 x -> Int32.to_int x
| _ -> failwith "Receive.log_and_parse: int expected"
let format_string_field = fun s ->
let s = String.copy s in
for i = 0 to String.length s - 1 do
match s.[i] with
' ' -> s.[i] <- '_'
| _ -> ()
' ' -> s.[i] <- '_'
| _ -> ()
done;
s
@@ -78,8 +78,8 @@ let update_waypoint = fun ac wp_id p alt ->
if new_wp <> prev_wp then
Hashtbl.replace ac.waypoints wp_id new_wp
with
Not_found ->
Hashtbl.add ac.waypoints wp_id new_wp
Not_found ->
Hashtbl.add ac.waypoints wp_id new_wp
@@ -90,34 +90,34 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
let fvalue = fun x ->
let f = fvalue (value x) in
match classify_float f with
FP_infinite | FP_nan ->
let msg = sprintf "Non normal number: %f in '%s %s %s'" f ac_name msg.Pprz.name (string_of_values values) in
raise (Telemetry_error (ac_name, format_string_field msg))
match classify_float f with
FP_infinite | FP_nan ->
let msg = sprintf "Non normal number: %f in '%s %s %s'" f ac_name msg.Pprz.name (string_of_values values) in
raise (Telemetry_error (ac_name, format_string_field msg))
| _ -> f
and ivalue = fun x -> ivalue (value x) in
if not (msg.Pprz.name = "DOWNLINK_STATUS") then
a.last_msg_date <- U.gettimeofday ();
match msg.Pprz.name with
"GPS" ->
a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE";
if a.gps_mode = _3D then begin
let p = { LL.utm_x = fvalue "utm_east" /. 100.;
utm_y = fvalue "utm_north" /. 100.;
utm_zone = ivalue "utm_zone" } in
a.pos <- LL.of_utm WGS84 p;
a.unix_time <- LL.unix_time_of_tow (truncate (fvalue "itow" /. 1000.));
a.itow <- Int32.of_float (fvalue "itow");
a.gspeed <- fvalue "speed" /. 100.;
a.course <- norm_course ((Deg>>Rad)(fvalue "course" /. 10.));
if !heading_from_course then
a.heading <- a.course;
a.agl <- a.alt -. float (try Srtm.of_wgs84 a.pos with _ -> 0);
if a.gspeed > 3. && a.ap_mode = _AUTO2 then
Wind.update ac_name a.gspeed a.course
end
| "GPS_LLA" ->
"GPS" ->
a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE";
if a.gps_mode = _3D then begin
let p = { LL.utm_x = fvalue "utm_east" /. 100.;
utm_y = fvalue "utm_north" /. 100.;
utm_zone = ivalue "utm_zone" } in
a.pos <- LL.of_utm WGS84 p;
a.unix_time <- LL.unix_time_of_tow (truncate (fvalue "itow" /. 1000.));
a.itow <- Int32.of_float (fvalue "itow");
a.gspeed <- fvalue "speed" /. 100.;
a.course <- norm_course ((Deg>>Rad)(fvalue "course" /. 10.));
if !heading_from_course then
a.heading <- a.course;
a.agl <- a.alt -. float (try Srtm.of_wgs84 a.pos with _ -> 0);
if a.gspeed > 3. && a.ap_mode = _AUTO2 then
Wind.update ac_name a.gspeed a.course
end
| "GPS_LLA" ->
let lat = ivalue "lat"
and lon = ivalue "lon" in
let geo = make_geo_deg (float lat /. 1e7) (float lon /. 1e7) in
@@ -131,32 +131,32 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
a.agl <- a.alt -. float (try Srtm.of_wgs84 a.pos with _ -> 0);
a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE";
if a.gspeed > 3. && a.ap_mode = _AUTO2 then
Wind.update ac_name a.gspeed a.course
| "GPS_SOL" ->
Wind.update ac_name a.gspeed a.course
| "GPS_SOL" ->
a.gps_Pacc <- ivalue "Pacc"
| "ESTIMATOR" ->
| "ESTIMATOR" ->
a.alt <- fvalue "z";
a.climb <- fvalue "z_dot"
| "DESIRED" ->
| "DESIRED" ->
(* Trying to be compatible with old logs ... *)
begin match a.nav_ref with
Some nav_ref ->
let x = (try fvalue "x" with _ -> fvalue "desired_x")
and y = (try fvalue "y" with _ -> fvalue "desired_y") in
a.desired_pos <- Aircraft.add_pos_to_nav_ref nav_ref (x, y);
| None -> ()
Some nav_ref ->
let x = (try fvalue "x" with _ -> fvalue "desired_x")
and y = (try fvalue "y" with _ -> fvalue "desired_y") in
a.desired_pos <- Aircraft.add_pos_to_nav_ref nav_ref (x, y);
| None -> ()
end;
a.desired_altitude <- (try fvalue "altitude" with _ -> fvalue "desired_altitude");
a.desired_climb <- (try fvalue "climb" with _ -> fvalue "desired_climb");
begin try a.desired_course <- norm_course (fvalue "course") with _ -> () end
| "NAVIGATION_REF" ->
begin try a.desired_course <- norm_course (fvalue "course") with _ -> () end
| "NAVIGATION_REF" ->
a.nav_ref <- Some (Utm { utm_x = fvalue "utm_east"; utm_y = fvalue "utm_north"; utm_zone = ivalue "utm_zone" })
| "NAVIGATION_REF_LLA" ->
| "NAVIGATION_REF_LLA" ->
let lat = ivalue "lat"
and lon = ivalue "lon" in
let geo = make_geo_deg (float lat /. 1e7) (float lon /. 1e7) in
a.nav_ref <- Some (Geo geo)
| "ATTITUDE" ->
| "ATTITUDE" ->
let roll = fvalue "phi"
and pitch = fvalue "theta" in
if (List.assoc "phi" msg.Pprz.fields).Pprz._type = Pprz.Scalar "int16" then begin (* Compatibility with old message in degrees *)
@@ -168,11 +168,11 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
a.pitch <- pitch;
a.heading <- norm_course (fvalue "psi")
end
| "NAVIGATION" ->
| "NAVIGATION" ->
a.cur_block <- ivalue "cur_block";
a.cur_stage <- ivalue "cur_stage";
a.dist_to_wp <- sqrt (fvalue "dist2_wp")
| "BAT" ->
| "BAT" ->
a.throttle <- fvalue "throttle" /. 9600. *. 100.;
a.kill_mode <- ivalue "kill_auto_throttle" <> 0;
a.flight_time <- ivalue "flight_time";
@@ -181,25 +181,25 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
a.stage_time <- ivalue "stage_time";
a.block_time <- ivalue "block_time";
a.energy <- ivalue "energy"
| "FBW_STATUS" ->
| "FBW_STATUS" ->
a.bat <- fvalue "vsupply" /. 10.;
a.fbw.pprz_mode_msgs_since_last_fbw_status_msg <- 0;
a.fbw.rc_rate <- ivalue "frame_rate";
let fbw_rc_mode = ivalue "rc_status" in
a.fbw.rc_status <- (
match fbw_rc_mode with
2 -> "NONE"
| 1 -> "LOST"
| _ -> "OK" );
2 -> "NONE"
| 1 -> "LOST"
| _ -> "OK" );
let fbw_mode = ivalue "mode" in
a.fbw.rc_mode <- (
match fbw_mode with
2 -> "FAILSAFE"
| 1 -> "AUTO"
| _ -> "MANUAL" )
| "STATE_FILTER_STATUS" ->
2 -> "FAILSAFE"
| 1 -> "AUTO"
| _ -> "MANUAL" )
| "STATE_FILTER_STATUS" ->
a.state_filter_mode <- check_index (ivalue "state_filter_mode") state_filter_modes "STATE_FILTER_MODES"
| "PPRZ_MODE" ->
| "PPRZ_MODE" ->
a.vehicle_type <- FixedWing;
a.gaz_mode <- check_index (ivalue "ap_gaz") gaz_modes "AP_GAZ";
a.lateral_mode <- check_index (ivalue "ap_lateral") lat_modes "AP_LAT";
@@ -228,11 +228,11 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
a.ap_mode <- 5 (* Override and set FAIL(Safe) Mode *)
else
a.ap_mode <- check_index (ivalue "ap_mode") fixedwing_ap_modes "AP_MODE"
| "CAM" ->
| "CAM" ->
a.cam.phi <- (Deg>>Rad) (fvalue "phi");
a.cam.theta <- (Deg>>Rad) (fvalue "theta");
a.cam.target <- (fvalue "target_x", fvalue "target_y")
| "SVINFO" ->
| "SVINFO" ->
let i = ivalue "chn" in
assert(i < Array.length a.svinfo);
a.svinfo.(i) <- {
@@ -244,64 +244,64 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
azim = ivalue "Azim";
age = 0
}
| "CIRCLE" ->
| "CIRCLE" ->
begin
match a.nav_ref, a.horizontal_mode with
Some nav_ref, 2 -> (** FIXME *)
a.horiz_mode <- Circle (Aircraft.add_pos_to_nav_ref nav_ref (fvalue "center_east", fvalue "center_north"), truncate (fvalue "radius"));
if !Kml.enabled then Kml.update_horiz_mode a
| _ -> ()
end
| "SEGMENT" ->
begin
match a.nav_ref, a.horizontal_mode with
Some nav_ref, 1 -> (** FIXME *)
let p1 = Aircraft.add_pos_to_nav_ref nav_ref (fvalue "segment_east_1", fvalue "segment_north_1")
and p2 = Aircraft.add_pos_to_nav_ref nav_ref (fvalue "segment_east_2", fvalue "segment_north_2") in
a.horiz_mode <- Segment (p1, p2);
if !Kml.enabled then Kml.update_horiz_mode a
Some nav_ref, 2 -> (** FIXME *)
a.horiz_mode <- Circle (Aircraft.add_pos_to_nav_ref nav_ref (fvalue "center_east", fvalue "center_north"), truncate (fvalue "radius"));
if !Kml.enabled then Kml.update_horiz_mode a
| _ -> ()
end
| "SETTINGS" ->
| "SEGMENT" ->
begin
match a.nav_ref, a.horizontal_mode with
Some nav_ref, 1 -> (** FIXME *)
let p1 = Aircraft.add_pos_to_nav_ref nav_ref (fvalue "segment_east_1", fvalue "segment_north_1")
and p2 = Aircraft.add_pos_to_nav_ref nav_ref (fvalue "segment_east_2", fvalue "segment_north_2") in
a.horiz_mode <- Segment (p1, p2);
if !Kml.enabled then Kml.update_horiz_mode a
| _ -> ()
end
| "SETTINGS" ->
a.inflight_calib.if_val1 <- fvalue "slider_1_val";
a.inflight_calib.if_val2 <- fvalue "slider_2_val";
| "SURVEY" ->
| "SURVEY" ->
begin
a.time_since_last_survey_msg <- 0.;
match a.nav_ref with
Some nav_ref ->
let p1 = Aircraft.add_pos_to_nav_ref nav_ref (fvalue "west", fvalue "south")
and p2 = Aircraft.add_pos_to_nav_ref nav_ref (fvalue "east", fvalue "north") in
a.survey <- Some (p1, p2)
| None -> ()
Some nav_ref ->
let p1 = Aircraft.add_pos_to_nav_ref nav_ref (fvalue "west", fvalue "south")
and p2 = Aircraft.add_pos_to_nav_ref nav_ref (fvalue "east", fvalue "north") in
a.survey <- Some (p1, p2)
| None -> ()
end
| "CALIBRATION" ->
| "CALIBRATION" ->
a.throttle_accu <- fvalue "climb_sum_err"
| "DL_VALUE" ->
| "DL_VALUE" ->
let i = ivalue "index" in
if i < max_nb_dl_setting_values then begin
a.dl_setting_values.(i) <- fvalue "value";
a.nb_dl_setting_values <- max a.nb_dl_setting_values (i+1)
a.dl_setting_values.(i) <- fvalue "value";
a.nb_dl_setting_values <- max a.nb_dl_setting_values (i+1)
end else
failwith "Too much dl_setting values !!!"
| "WP_MOVED" ->
failwith "Too much dl_setting values !!!"
| "WP_MOVED" ->
begin
match a.nav_ref with
Some Utm nav_ref ->
let utm_zone = try ivalue "utm_zone" with _ -> nav_ref.utm_zone in
let p = { LL.utm_x = fvalue "utm_east";
utm_y = fvalue "utm_north";
utm_zone = utm_zone } in
update_waypoint a (ivalue "wp_id") (LL.of_utm WGS84 p) (fvalue "alt")
| _ -> () (** Can't use this message *)
Some Utm nav_ref ->
let utm_zone = try ivalue "utm_zone" with _ -> nav_ref.utm_zone in
let p = { LL.utm_x = fvalue "utm_east";
utm_y = fvalue "utm_north";
utm_zone = utm_zone } in
update_waypoint a (ivalue "wp_id") (LL.of_utm WGS84 p) (fvalue "alt")
| _ -> () (** Can't use this message *)
end
| "WP_MOVED_LLA" ->
| "WP_MOVED_LLA" ->
let lat = ivalue "lat"
and lon = ivalue "lon"
and alt = ivalue "alt" in
let geo = make_geo_deg (float lat /. 1e7) (float lon /. 1e7) in
update_waypoint a (ivalue "wp_id") geo (float alt /. 100.)
| "GENERIC_COM" ->
update_waypoint a (ivalue "wp_id") geo (float alt /. 100.)
| "GENERIC_COM" ->
let flight_time = ivalue "flight_time" in
if flight_time >= a.flight_time then begin
a.flight_time <- flight_time;
@@ -321,16 +321,16 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
a.ap_mode <- check_index (ivalue "ap_mode") fixedwing_ap_modes "AP_MODE";
a.cur_block <- ivalue "nav_block";
end
| "FORMATION_SLOT_TM" ->
| "FORMATION_SLOT_TM" ->
Dl_Pprz.message_send "ground_dl" "FORMATION_SLOT" values
| "FORMATION_STATUS_TM" ->
| "FORMATION_STATUS_TM" ->
Dl_Pprz.message_send "ground_dl" "FORMATION_STATUS" values
| "TCAS_RA" ->
| "TCAS_RA" ->
let vs = [
"ac_id", Pprz.Int (ivalue "ac_id");
"ac_id_conflict", Pprz.Int (int_of_string a.id);
"resolve", Pprz.Int (ivalue "resolve")
] in
Dl_Pprz.message_send "ground_dl" "TCAS_RESOLVE" vs
| _ -> ()
| _ -> ()
sw/ground_segment/tmtc/ihm.ml
+54 -54
View File
@@ -18,10 +18,10 @@ let print_p = fun c p ->
let print_pattern = fun a ->
match a with
Circle (p, r) -> printf "Circle (%a %d) " print_p p r
| Eight (p1, p2, r) -> printf "Eight (%a %a %d) " print_p p1 print_p p2 r
| Line (p1, p2) -> printf "Line (%a %a) " print_p p1 print_p p2
| Nop -> printf "Nop "
Circle (p, r) -> printf "Circle (%a %d) " print_p p r
| Eight (p1, p2, r) -> printf "Eight (%a %a %d) " print_p p1 print_p p2 r
| Line (p1, p2) -> printf "Line (%a %a) " print_p p1 print_p p2
| Nop -> printf "Nop "
let print_patterns = fun t ->
let i = ref 0 in
@@ -56,41 +56,41 @@ let geo_of = fun p -> of_utm WGS84 (utm_add utm_ref (float p.x, float p.y))
let send_circle = fun ac_id p r ->
let wgs84 = geo_of p in
let vs = [ "ac_id", Pprz.String ac_id;
"wp_id", Pprz.Int 1; (* FIXME *)
"alt", Pprz.Float (float p.z);
"lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat);
"long", Pprz.Float ((Rad>>Deg)wgs84.posn_long)] in
"wp_id", Pprz.Int 1; (* FIXME *)
"alt", Pprz.Float (float p.z);
"lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat);
"long", Pprz.Float ((Rad>>Deg)wgs84.posn_long)] in
GroundPprz.message_send "ihm" "MOVE_WAYPOINT" vs;
let vs = [ "ac_id", Pprz.String ac_id;
"index", Pprz.Int nav_radius_id;
"value", Pprz.Float (float r) ] in
"index", Pprz.Int nav_radius_id;
"value", Pprz.Float (float r) ] in
GroundPprz.message_send "ihm" "DL_SETTING" vs;
let vs = [ "ac_id", Pprz.String ac_id;
"block_id", Pprz.Int circle_block ] in
GroundPprz.message_send "ihm" "JUMP_TO_BLOCK" vs
"block_id", Pprz.Int circle_block ] in
GroundPprz.message_send "ihm" "JUMP_TO_BLOCK" vs
let send_line = fun ac_id p1 p2 ->
let wgs84_1 = geo_of p1
and wgs84_2 = geo_of p2 in
let vs = [ "ac_id", Pprz.String ac_id;
"wp_id", Pprz.Int 1; (* FIXME *)
"alt", Pprz.Float (float p1.z);
"lat", Pprz.Float ((Rad>>Deg)wgs84_1.posn_lat);
"long", Pprz.Float ((Rad>>Deg)wgs84_1.posn_long)] in
"wp_id", Pprz.Int 1; (* FIXME *)
"alt", Pprz.Float (float p1.z);
"lat", Pprz.Float ((Rad>>Deg)wgs84_1.posn_lat);
"long", Pprz.Float ((Rad>>Deg)wgs84_1.posn_long)] in
GroundPprz.message_send "ihm" "MOVE_WAYPOINT" vs;
let vs = [ "ac_id", Pprz.String ac_id;
"wp_id", Pprz.Int 2; (* FIXME *)
"alt", Pprz.Float (float p2.z);
"lat", Pprz.Float ((Rad>>Deg)wgs84_2.posn_lat);
"long", Pprz.Float ((Rad>>Deg)wgs84_2.posn_long)] in
"wp_id", Pprz.Int 2; (* FIXME *)
"alt", Pprz.Float (float p2.z);
"lat", Pprz.Float ((Rad>>Deg)wgs84_2.posn_lat);
"long", Pprz.Float ((Rad>>Deg)wgs84_2.posn_long)] in
GroundPprz.message_send "ihm" "MOVE_WAYPOINT" vs;
let vs = [ "ac_id", Pprz.String ac_id;
"block_id", Pprz.Int glide_block ] in
"block_id", Pprz.Int glide_block ] in
GroundPprz.message_send "ihm" "JUMP_TO_BLOCK" vs
@@ -98,26 +98,26 @@ let send_eight = fun ac_id p1 p2 r ->
let wgs84_1 = geo_of p1
and wgs84_2 = geo_of p2 in
let vs = [ "ac_id", Pprz.String ac_id;
"wp_id", Pprz.Int 1; (* FIXME *)
"alt", Pprz.Float (float p1.z);
"lat", Pprz.Float ((Rad>>Deg)wgs84_1.posn_lat);
"long", Pprz.Float ((Rad>>Deg)wgs84_1.posn_long)] in
"wp_id", Pprz.Int 1; (* FIXME *)
"alt", Pprz.Float (float p1.z);
"lat", Pprz.Float ((Rad>>Deg)wgs84_1.posn_lat);
"long", Pprz.Float ((Rad>>Deg)wgs84_1.posn_long)] in
GroundPprz.message_send "ihm" "MOVE_WAYPOINT" vs;
let vs = [ "ac_id", Pprz.String ac_id;
"wp_id", Pprz.Int 2; (* FIXME *)
"alt", Pprz.Float (float p2.z);
"lat", Pprz.Float ((Rad>>Deg)wgs84_2.posn_lat);
"long", Pprz.Float ((Rad>>Deg)wgs84_2.posn_long)] in
"wp_id", Pprz.Int 2; (* FIXME *)
"alt", Pprz.Float (float p2.z);
"lat", Pprz.Float ((Rad>>Deg)wgs84_2.posn_lat);
"long", Pprz.Float ((Rad>>Deg)wgs84_2.posn_long)] in
GroundPprz.message_send "ihm" "MOVE_WAYPOINT" vs;
let vs = [ "ac_id", Pprz.String ac_id;
"index", Pprz.Int nav_radius_id;
"value", Pprz.Float (float r) ] in
"index", Pprz.Int nav_radius_id;
"value", Pprz.Float (float r) ] in
GroundPprz.message_send "ihm" "DL_SETTING" vs;
let vs = [ "ac_id", Pprz.String ac_id;
"block_id", Pprz.Int eight_block ] in
"block_id", Pprz.Int eight_block ] in
GroundPprz.message_send "ihm" "JUMP_TO_BLOCK" vs
@@ -126,33 +126,33 @@ let send_pattern_up = fun ac_id ->
let tl = Hashtbl.find timelines ac_id in
begin
match tl with
Circle (p, r) :: _ -> send_circle ac_id p r
| Eight (p1, p2, r) :: _ -> send_eight ac_id p1 p2 r
| Line (p1, p2) :: _ -> send_line ac_id p1 p2
| Nop :: _ ->
let vs = [ "ac_id", Pprz.String ac_id;
"block_id", Pprz.Int nop_block ] in
GroundPprz.message_send "ihm" "JUMP_TO_BLOCK" vs
| [] -> failwith (Printf.sprintf "send_pattern_up: %s - empty list" ac_id)
Circle (p, r) :: _ -> send_circle ac_id p r
| Eight (p1, p2, r) :: _ -> send_eight ac_id p1 p2 r
| Line (p1, p2) :: _ -> send_line ac_id p1 p2
| Nop :: _ ->
let vs = [ "ac_id", Pprz.String ac_id;
"block_id", Pprz.Int nop_block ] in
GroundPprz.message_send "ihm" "JUMP_TO_BLOCK" vs
| [] -> failwith (Printf.sprintf "send_pattern_up: %s - empty list" ac_id)
end
with
Not_found -> failwith (Printf.sprintf "send_pattern_up: %s" ac_id)
Not_found -> failwith (Printf.sprintf "send_pattern_up: %s" ac_id)
let get_ac = fun vs ->
let ac_id = Pprz.string_assoc "ac_id" vs in
try
Hashtbl.find timelines ac_id
with
Not_found ->
add_timeline ac_id
Not_found ->
add_timeline ac_id
let insert_in_timeline values idx action =
let t = get_ac values in
let rec iter t i =
if i = 0 then action :: t else
match t with
[] -> failwith "insert_in_timeline"
| x :: xs -> x :: iter xs (i-1) in
match t with
[] -> failwith "insert_in_timeline"
| x :: xs -> x :: iter xs (i-1) in
let newt = iter t idx in
(***)print_patterns newt;
@@ -207,25 +207,25 @@ let ihm_eight_cb = fun _sender values ->
if idx = 0 then
send_pattern_up (Pprz.string_assoc "ac_id" values)
(*
let delete = fun timeline idx values ->
(* Shift left *)
let delete = fun timeline idx values ->
(* Shift left *)
for i = max 0 idx to timeline_max_length - 2 do
timeline.(i) <- timeline.(i+1)
timeline.(i) <- timeline.(i+1)
done;
if idx = 0 then
send_pattern_up (Pprz.string_assoc "ac_id" values)
send_pattern_up (Pprz.string_assoc "ac_id" values)
*)
let delete_in_timeline values idx =
let rec iter t idx =
if idx = 0 then
match t with
[] -> failwith "delete_in_timeline"
| x :: xs -> xs
[] -> failwith "delete_in_timeline"
| x :: xs -> xs
else
match t with
[] -> failwith "delete_in_timeline"
| x :: xs -> x :: iter xs (idx-1) in
[] -> failwith "delete_in_timeline"
| x :: xs -> x :: iter xs (idx-1) in
let t = get_ac values in
let newt = iter t idx in
let ac_id = Pprz.string_assoc "ac_id" values in
sw/ground_segment/tmtc/ivy2udp.ml
+11 -11
View File
@@ -78,20 +78,20 @@ let () =
let get_datalink_message = fun _ ->
begin
try
let n = input (Unix.in_channel_of_descr socket) buffer 0 buffer_size in
let b = String.sub buffer 0 n in
Debug.trace 'x' (Debug.xprint b);
let n = input (Unix.in_channel_of_descr socket) buffer 0 buffer_size in
let b = String.sub buffer 0 n in
Debug.trace 'x' (Debug.xprint b);
let use_dl_message = fun payload ->
Debug.trace 'x' (Debug.xprint (Serial.string_of_payload payload));
let (msg_id, ac_id, values) = Dl_Pprz.values_of_payload payload in
let msg = Dl_Pprz.message_of_id msg_id in
Dl_Pprz.message_send "ground_dl" msg.Pprz.name values in
let use_dl_message = fun payload ->
Debug.trace 'x' (Debug.xprint (Serial.string_of_payload payload));
let (msg_id, ac_id, values) = Dl_Pprz.values_of_payload payload in
let msg = Dl_Pprz.message_of_id msg_id in
Dl_Pprz.message_send "ground_dl" msg.Pprz.name values in
assert (PprzTransport.parse use_dl_message b = n)
assert (PprzTransport.parse use_dl_message b = n)
with
exc ->
prerr_endline (Printexc.to_string exc)
exc ->
prerr_endline (Printexc.to_string exc)
end;
true in
sw/ground_segment/tmtc/ivy_tcp_aircraft.ml
+11 -11
View File
@@ -44,20 +44,20 @@ let () =
let get_datalink_message = fun _ ->
begin
try
let n = input i buffer 0 buffer_size in
let b = String.sub buffer 0 n in
Debug.trace 'x' (Debug.xprint b);
let n = input i buffer 0 buffer_size in
let b = String.sub buffer 0 n in
Debug.trace 'x' (Debug.xprint b);
let use_dl_message = fun payload ->
Debug.trace 'x' (Debug.xprint (Serial.string_of_payload payload));
let (msg_id, ac_id, values) = Dl_Pprz.values_of_payload payload in
let msg = Dl_Pprz.message_of_id msg_id in
Dl_Pprz.message_send "ground_dl" msg.Pprz.name values in
let use_dl_message = fun payload ->
Debug.trace 'x' (Debug.xprint (Serial.string_of_payload payload));
let (msg_id, ac_id, values) = Dl_Pprz.values_of_payload payload in
let msg = Dl_Pprz.message_of_id msg_id in
Dl_Pprz.message_send "ground_dl" msg.Pprz.name values in
assert (PprzTransport.parse use_dl_message b = n)
assert (PprzTransport.parse use_dl_message b = n)
with
exc ->
prerr_endline (Printexc.to_string exc)
exc ->
prerr_endline (Printexc.to_string exc)
end;
true in
sw/ground_segment/tmtc/ivy_tcp_controller.ml
+12 -12
View File
@@ -32,21 +32,21 @@ let () =
let get_message = fun _ ->
begin
try
let n = input i buffer 0 buffer_size in
let b = String.sub buffer 0 n in
Debug.trace 'x' (Debug.xprint b);
let n = input i buffer 0 buffer_size in
let b = String.sub buffer 0 n in
Debug.trace 'x' (Debug.xprint b);
let use_tele_message = fun payload ->
Debug.trace 'x' (Debug.xprint (Serial.string_of_payload payload));
let (msg_id, ac_id, values) = Tm_Pprz.values_of_payload payload in
let msg = Tm_Pprz.message_of_id msg_id in
Tm_Pprz.message_send (string_of_int ac_id) msg.Pprz.name values in
let use_tele_message = fun payload ->
Debug.trace 'x' (Debug.xprint (Serial.string_of_payload payload));
let (msg_id, ac_id, values) = Tm_Pprz.values_of_payload payload in
let msg = Tm_Pprz.message_of_id msg_id in
Tm_Pprz.message_send (string_of_int ac_id) msg.Pprz.name values in
ignore (PprzTransport.parse use_tele_message b)
ignore (PprzTransport.parse use_tele_message b)
with
exc ->
prerr_endline (Printexc.to_string exc)
end;
exc ->
prerr_endline (Printexc.to_string exc)
end;
true in
let ginput = GMain.Io.channel_of_descr (Unix.descr_of_in_channel i) in
sw/ground_segment/tmtc/kml.ml
+40 -40
View File
@@ -52,21 +52,21 @@ let waypoint = fun utm0 alt0 wp ->
data "styleUrl" "#msn_wp_icon";
el "Point" []
[data "extrude" "1";
data "coordinates" (coordinates wgs84 a)]]
data "coordinates" (coordinates wgs84 a)]]
let icon_style = fun ?(heading=0) ?(color="ffffffff") id icon ->
el "Style" ["id", id]
[el "IconStyle" []
[data "heading" (string_of_int heading);
data "color" color;
el "Icon" []
[data "href" (sprintf "http://maps.google.com/mapfiles/kml/%s" icon)]]]
[data "heading" (string_of_int heading);
data "color" color;
el "Icon" []
[data "href" (sprintf "http://maps.google.com/mapfiles/kml/%s" icon)]]]
let line_style = fun id ?(width = 2) color ->
el "Style" ["id", id]
[el "LineStyle" []
[data "color" color;
data "width" (string_of_int width)]]
[data "color" color;
data "width" (string_of_int width)]]
let pair = fun key icon ->
el "Pair" []
@@ -104,8 +104,8 @@ let ring_around_home = fun utm0 fp ->
line_style "red" "800000ff";
el "LinearRing" []
[ data "extrude" "1";
data "altitudeMode" "relativeToGround";
data "coordinates" coords]]
data "altitudeMode" "relativeToGround";
data "coordinates" coords]]
let horiz_mode =
@@ -114,7 +114,7 @@ let horiz_mode =
line_style ~width:4 "green" "8000ff00";
el "LineString" []
[ data "altitudeMode" "absolute";
data "coordinates" ""]]
data "coordinates" ""]]
let georef_of_xml = fun xml ->
let lat0 = Latlong.deg_of_string (ExtXml.attrib xml "lat0")
@@ -152,34 +152,34 @@ let aircraft = fun ac url_flight_plan url_changes ->
let dyn_links =
List.map (fun url ->
el "NetworkLink" []
[data "name" ("Update "^ac);
el "Link" []
[data "refreshMode" "onInterval";
data "refreshInterval" "0.5";
data "href" url]])
[data "name" ("Update "^ac);
el "Link" []
[data "refreshMode" "onInterval";
data "refreshInterval" "0.5";
data "href" url]])
url_changes in
let description = data "description" "Beta version. Open and double-click on flight plan. You may need to refresh following Update objects on errors" in
kml
[el "Document" []
(description::(el "NetworkLink" []
[data "name" (ac^" flight plan");
el "Link" []
[data "href" url_flight_plan]]):: dyn_links)]
(description::(el "NetworkLink" []
[data "name" (ac^" flight plan");
el "Link" []
[data "href" url_flight_plan]]):: dyn_links)]
let change_placemark = fun ?(description="") id wgs84 alt ->
el "Change" []
[el "Placemark" ["targetId", id]
[data "description" description;
el "Point" []
[data "altitudeMode" "absolute";
data "coordinates" (coordinates wgs84 alt)]]]
[data "description" description;
el "Point" []
[data "altitudeMode" "absolute";
data "coordinates" (coordinates wgs84 alt)]]]
let link_update = fun target_href changes ->
kml
[el "NetworkLinkControl" []
[el "Update" [] (data "targetHref" target_href :: changes)]]
[el "Update" [] (data "targetHref" target_href :: changes)]]
@@ -190,12 +190,12 @@ let change_waypoint = fun ac_name wp_id wgs84 alt ->
let update_linear_ring = fun target_href id coordinates ->
kml
[el "NetworkLinkControl" []
[el "Update" []
[data "targetHref" target_href;
el "Change" []
[el "Placemark" ["targetId", id]
[el "LineString" []
[data "coordinates" coordinates]]]]]]
[el "Update" []
[data "targetHref" target_href;
el "Change" []
[el "Placemark" ["targetId", id]
[el "LineString" []
[data "coordinates" coordinates]]]]]]
let print_xml = fun ac_name file xml ->
@@ -236,15 +236,15 @@ let update_horiz_mode =
in
let alt = ac.desired_altitude in
match ac.horiz_mode with
Segment (p1, p2) ->
let coordinates = String.concat " " (List.map (fun p -> coordinates p alt) [p1; p2]) in
let kml_changes = update_linear_ring url_flight_plan "horiz_mode" coordinates in
print_xml ac.name "route_changes.kml" kml_changes
| Circle (p, r) ->
let coordinates = circle p (float r) alt in
let kml_changes = update_linear_ring url_flight_plan "horiz_mode" coordinates in
print_xml ac.name "route_changes.kml" kml_changes
| _ -> ()
Segment (p1, p2) ->
let coordinates = String.concat " " (List.map (fun p -> coordinates p alt) [p1; p2]) in
let kml_changes = update_linear_ring url_flight_plan "horiz_mode" coordinates in
print_xml ac.name "route_changes.kml" kml_changes
| Circle (p, r) ->
let coordinates = circle p (float r) alt in
let kml_changes = update_linear_ring url_flight_plan "horiz_mode" coordinates in
print_xml ac.name "route_changes.kml" kml_changes
| _ -> ()
end
@@ -270,7 +270,7 @@ let update_ac = fun ac ->
let kml_changes = link_update url_flight_plan [change] in
print_xml ac.name "ac_changes.kml" kml_changes
with
_ -> ()
_ -> ()
let build_files = fun a ->
sw/ground_segment/tmtc/link.ml
+137 -137
View File
@@ -21,7 +21,7 @@
*)
(** Agent connecting a hardware modem, usually through USB/serial, with
the Ivy sowtware bus.
the Ivy sowtware bus.
*)
open Latlong
@@ -40,15 +40,15 @@ type transport =
| Pprz2 (* Paparazzi protocol, with timestamp, A/C id, message id and CRC *)
| XBee (* Maxstream protocol, API mode *)
let transport_of_string = function
"pprz" -> Pprz
"pprz" -> Pprz
| "pprz2" -> Pprz2
| "xbee" -> XBee
| x -> invalid_arg (sprintf "transport_of_string: %s" x)
type ground_device = {
fd : Unix.file_descr; transport : transport ; baud_rate : int
}
fd : Unix.file_descr; transport : transport ; baud_rate : int
}
(* We assume here a single modem is used *)
let my_id = 0
@@ -68,23 +68,23 @@ let add_timestamp = ref None
let send_message_over_ivy = fun sender name vs ->
let timestamp =
match !add_timestamp with
None -> None
| Some start_time -> Some (Unix.gettimeofday () -. start_time) in
None -> None
| Some start_time -> Some (Unix.gettimeofday () -. start_time) in
Tm_Pprz.message_send ?timestamp sender name vs
(*********** Monitoring *************************************************)
type status = {
mutable last_rx_byte : int;
mutable last_rx_msg : int;
mutable rx_byte : int;
mutable rx_msg : int;
mutable rx_err : int;
mutable ms_since_last_msg : int;
mutable last_ping : float; (* s *)
mutable last_pong : float; (* s *)
udp_peername : Unix.sockaddr option
}
mutable last_rx_byte : int;
mutable last_rx_msg : int;
mutable rx_byte : int;
mutable rx_msg : int;
mutable rx_err : int;
mutable ms_since_last_msg : int;
mutable last_ping : float; (* s *)
mutable last_pong : float; (* s *)
udp_peername : Unix.sockaddr option
}
let statuss = Hashtbl.create 3
let dead_aircraft_time_ms = 5000
@@ -121,13 +121,13 @@ let live_aircraft = fun ac_id ->
let s = Hashtbl.find statuss ac_id in
s.ms_since_last_msg < dead_aircraft_time_ms
with
Not_found -> false
Not_found -> false
let udp_peername = fun ac_id ->
try
(Hashtbl.find statuss ac_id).udp_peername
with
Not_found -> invalid_arg "udp_peername"
Not_found -> invalid_arg "udp_peername"
let last_udp_peername = ref (Unix.ADDR_UNIX "not initialized")
let udp_read = fun fd buf pos len ->
@@ -147,13 +147,13 @@ let send_status_msg =
status.last_rx_byte <- status.rx_byte;
status.ms_since_last_msg <- status.ms_since_last_msg + status_msg_period;
let vs = ["run_time", Pprz.Int t;
"rx_bytes_rate", Pprz.Float byte_rate;
"rx_msgs_rate", Pprz.Float msg_rate;
"rx_err", Pprz.Int status.rx_err;
"rx_bytes", Pprz.Int status.rx_byte;
"rx_msgs", Pprz.Int status.rx_msg;
"ping_time", Pprz.Float (1000. *. (status.last_pong -. status.last_ping))
] in
"rx_bytes_rate", Pprz.Float byte_rate;
"rx_msgs_rate", Pprz.Float msg_rate;
"rx_err", Pprz.Int status.rx_err;
"rx_bytes", Pprz.Int status.rx_byte;
"rx_msgs", Pprz.Int status.rx_msg;
"ping_time", Pprz.Float (1000. *. (status.last_pong -. status.last_ping))
] in
send_message_over_ivy (string_of_int ac_id) "DOWNLINK_STATUS" vs)
statuss
@@ -168,9 +168,9 @@ let use_tele_message = fun ?udp_peername ?raw_data_size payload ->
send_message_over_ivy (string_of_int ac_id) msg.Pprz.name values;
update_status ?udp_peername ac_id raw_data_size (msg.Pprz.name = "PONG")
with
exc ->
prerr_endline (Printexc.to_string exc);
Debug.call 'W' (fun f -> fprintf f "Warning, cannot use: %s\n" (Debug.xprint buf));
exc ->
prerr_endline (Printexc.to_string exc);
Debug.call 'W' (fun f -> fprintf f "Warning, cannot use: %s\n" (Debug.xprint buf));
type priority = Null | Low | Normal | High
@@ -228,7 +228,7 @@ module XB = struct (** XBee module *)
fun () ->
incr x;
if !x >= 256 then
x := 1;
x := 1;
!x
let oversize_packet = 4 (* Start + msb_len + lsb_len + cksum *)
@@ -237,33 +237,33 @@ module XB = struct (** XBee module *)
let frame_data = Serial.string_of_payload frame_data in
Debug.trace 'x' (Debug.xprint frame_data);
match Xbee.api_parse_frame frame_data with
Xbee.Modem_Status x ->
Debug.trace 'x' (sprintf "getting XBee status %d" x)
| Xbee.AT_Command_Response (frame_id, comm, status, value) ->
Debug.trace 'x' (sprintf "getting XBee AT command response: %d %s %d %s" frame_id comm status (Debug.xprint value))
| Xbee.TX_Status (frame_id,status) | Xbee.TX868_Status (frame_id,status,_) ->
Debug.trace 'x' (sprintf "getting XBee TX status: %d %d" frame_id status);
if status = 1 then (* no ack, retry *)
let (packet, nb_prev_retries) = packets.(frame_id) in
if nb_prev_retries < !nb_retries then begin
packets.(frame_id) <- (packet, nb_prev_retries+1);
let o = Unix.out_channel_of_descr device.fd in
ignore (GMain.Timeout.add (10 + Random.int retry_delay)
(fun _ ->
fprintf o "%s%!" packet;
Debug.call 'y' (fun f -> fprintf f "Resending (%d) %s\n" (nb_prev_retries+1) (Debug.xprint packet));
false));
end
Xbee.Modem_Status x ->
Debug.trace 'x' (sprintf "getting XBee status %d" x)
| Xbee.AT_Command_Response (frame_id, comm, status, value) ->
Debug.trace 'x' (sprintf "getting XBee AT command response: %d %s %d %s" frame_id comm status (Debug.xprint value))
| Xbee.TX_Status (frame_id,status) | Xbee.TX868_Status (frame_id,status,_) ->
Debug.trace 'x' (sprintf "getting XBee TX status: %d %d" frame_id status);
if status = 1 then (* no ack, retry *)
let (packet, nb_prev_retries) = packets.(frame_id) in
if nb_prev_retries < !nb_retries then begin
packets.(frame_id) <- (packet, nb_prev_retries+1);
let o = Unix.out_channel_of_descr device.fd in
ignore (GMain.Timeout.add (10 + Random.int retry_delay)
(fun _ ->
fprintf o "%s%!" packet;
Debug.call 'y' (fun f -> fprintf f "Resending (%d) %s\n" (nb_prev_retries+1) (Debug.xprint packet));
false));
end
| Xbee.RX_Packet_64 (addr64, rssi, options, data) ->
Debug.trace 'x' (sprintf "getting XBee RX64: %Lx %d %d %s" addr64 rssi options (Debug.xprint data));
use_tele_message ~raw_data_size:(String.length frame_data + oversize_packet) (Serial.payload_of_string data)
| Xbee.RX868_Packet (addr64, options, data) ->
Debug.trace 'x' (sprintf "getting XBee868 RX: %Lx %d %s" addr64 options (Debug.xprint data));
use_tele_message ~raw_data_size:(String.length frame_data + oversize_packet) (Serial.payload_of_string data)
| Xbee.RX_Packet_16 (addr16, rssi, options, data) ->
Debug.trace 'x' (sprintf "getting XBee RX16: from=%x %d %d %s" addr16 rssi options (Debug.xprint data));
use_tele_message ~raw_data_size:(String.length frame_data + oversize_packet) (Serial.payload_of_string data)
| Xbee.RX_Packet_64 (addr64, rssi, options, data) ->
Debug.trace 'x' (sprintf "getting XBee RX64: %Lx %d %d %s" addr64 rssi options (Debug.xprint data));
use_tele_message ~raw_data_size:(String.length frame_data + oversize_packet) (Serial.payload_of_string data)
| Xbee.RX868_Packet (addr64, options, data) ->
Debug.trace 'x' (sprintf "getting XBee868 RX: %Lx %d %s" addr64 options (Debug.xprint data));
use_tele_message ~raw_data_size:(String.length frame_data + oversize_packet) (Serial.payload_of_string data)
| Xbee.RX_Packet_16 (addr16, rssi, options, data) ->
Debug.trace 'x' (sprintf "getting XBee RX16: from=%x %d %d %s" addr16 rssi options (Debug.xprint data));
use_tele_message ~raw_data_size:(String.length frame_data + oversize_packet) (Serial.payload_of_string data)
let send = fun ?ac_id device rf_data ->
@@ -272,9 +272,9 @@ module XB = struct (** XBee module *)
let frame_id = gen_frame_id () in
let frame_data =
if !Xbee.mode868 then
Xbee.api_tx64 ~frame_id (Int64.of_int ac_id) rf_data
Xbee.api_tx64 ~frame_id (Int64.of_int ac_id) rf_data
else
Xbee.api_tx16 ~frame_id ac_id rf_data in
Xbee.api_tx16 ~frame_id ac_id rf_data in
let packet = Xbee.Protocol.packet (Serial.payload_of_string frame_data) in
(* Store the packet for further retry *)
@@ -294,45 +294,45 @@ let udp_send = fun fd payload peername ->
assert (n = len)
let send = fun ac_id device payload _priority ->
Debug.call 's' (fun f -> fprintf f "%d\n" ac_id);
Debug.call 's' (fun f -> fprintf f "%d\n" ac_id);
if live_aircraft ac_id then
match udp_peername ac_id with
Some (Unix.ADDR_INET (peername, _port)) ->
udp_send device.fd payload peername
| _ ->
match device.transport with
Pprz ->
let o = Unix.out_channel_of_descr device.fd in
let buf = Pprz.Transport.packet payload in
Printf.fprintf o "%s" buf; flush o;
Debug.call 's' (fun f -> fprintf f "mm sending: %s\n" (Debug.xprint buf));
| XBee ->
XB.send ~ac_id device payload
Some (Unix.ADDR_INET (peername, _port)) ->
udp_send device.fd payload peername
| _ ->
match device.transport with
Pprz ->
let o = Unix.out_channel_of_descr device.fd in
let buf = Pprz.Transport.packet payload in
Printf.fprintf o "%s" buf; flush o;
Debug.call 's' (fun f -> fprintf f "mm sending: %s\n" (Debug.xprint buf));
| XBee ->
XB.send ~ac_id device payload
let broadcast = fun device payload _priority ->
if !udp then
Hashtbl.iter (* Sending to all alive A/C *)
(fun ac_id status ->
if live_aircraft ac_id then
match status.udp_peername with
Some (Unix.ADDR_INET (peername, _port)) ->
udp_send device.fd payload peername
| _ -> ())
if live_aircraft ac_id then
match status.udp_peername with
Some (Unix.ADDR_INET (peername, _port)) ->
udp_send device.fd payload peername
| _ -> ())
statuss
else
match device.transport with
Pprz ->
let o = Unix.out_channel_of_descr device.fd in
let buf = Pprz.Transport.packet payload in
Printf.fprintf o "%s" buf; flush o;
Debug.call 'l' (fun f -> fprintf f "mm sending: %s\n" (Debug.xprint buf));
| Pprz2 ->
Pprz ->
let o = Unix.out_channel_of_descr device.fd in
let buf = Pprz.Transport.packet payload in
Printf.fprintf o "%s" buf; flush o;
Debug.call 'l' (fun f -> fprintf f "mm sending: %s\n" (Debug.xprint buf));
| Pprz2 ->
let o = Unix.out_channel_of_descr device.fd in
let buf = Pprz.TransportExtended.packet payload in
Printf.fprintf o "%s" buf; flush o;
Printf.fprintf o "%s" buf; flush o;
Debug.call 'l' (fun f -> fprintf f "mm sending: %s\n" (Debug.xprint buf));
| XBee ->
| XBee ->
XB.send device payload
@@ -350,27 +350,27 @@ end
let parser_of_device = fun device ->
match device.transport with
Pprz ->
Pprz ->
let use = fun s ->
let raw_data_size = String.length (Serial.string_of_payload s) + 4 (*stx,len,ck_a, ck_b*) in
let udp_peername =
if !udp then
Some !last_udp_peername
else
None in
use_tele_message ?udp_peername ~raw_data_size s in
PprzTransport.parse use
| Pprz2 ->
let use = fun s ->
let raw_data_size = String.length (Serial.string_of_payload s) + 4 (*stx,len,ck_a, ck_b*) in
let udp_peername =
if !udp then
Some !last_udp_peername
else
None in
use_tele_message ?udp_peername ~raw_data_size s in
PprzTransport.parse use
| Pprz2 ->
let use = fun s ->
let raw_data_size = String.length (Serial.string_of_payload s) + 8 (*stx,len, timestamp, ck_a, ck_b*) in
let udp_peername =
if !udp then
Some !last_udp_peername
else
None in
use_tele_message ?udp_peername ~raw_data_size s in
let raw_data_size = String.length (Serial.string_of_payload s) + 8 (*stx,len, timestamp, ck_a, ck_b*) in
let udp_peername =
if !udp then
Some !last_udp_peername
else
None in
use_tele_message ?udp_peername ~raw_data_size s in
PprzTransportExtended.parse use
| XBee ->
| XBee ->
let module XbeeTransport = Serial.Transport (Xbee.Protocol) in
XbeeTransport.parse (XB.use_message device)
@@ -403,9 +403,9 @@ let message_uplink = fun device ->
Hashtbl.iter
(fun _m_id msg ->
match msg.Pprz.link with
Some Pprz.Forwarded -> set_forwarder msg.Pprz.name
| Some Pprz.Broadcasted -> if !ac_info then set_broadcaster msg.Pprz.name
| _ -> ())
Some Pprz.Forwarded -> set_forwarder msg.Pprz.name
| Some Pprz.Broadcasted -> if !ac_info then set_broadcaster msg.Pprz.name
| _ -> ())
Dl_Pprz.messages
let send_ping_msg = fun device ->
@@ -468,16 +468,16 @@ let () =
String.length !port >= 4 && String.sub !port 0 4 = "/dev" in (* FIXME *)
let fd =
if !udp then begin
let sockaddr = Unix.ADDR_INET (Unix.inet_addr_any, !udp_port)
and socket = Unix.socket Unix.PF_INET Unix.SOCK_DGRAM 0 in
Unix.bind socket sockaddr;
socket
let sockaddr = Unix.ADDR_INET (Unix.inet_addr_any, !udp_port)
and socket = Unix.socket Unix.PF_INET Unix.SOCK_DGRAM 0 in
Unix.bind socket sockaddr;
socket
end else if !audio then
Demod.init !port
else if on_serial_device then
Serial.opendev !port (Serial.speed_of_baudrate !baudrate) !hw_flow_control
else
Unix.openfile !port [Unix.O_RDWR] 0o640
Demod.init !port
else if on_serial_device then
Serial.opendev !port (Serial.speed_of_baudrate !baudrate) !hw_flow_control
else
Unix.openfile !port [Unix.O_RDWR] 0o640
in
(* Create the device object *)
@@ -487,23 +487,23 @@ let () =
(* The function to be called when data is available *)
let read_fd =
if !audio then
fun _io_event -> (* Demodulation *)
let (data_left, _data_right) = Demod.get_data () in
Audio.use_data data_left;
true (* Returns true to be called again *)
fun _io_event -> (* Demodulation *)
let (data_left, _data_right) = Demod.get_data () in
Audio.use_data data_left;
true (* Returns true to be called again *)
else (* Buffering and parsing *)
let buffered_parser =
(* Get the specific parser for the given transport protocol *)
let parser = parser_of_device device in
let read = if !udp then udp_read else Unix.read in
(* Wrap the parser into the buffered bytes reader *)
match Serial.input ~read parser with Serial.Closure f -> f in
fun _io_event ->
begin
try buffered_parser fd with
exc -> prerr_endline (Printexc.to_string exc)
end;
true (* Returns true to be called again *)
let buffered_parser =
(* Get the specific parser for the given transport protocol *)
let parser = parser_of_device device in
let read = if !udp then udp_read else Unix.read in
(* Wrap the parser into the buffered bytes reader *)
match Serial.input ~read parser with Serial.Closure f -> f in
fun _io_event ->
begin
try buffered_parser fd with
exc -> prerr_endline (Printexc.to_string exc)
end;
true (* Returns true to be called again *)
in
ignore (Glib.Io.add_watch [`HUP] hangup (GMain.Io.channel_of_descr fd));
ignore (Glib.Io.add_watch [`IN] read_fd (GMain.Io.channel_of_descr fd));
@@ -516,15 +516,15 @@ let () =
begin
ignore (Glib.Timeout.add status_msg_period (fun () -> send_status_msg (); true));
let start_ping = fun () ->
ignore (Glib.Timeout.add ping_msg_period (fun () -> send_ping_msg device; true));
false in
ignore (Glib.Timeout.add ping_msg_period (fun () -> send_ping_msg device; true));
false in
ignore (Glib.Timeout.add status_ping_diff start_ping);
if !aerocomm then
Aerocomm.set_data_mode fd;
Aerocomm.set_data_mode fd;
match transport with
XBee ->
XB.init device
| _ -> ()
XBee ->
XB.init device
| _ -> ()
end;
@@ -534,5 +534,5 @@ let () =
ignore (Glib.Main.iteration true)
done
with
Xml.Error e -> prerr_endline (Xml.error e); exit 1
| exn -> fprintf stderr "%s\n" (Printexc.to_string exn); exit 1
Xml.Error e -> prerr_endline (Xml.error e); exit 1
| exn -> fprintf stderr "%s\n" (Printexc.to_string exn); exit 1
sw/ground_segment/tmtc/messages.ml
+9 -9
View File
@@ -160,20 +160,20 @@ let rec one_class = fun (notebook:GPack.notebook) (ident, xml_class, sender) ->
| Some "*" ->
(* Waiting for a new sender in this class *)
let get_one = fun sender _vs ->
if not (Hashtbl.mem senders sender) then begin
Hashtbl.add senders sender ();
one_class notebook (ident, xml_class, Some sender)
end in
if not (Hashtbl.mem senders sender) then begin
Hashtbl.add senders sender ();
one_class notebook (ident, xml_class, Some sender)
end in
List.iter
(fun m -> ignore (P.message_bind (Xml.attrib m "name") get_one))
messages
(fun m -> ignore (P.message_bind (Xml.attrib m "name") get_one))
messages
| _ ->
let class_notebook = GPack.notebook ~tab_border:0 ~tab_pos:`LEFT () in
let l = match sender with None -> "" | Some s -> ":"^s in
let label = GMisc.label ~text:(ident^l) () in
ignore (notebook#append_page ~tab_label:label#coerce class_notebook#coerce);
let bind, sender_name = match sender with
None -> (fun m cb -> (P.message_bind m cb)), "*"
None -> (fun m cb -> (P.message_bind m cb)), "*"
| Some sender -> (fun m cb -> (P.message_bind ~sender m cb)), sender in
(** Forall messages in the class *)
@@ -210,12 +210,12 @@ let _ =
let xml = Pprz.messages_xml () in
let class_of = fun n ->
try
List.find (fun x -> ExtXml.attrib x "name" = n) (Xml.children xml)
List.find (fun x -> ExtXml.attrib x "name" = n) (Xml.children xml)
with Not_found -> failwith (sprintf "Unknown messages class: %s" n) in
List.map (fun x ->
match Str.split (Str.regexp ":") x with
[cl; s] -> (cl, class_of cl, Some s)
[cl; s] -> (cl, class_of cl, Some s)
| [cl] -> (x, class_of cl, None)
| _ -> failwith (sprintf "Wrong class '%s', class[:sender] expected" x))
!classes in
sw/ground_segment/tmtc/modem.ml
+24 -24
View File
@@ -25,9 +25,9 @@
open Printf
module Protocol = struct
(* Header: STX, length of (payload + checksum) *)
(* Payload: tag, data *)
(* Trailer : checksum, ETX *)
(* Header: STX, length of (payload + checksum) *)
(* Payload: tag, data *)
(* Trailer : checksum, ETX *)
let stx = Char.chr 0x02
let etx = 0x03
@@ -68,15 +68,15 @@ let msg_debug = 3
let msg_valim = 4
type status = {
mutable valim : float;
mutable cd : int;
mutable error : int;
mutable debug : int;
mutable nb_byte : int;
mutable nb_msg : int;
mutable nb_err : int;
mutable detected : int
}
mutable valim : float;
mutable cd : int;
mutable error : int;
mutable debug : int;
mutable nb_byte : int;
mutable nb_msg : int;
mutable nb_err : int;
mutable detected : int
}
let status = {
valim = 0.;
@@ -88,8 +88,8 @@ let status = {
nb_err = 0;
detected = 0;
}
(* FIXME *)
let valim = fun x -> float x *. 0.0162863 -. 1.17483
(* FIXME *)
let valim = fun x -> float x *. 0.0162863 -. 1.17483
(* FIXME *)
let parse_payload = fun payload ->
@@ -104,16 +104,16 @@ let parse_payload = fun payload ->
else begin
begin
match id with
| x when x = msg_error ->
status.error <- (Char.code payload.[1])
| x when x = msg_cd ->
status.cd <- (Char.code payload.[1])
| x when x = msg_debug ->
status.debug <- (Char.code payload.[1])
| x when x = msg_valim ->
status.valim <- (valim (Char.code payload.[2] * 0x100 + Char.code payload.[1]));
| _ -> (* Uncorrect id *)
status.nb_err <- status.nb_err + 1
| x when x = msg_error ->
status.error <- (Char.code payload.[1])
| x when x = msg_cd ->
status.cd <- (Char.code payload.[1])
| x when x = msg_debug ->
status.debug <- (Char.code payload.[1])
| x when x = msg_valim ->
status.valim <- (valim (Char.code payload.[2] * 0x100 + Char.code payload.[1]));
| _ -> (* Uncorrect id *)
status.nb_err <- status.nb_err + 1
end;
None
end
sw/ground_segment/tmtc/rotorcraft_server.ml
+70 -70
View File
@@ -44,7 +44,7 @@ let rec norm_course =
let fvalue = fun x ->
match x with
Pprz.Float x -> x
Pprz.Float x -> x
| Pprz.Int32 x -> Int32.to_float x
| Pprz.Int x -> float_of_int x
| _ -> failwith (sprintf "Receive.log_and_parse: float expected, got '%s'" (Pprz.string_of_value x))
@@ -52,28 +52,28 @@ let fvalue = fun x ->
let ivalue = fun x ->
match x with
Pprz.Int x -> x
| Pprz.Int32 x -> Int32.to_int x
| _ -> failwith "Receive.log_and_parse: int expected"
Pprz.Int x -> x
| Pprz.Int32 x -> Int32.to_int x
| _ -> failwith "Receive.log_and_parse: int expected"
(*
let i32value = fun x ->
let i32value = fun x ->
match x with
Pprz.Int32 x -> x
Pprz.Int32 x -> x
| _ -> failwith "Receive.log_and_parse: int32 expected"
*)
let foi32value = fun x ->
match x with
Pprz.Int32 x -> Int32.to_float x
| _ -> failwith "Receive.log_and_parse: int32 expected"
Pprz.Int32 x -> Int32.to_float x
| _ -> failwith "Receive.log_and_parse: int32 expected"
let format_string_field = fun s ->
let s = String.copy s in
for i = 0 to String.length s - 1 do
match s.[i] with
' ' -> s.[i] <- '_'
| _ -> ()
' ' -> s.[i] <- '_'
| _ -> ()
done;
s
@@ -92,8 +92,8 @@ let update_waypoint = fun ac wp_id p alt ->
if new_wp <> prev_wp then
Hashtbl.replace ac.waypoints wp_id new_wp
with
Not_found ->
Hashtbl.add ac.waypoints wp_id new_wp
Not_found ->
Hashtbl.add ac.waypoints wp_id new_wp
(*let get_pprz_mode = fun ap_mode ->
let mode = ref 0 in
@@ -117,27 +117,27 @@ let gps_frac = 1e7
let geo_hmsl_of_ltp = fun ned nav_ref d_hmsl ->
match nav_ref with
| Ltp nav_ref_ecef ->
let (geo, alt) = LL.geo_of_ecef LL.WGS84 (LL.ecef_of_ned nav_ref_ecef ned) in
(geo, alt +. d_hmsl)
| _ -> (LL.make_geo 0. 0., 0.)
| Ltp nav_ref_ecef ->
let (geo, alt) = LL.geo_of_ecef LL.WGS84 (LL.ecef_of_ned nav_ref_ecef ned) in
(geo, alt +. d_hmsl)
| _ -> (LL.make_geo 0. 0., 0.)
let hmsl_of_ref = fun nav_ref d_hmsl ->
match nav_ref with
| Ltp nav_ref_ecef ->
let (_, alt) = LL.geo_of_ecef LL.WGS84 nav_ref_ecef in
alt +. d_hmsl
| _ -> 0.
| Ltp nav_ref_ecef ->
let (_, alt) = LL.geo_of_ecef LL.WGS84 nav_ref_ecef in
alt +. d_hmsl
| _ -> 0.
let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
let value = fun x -> try Pprz.assoc x values with Not_found -> failwith (sprintf "Error: field '%s' not found\n" x) in
let fvalue = fun x ->
let f = fvalue (value x) in
match classify_float f with
FP_infinite | FP_nan ->
let msg = sprintf "Non normal number: %f in '%s %s %s'" f ac_name msg.Pprz.name (string_of_values values) in
raise (Telemetry_error (ac_name, format_string_field msg))
match classify_float f with
FP_infinite | FP_nan ->
let msg = sprintf "Non normal number: %f in '%s %s %s'" f ac_name msg.Pprz.name (string_of_values values) in
raise (Telemetry_error (ac_name, format_string_field msg))
| _ -> f
and ivalue = fun x -> ivalue (value x)
@@ -146,42 +146,42 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
if not (msg.Pprz.name = "DOWNLINK_STATUS") then
a.last_msg_date <- U.gettimeofday ();
match msg.Pprz.name with
"ROTORCRAFT_FP" ->
begin match a.nav_ref with
None -> (); (* No nav_ref yet *)
| Some nav_ref ->
let north = foi32value "north" /. pos_frac
and east = foi32value "east" /. pos_frac
and up = foi32value "up" /. pos_frac in
let (geo, h) = geo_hmsl_of_ltp (LL.make_ned [| north; east; -. up |]) nav_ref a.d_hmsl in
a.pos <- geo;
a.alt <- h;
let desired_east = foi32value "carrot_east" /. pos_frac
and desired_north = foi32value "carrot_north" /. pos_frac
and desired_alt = foi32value "carrot_up" /. pos_frac in
a.desired_pos <- Aircraft.add_pos_to_nav_ref nav_ref ~z:desired_alt (desired_east, desired_north);
a.desired_altitude <- desired_alt +. (hmsl_of_ref nav_ref a.d_hmsl);
a.desired_course <- foi32value "carrot_psi" /. angle_frac
(* a.desired_climb <- ?? *)
end;
let veast = foi32value "veast" /. speed_frac
and vnorth = foi32value "vnorth" /. speed_frac in
a.gspeed <- sqrt(vnorth*.vnorth +. veast*.veast);
a.climb <- foi32value "vup" /. speed_frac;
a.agl <- a.alt -. float (try Srtm.of_wgs84 a.pos with _ -> 0);
a.course <- norm_course ((Rad>>Deg) (foi32value "psi" /. angle_frac));
a.heading <- norm_course (foi32value "psi" /. angle_frac);
a.roll <- foi32value "phi" /. angle_frac;
a.pitch <- foi32value "theta" /. angle_frac;
a.throttle <- foi32value "thrust" /. 9600. *. 100.;
a.flight_time <- ivalue "flight_time";
(*if a.gspeed > 3. && a.ap_mode = _AUTO2 then
Wind.update ac_name a.gspeed a.course*)
| "GPS_INT" ->
"ROTORCRAFT_FP" ->
begin match a.nav_ref with
None -> (); (* No nav_ref yet *)
| Some nav_ref ->
let north = foi32value "north" /. pos_frac
and east = foi32value "east" /. pos_frac
and up = foi32value "up" /. pos_frac in
let (geo, h) = geo_hmsl_of_ltp (LL.make_ned [| north; east; -. up |]) nav_ref a.d_hmsl in
a.pos <- geo;
a.alt <- h;
let desired_east = foi32value "carrot_east" /. pos_frac
and desired_north = foi32value "carrot_north" /. pos_frac
and desired_alt = foi32value "carrot_up" /. pos_frac in
a.desired_pos <- Aircraft.add_pos_to_nav_ref nav_ref ~z:desired_alt (desired_east, desired_north);
a.desired_altitude <- desired_alt +. (hmsl_of_ref nav_ref a.d_hmsl);
a.desired_course <- foi32value "carrot_psi" /. angle_frac
(* a.desired_climb <- ?? *)
end;
let veast = foi32value "veast" /. speed_frac
and vnorth = foi32value "vnorth" /. speed_frac in
a.gspeed <- sqrt(vnorth*.vnorth +. veast*.veast);
a.climb <- foi32value "vup" /. speed_frac;
a.agl <- a.alt -. float (try Srtm.of_wgs84 a.pos with _ -> 0);
a.course <- norm_course ((Rad>>Deg) (foi32value "psi" /. angle_frac));
a.heading <- norm_course (foi32value "psi" /. angle_frac);
a.roll <- foi32value "phi" /. angle_frac;
a.pitch <- foi32value "theta" /. angle_frac;
a.throttle <- foi32value "thrust" /. 9600. *. 100.;
a.flight_time <- ivalue "flight_time";
(*if a.gspeed > 3. && a.ap_mode = _AUTO2 then
Wind.update ac_name a.gspeed a.course*)
| "GPS_INT" ->
a.unix_time <- LL.unix_time_of_tow (truncate (fvalue "tow" /. 1000.));
a.itow <- Int32.of_float (fvalue "tow");
a.gps_Pacc <- ivalue "pacc"
| "ROTORCRAFT_STATUS" ->
| "ROTORCRAFT_STATUS" ->
a.vehicle_type <- Rotorcraft;
a.fbw.rc_status <- get_rc_status (ivalue "rc_status");
a.fbw.rc_rate <- ivalue "frame_rate";
@@ -189,9 +189,9 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
a.ap_mode <- check_index (ivalue "ap_mode") rotorcraft_ap_modes "ROTORCRAFT_AP_MODE";
a.kill_mode <- ivalue "ap_motors_on" == 0;
a.bat <- fvalue "vsupply" /. 10.
| "STATE_FILTER_STATUS" ->
| "STATE_FILTER_STATUS" ->
a.state_filter_mode <- check_index (ivalue "state_filter_mode") state_filter_modes "STATE_FILTER_MODES"
| "INS_REF" ->
| "INS_REF" ->
let x = foi32value "ecef_x0" /. 100.
and y = foi32value "ecef_y0" /. 100.
and z = foi32value "ecef_z0" /. 100.
@@ -200,22 +200,22 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
let nav_ref_ecef = LL.make_ecef [| x; y; z |] in
a.nav_ref <- Some (Ltp nav_ref_ecef);
a.d_hmsl <- hmsl -. alt;
| "ROTORCRAFT_NAV_STATUS" ->
| "ROTORCRAFT_NAV_STATUS" ->
a.block_time <- ivalue "block_time";
a.stage_time <- ivalue "stage_time";
a.cur_block <- ivalue "cur_block";
a.cur_stage <- ivalue "cur_stage";
a.horizontal_mode <- check_index (ivalue "horizontal_mode") horiz_modes "AP_HORIZ";
(*a.dist_to_wp <- sqrt (fvalue "dist2_wp")*)
| "WP_MOVED_ENU" ->
(*a.dist_to_wp <- sqrt (fvalue "dist2_wp")*)
| "WP_MOVED_ENU" ->
begin
match a.nav_ref with
Some nav_ref ->
let east = foi32value "east" /. pos_frac
and north = foi32value "north" /. pos_frac
and up = foi32value "up" /. pos_frac in
let (geo, h) = geo_hmsl_of_ltp (LL.make_ned [| north; east; -. up |]) nav_ref a.d_hmsl in
update_waypoint a (ivalue "wp_id") geo h;
| None -> (); (** Can't use this message *)
Some nav_ref ->
let east = foi32value "east" /. pos_frac
and north = foi32value "north" /. pos_frac
and up = foi32value "up" /. pos_frac in
let (geo, h) = geo_hmsl_of_ltp (LL.make_ned [| north; east; -. up |]) nav_ref a.d_hmsl in
update_waypoint a (ivalue "wp_id") geo h;
| None -> (); (** Can't use this message *)
end
| _ -> ()
| _ -> ()
sw/ground_segment/tmtc/server.ml
+166 -166
View File
File diff suppressed because it is too large Load Diff
sw/ground_segment/tmtc/settings.ml
+1 -1
View File
@@ -60,7 +60,7 @@ let one_ac = fun (notebook:GPack.notebook) ac_name ->
(* Bind to values updates *)
let get_dl_value = fun _sender vs ->
settings#set (Pprz.int_assoc "index" vs) (Pprz.float_assoc "value" vs)
in
in
ignore (Tele_Pprz.message_bind "DL_VALUE" get_dl_value);
(* Get the aiframe file *)
sw/ground_segment/tmtc/stereo_demod.ml
+17 -17
View File
@@ -33,13 +33,13 @@ module PprzTransport = Serial.Transport(Tele_Pprz)
(** Monitoring of the message reception *)
type status = {
mutable ac_id : string;
mutable rx_byte : int;
mutable rx_msg : int;
mutable rx_err : int
}
mutable ac_id : string;
mutable rx_byte : int;
mutable rx_msg : int;
mutable rx_err : int
}
(** Ivy messages are initially tagged "modem" and with the A/C
id as soon as it is identified (IDENT message) *)
id as soon as it is identified (IDENT message) *)
let make_status = fun id ->
{ ac_id = id; rx_byte = 0; rx_msg = 0; rx_err = 0 }
@@ -69,13 +69,13 @@ let listen_pprz_modem = fun pprz_message_cb devdsp ->
(** Callback for available chars *)
let cb = fun status buffer data ->
(** Accumulate in a buffer *)
let b = !buffer ^ data in
Debug.call 'M' (fun f -> fprintf f "Pprz buffer: %s\n" (Debug.xprint b));
let b = !buffer ^ data in
Debug.call 'M' (fun f -> fprintf f "Pprz buffer: %s\n" (Debug.xprint b));
(** Parse as pprz message and ... *)
let x = PprzTransport.parse (use_pprz_buf status) b in
status.rx_err <- !PprzTransport.nb_err;
let x = PprzTransport.parse (use_pprz_buf status) b in
status.rx_err <- !PprzTransport.nb_err;
(** ... remove from the buffer the chars which have been used *)
buffer := String.sub b x (String.length b - x)
buffer := String.sub b x (String.length b - x)
in
let buffer_left = ref "" and buffer_right = ref "" in
let cb_stereo = fun _ ->
@@ -100,12 +100,12 @@ let send_modem_msg = fun status ->
rx_msg := status.rx_msg;
rx_byte := status.rx_byte;
let vs = ["run_time", Pprz.Int t;
"rx_bytes_rate", Pprz.Float byte_rate;
"rx_msgs_rate", Pprz.Float msg_rate;
"rx_err", Pprz.Int status.rx_err;
"rx_bytes", Pprz.Int status.rx_byte;
"rx_msgs", Pprz.Int status.rx_msg
] in
"rx_bytes_rate", Pprz.Float byte_rate;
"rx_msgs_rate", Pprz.Float msg_rate;
"rx_err", Pprz.Int status.rx_err;
"rx_bytes", Pprz.Int status.rx_byte;
"rx_msgs", Pprz.Int status.rx_msg
] in
Tele_Pprz.message_send status.ac_id "DOWNLINK_STATUS" vs
(* main loop *)
sw/ground_segment/tmtc/wind.ml
+50 -50
View File
@@ -24,26 +24,26 @@
(*
Wind speed and direction are estimated from a dataset of ground speeds,
with the hypothesis that the airspeed is constant. This estimation is computed
by solving an optimization problem. The Nelder-Mead method is used
(http://en.wikipedia.org/wiki/Nelder-Mead_method).
with the hypothesis that the airspeed is constant. This estimation is computed
by solving an optimization problem. The Nelder-Mead method is used
(http://en.wikipedia.org/wiki/Nelder-Mead_method).
Let GS(i) a set of n recorded ground speed vectors and W the wind speed.
The norm of the (hypothetically constant) mean airspeed is
Let GS(i) a set of n recorded ground speed vectors and W the wind speed.
The norm of the (hypothetically constant) mean airspeed is
as = 1/n sum(norm(GS(i)-W))
Let
Let
stderr = 1/n sum (norm(GS(i)-W)-as)^2
The minimization of stderr, on the W decision variable, returns an estimation
of W.
The minimization of stderr, on the W decision variable, returns an estimation
of W.
Remarks:
- GS(i) actually is the sequence of the _last_ recorded ground speeds.
- In the "isotropic" implementation, each sample is weighted by its relative
difference in direction to the other samples.
Remarks:
- GS(i) actually is the sequence of the _last_ recorded ground speeds.
- In the "isotropic" implementation, each sample is weighted by its relative
difference in direction to the other samples.
*)
@@ -91,23 +91,23 @@ let simplex p fmax step max_iter precision =
let vr = calcnew vs.c.p vb (-1.) in
let fvr = f vr in
let new_vs =
if fvr > vs.a.f then
let ve = calcnew vs.c.p vb (-2.) in
let fve = f ve in
if fve > fvr then shift ve fve vs
else shift vr fvr vs
else
let vc = calcnew vs.c.p vb 0.5 in
let fvc = f vc in
if fvc > vs.b.f || fvr > vs.b.f then
let v = if fvr > fvc then {p = vr; f = fvr} else {p = vc; f = fvc} in
if v.f <= vs.b.f then {vs with c = v}
else if v.f > vs.a.f then shiftpv v vs
else {vs with b = v; c = vs.b}
else
let vcb = calcnew vs.b.p vs.a.p 0.5
and vcc = calcnew vs.c.p vs.a.p 0.5 in
triangle_sort {vs with b = {p = vcb; f = f vcb}; c = {p = vcc; f = f vcc}} in
if fvr > vs.a.f then
let ve = calcnew vs.c.p vb (-2.) in
let fve = f ve in
if fve > fvr then shift ve fve vs
else shift vr fvr vs
else
let vc = calcnew vs.c.p vb 0.5 in
let fvc = f vc in
if fvc > vs.b.f || fvr > vs.b.f then
let v = if fvr > fvc then {p = vr; f = fvr} else {p = vc; f = fvc} in
if v.f <= vs.b.f then {vs with c = v}
else if v.f > vs.a.f then shiftpv v vs
else {vs with b = v; c = vs.b}
else
let vcb = calcnew vs.b.p vs.a.p 0.5
and vcc = calcnew vs.c.p vs.a.p 0.5 in
triangle_sort {vs with b = {p = vcb; f = f vcb}; c = {p = vcc; f = f vcc}} in
loop (num_iter + 1) new_vs end
else vs.a in
@@ -123,15 +123,15 @@ let isotropic_wind wind_init speeds precision =
let air_speeds = Array.map (fun speed -> cart2polar (vect_sub speed wind)) speeds in
let weights =
Array.mapi
(fun i airi ->
let sum = ref 0. in
for j = 0 to n-1 do
if j <> i then
sum := !sum +.
norm_angle_rad (abs_float (airi.theta2D -. air_speeds.(j).theta2D)) /. m_pi
done;
!sum /. (float (n-1)))
air_speeds in
(fun i airi ->
let sum = ref 0. in
for j = 0 to n-1 do
if j <> i then
sum := !sum +.
norm_angle_rad (abs_float (airi.theta2D -. air_speeds.(j).theta2D)) /. m_pi
done;
!sum /. (float (n-1)))
air_speeds in
let sum_weights = Array.fold_left (+.) 0. weights in
let mean = ref 0. in
@@ -157,7 +157,7 @@ let isotropic_wind wind_init speeds precision =
(* val wind : Geometry_2d.pt_2D -> Geometry_2d.pt_2D array -> float
-> (Geometry_2d.pt_2Dfloat * float * float) *)
-> (Geometry_2d.pt_2Dfloat * float * float) *)
(** [wind wind_init speeds precision] returns the wind and air speed mean and std dev. *)
let wind wind_init speeds precision =
let mean wind =
@@ -171,10 +171,10 @@ let wind wind_init speeds precision =
let m = mean wind in
let sum =
Array.fold_left
(fun acc speed ->
let err = vect_norm (vect_sub speed wind) -. m in
acc +. err *. err)
0. speeds in
(fun acc speed ->
let err = vect_norm (vect_sub speed wind) -. m in
acc +. err *. err)
0. speeds in
sum /. float (Array.length speeds) in
let step = 2. and max_iter = 100 in
@@ -183,11 +183,11 @@ let wind wind_init speeds precision =
(wind.p, mean wind.p, -.wind.f)
type wind_ac = {
speeds : Geometry_2d.pt_2D option array;
mutable index : int;
mutable length : int;
mutable wind_init : Geometry_2d.pt_2D
}
speeds : Geometry_2d.pt_2D option array;
mutable index : int;
mutable length : int;
mutable wind_init : Geometry_2d.pt_2D
}
let h = Hashtbl.create 17
@@ -210,7 +210,7 @@ let update = fun id r course ->
let speed = polar2cart {r2D = r; theta2D = theta} in
let wind_ac = Hashtbl.find h id in
let i = truncate (float (Array.length wind_ac.speeds) *. course /. 2. /. Latlong.pi) in
(* Printf.printf "i=%d\n%!" i; *)
(* Printf.printf "i=%d\n%!" i; *)
wind_ac.speeds.(i) <- Some speed
let compute = fun compute_wind id ->
@@ -218,7 +218,7 @@ let compute = fun compute_wind id ->
let wind_ac = Hashtbl.find h id in
let speeds = List.fold_right (fun s r -> match s with Some s -> s::r | None -> r) (Array.to_list wind_ac.speeds) [] in
let speeds = Array.of_list speeds in
(* Printf.printf "l=%d\n%!" (Array.length speeds); *)
(* Printf.printf "l=%d\n%!" (Array.length speeds); *)
if Array.length speeds >= 3 then begin
let wind_init = wind_ac.wind_init in
let (wind, mean, stddev) = compute_wind wind_init speeds precision in
sw/lib/ocaml/base64.ml
+47 -47
View File
@@ -28,7 +28,7 @@
(** Exception raised when there's an attempt to encode a chunk incorrectly *)
exception Invalid_encode_chunk of int
(** The character map of all base64 characters *)
(** The character map of all base64 characters *)
let char_map = [|
'A'; 'B'; 'C'; 'D'; 'E'; 'F'; 'G'; 'H'; 'I'; 'J'; 'K'; 'L'; 'M';
@@ -37,12 +37,12 @@ let char_map = [|
'n'; 'o'; 'p'; 'q'; 'r'; 's'; 't'; 'u'; 'v'; 'w'; 'x'; 'y'; 'z';
'0'; '1'; '2'; '3'; '4'; '5'; '6'; '7'; '8'; '9'; '+'; '/'|]
(**
Functions for encoding
*)
(**
Functions for encoding
*)
(** Encode a chunk. The chunk is either a 1, 2, or 3 element array. *)
(** Encode a chunk. The chunk is either a 1, 2, or 3 element array. *)
let encode_chunk chars =
let llength = List.length chars in
@@ -56,7 +56,7 @@ let encode_chunk chars =
if (llength < 2) then (
chunk.[1] <- char_map.(tmpa);
chunk;
) else (
) else (
let b = List.nth chars 1 in
let tmpb = ((Char.code b) lsr 4) in
let tmpa2 = ((Char.code b) land 0x0f) lsl 2 in
@@ -64,18 +64,18 @@ let encode_chunk chars =
if (llength < 3) then (
chunk.[2] <- char_map.(tmpa2);
chunk
) else (
) else (
let c = List.nth chars 2 in
let tmpb2 = ((Char.code c) land 0xc0) lsr 6 in
chunk.[2] <- char_map.(tmpa2 lor tmpb2);
chunk.[3] <- char_map.((Char.code c) land 0x3f);
chunk
)
)
)
)
(** Stream chunk encoder.
(** Stream chunk encoder.
Use ``Stream.from'' to produce a stream of encoded data from a data stream. *)
Use ``Stream.from'' to produce a stream of encoded data from a data stream. *)
let encode_stream_chunk data_stream cnt =
let stream_empty s =
@@ -85,42 +85,42 @@ let encode_stream_chunk data_stream cnt =
with Stream.Failure -> false in
if (stream_empty data_stream) then (
None
) else (
) else (
let next = Stream.npeek 3 data_stream in
List.iter (fun x -> Stream.junk data_stream) next;
(* We don't do 76 here as they're in blocks of 4. *)
Some (encode_chunk next ^
(if (((cnt + 1) mod 19) = 0) then "\013\n" else ""))
)
(if (((cnt + 1) mod 19) = 0) then "\013\n" else ""))
)
(** Get a Stream of encoded data from the given stream of data. *)
(** Get a Stream of encoded data from the given stream of data. *)
let encode data_stream =
Stream.from (encode_stream_chunk data_stream)
(** Base64 encode the string data into a base64 encoded string. *)
(** Base64 encode the string data into a base64 encoded string. *)
let encode_to_string data_stream =
let buf = Buffer.create 512 in
Stream.iter (fun c -> Buffer.add_string buf c) (encode data_stream);
Buffer.contents buf
(** Base64 encode a string *)
(** Base64 encode a string *)
let encode_string s = encode_to_string (Stream.of_string s)
(* ---------------------------------------------------------------------- *)
(**
Functions for decoding
*)
(**
Functions for decoding
*)
(** Exception raised when there's a problem with the input stream. *)
(** Exception raised when there's a problem with the input stream. *)
exception Invalid_decode_chunk of int
(** Reverse mapping of character to its index in the char_map *)
(** Reverse mapping of character to its index in the char_map *)
let char_index =
let rv = Array.make 256 (-1) in
@@ -130,34 +130,34 @@ let char_index =
done;
rv
(** Is the given character a valid base64 character? *)
(** Is the given character a valid base64 character? *)
let is_base64_char c =
char_index.(Char.code c) != -1
(** Decode a chunk represented as a list of characters. The chunk must be 2, 3, or 4 elements large. *)
(** Decode a chunk represented as a list of characters. The chunk must be 2, 3, or 4 elements large. *)
let decode_chunk chars =
let rv = Buffer.create 3 in
let fchars = (List.filter (fun c -> c != '=') chars) in
let packer = List.fold_left (fun o x -> (o lsl 6) lor x) 0
(List.map (fun c -> char_index.(Char.code c)) fchars) in
(List.map (fun c -> char_index.(Char.code c)) fchars) in
(
match List.length fchars with
| 4 ->
Buffer.add_char rv (Char.chr (0xff land (packer lsr 16)));
Buffer.add_char rv (Char.chr (0xff land (packer lsr 8)));
Buffer.add_char rv (Char.chr (0xff land packer));
| 3 ->
Buffer.add_char rv (Char.chr (0xff land ((packer lsl 6) lsr 16)));
Buffer.add_char rv (Char.chr (0xff land ((packer lsl 6) lsr 8)));
| 2 ->
Buffer.add_char rv (Char.chr (0xff land ((packer lsl 12) lsr 16)));
| _ -> raise (Invalid_decode_chunk(List.length fchars));
match List.length fchars with
| 4 ->
Buffer.add_char rv (Char.chr (0xff land (packer lsr 16)));
Buffer.add_char rv (Char.chr (0xff land (packer lsr 8)));
Buffer.add_char rv (Char.chr (0xff land packer));
| 3 ->
Buffer.add_char rv (Char.chr (0xff land ((packer lsl 6) lsr 16)));
Buffer.add_char rv (Char.chr (0xff land ((packer lsl 6) lsr 8)));
| 2 ->
Buffer.add_char rv (Char.chr (0xff land ((packer lsl 12) lsr 16)));
| _ -> raise (Invalid_decode_chunk(List.length fchars));
);
Buffer.contents rv
(** Decode a stream of base64 characters into a stream of 3 or fewer byte strings. *)
(** Decode a stream of base64 characters into a stream of 3 or fewer byte strings. *)
let decode data_stream =
let rec find_next x =
@@ -169,35 +169,35 @@ let decode data_stream =
if (is_base64_char(rv)) then
Some rv
else (find_next x)
) in
) in
let clean_stream = Stream.from find_next in
let get_block x =
try
let chunk = Stream.npeek 4 clean_stream in
List.iter (fun x -> Stream.junk clean_stream) chunk;
match chunk with
[] -> None
| _ -> Some(decode_chunk chunk)
[] -> None
| _ -> Some(decode_chunk chunk)
with Stream.Failure -> None in
Stream.from get_block
(** Base64 decode the stream of base64 encoded data into a string. *)
(** Base64 decode the stream of base64 encoded data into a string. *)
let decode_to_string data_stream =
let buf = Buffer.create 512 in
Stream.iter (fun c -> Buffer.add_string buf c) (decode data_stream);
Buffer.contents buf
(** Base64 decode a string to a string *)
(** Base64 decode a string to a string *)
let decode_string s = decode_to_string (Stream.of_string s)
(**
Functions for testing
*)
(**
Functions for testing
*)
(** Simple test function. *)
(** Simple test function. *)
let test() =
let wordlist = ["A";"AB";"ABC";"Dustin";String.create 128] in
@@ -208,7 +208,7 @@ let test() =
List.iter (fun x ->
Stream.iter print_string (encode (Stream.of_string x));
print_newline()
) wordlist;
) wordlist;
print_endline("Decode:");
List.iter (fun x -> print_endline(decode_string (encode_string x)))
wordlist
sw/lib/ocaml/debug.ml
+7 -7
View File
@@ -1,4 +1,4 @@
(*
(*
* Debugging facilities
*
* Copyright (C) 2004 CENA/ENAC, Pascal Brisset, Antoine Drouin
@@ -26,12 +26,12 @@ let level = ref (try Sys.getenv "PPRZ_DEBUG" with Not_found -> "")
let log = ref stderr
let call lev f =
assert( (* assert permet au compilo de tout virer avec l'option -noassert *)
if (String.contains !level '*' || String.contains !level lev)
then begin
f !log;
flush !log
end;
true)
if (String.contains !level '*' || String.contains !level lev)
then begin
f !log;
flush !log
end;
true)
let trace lev s = call lev (fun f -> Printf.fprintf f "%s\n" s)
sw/lib/ocaml/defivybus.ml
+3 -3
View File
@@ -23,8 +23,8 @@
*)
let default_ivy_bus = String.copy (
try (Sys.getenv "IVY_BUS" )
with Not_found ->
(if Os_calls.contains (Os_calls.os_name) "Darwin" then
with Not_found ->
(if Os_calls.contains (Os_calls.os_name) "Darwin" then
"224.255.255.255:2010"
else
else
"127.255.255.255:2010"))
sw/lib/ocaml/editAirframe.ml
+26 -26
View File
@@ -30,29 +30,29 @@ exception No_param of string
let get = fun xml param ->
let rec iter_get prefix xml =
match xml with
Nethtml.Element ("define", params, children)
when List.exists (fun (p, v) -> p = "name" && (prefix^v) = param) params ->
let old_val = snd (List.find (fun (p, v) -> p = "value") params)
and units =
try Some (snd (List.find (fun (p, v) -> p = "unit") params))
with Not_found -> None
and code_units =
try Some (snd (List.find (fun (p, v) -> p = "code_unit") params))
with Not_found -> None
in
raise (Got_it (old_val, units, code_units))
| Nethtml.Element (block, params, children) ->
let new_prefix =
List.fold_left (fun acc (p, v) -> if p = "prefix" then v^acc else acc) prefix params in
List.iter (iter_get new_prefix) children
| Nethtml.Data s -> ()
match xml with
Nethtml.Element ("define", params, children)
when List.exists (fun (p, v) -> p = "name" && (prefix^v) = param) params ->
let old_val = snd (List.find (fun (p, v) -> p = "value") params)
and units =
try Some (snd (List.find (fun (p, v) -> p = "unit") params))
with Not_found -> None
and code_units =
try Some (snd (List.find (fun (p, v) -> p = "code_unit") params))
with Not_found -> None
in
raise (Got_it (old_val, units, code_units))
| Nethtml.Element (block, params, children) ->
let new_prefix =
List.fold_left (fun acc (p, v) -> if p = "prefix" then v^acc else acc) prefix params in
List.iter (iter_get new_prefix) children
| Nethtml.Data s -> ()
in
try
iter_get "" xml;
raise (No_param param)
with
Got_it result -> result
Got_it result -> result
@@ -61,13 +61,13 @@ let set = fun xml param newval ->
let update_param params =
List.map (fun (p, v) -> if p = "value" then (p, newval) else (p, v)) params in
match xml with
Nethtml.Element ("define", params, children)
when List.exists (fun (p, v) -> p = "name" && (prefix^v) = param) params ->
Nethtml.Element ("define", update_param params, children)
| Nethtml.Element (block, params, children) ->
let new_prefix =
List.fold_left (fun acc (p, v) -> if p = "prefix" then v^acc else acc) prefix params in
Nethtml.Element (block, params, List.map (iter_replace new_prefix) children)
| Nethtml.Data s -> Nethtml.Data s
Nethtml.Element ("define", params, children)
when List.exists (fun (p, v) -> p = "name" && (prefix^v) = param) params ->
Nethtml.Element ("define", update_param params, children)
| Nethtml.Element (block, params, children) ->
let new_prefix =
List.fold_left (fun acc (p, v) -> if p = "prefix" then v^acc else acc) prefix params in
Nethtml.Element (block, params, List.map (iter_replace new_prefix) children)
| Nethtml.Data s -> Nethtml.Data s
in
iter_replace "" xml
sw/lib/ocaml/env.ml
+9 -9
View File
@@ -32,13 +32,13 @@ let paparazzi_src =
try
Sys.getenv "PAPARAZZI_SRC"
with
_ -> "/usr/share/paparazzi"
_ -> "/usr/share/paparazzi"
let paparazzi_home =
try
Sys.getenv "PAPARAZZI_HOME"
with
_ -> Filename.concat (Sys.getenv "HOME") "paparazzi"
_ -> Filename.concat (Sys.getenv "HOME") "paparazzi"
let flight_plans_path = paparazzi_home // "conf" // "flight_plans"
@@ -59,13 +59,13 @@ let expand_ac_xml = fun ?(raise_exception = true) ac_conf ->
try
ExtXml.parse_file file
with
Failure msg ->
if raise_exception then
failwith msg
else begin
prerr_endline msg;
make_element "parse error" ["file",a; "msg", msg] []
end in
Failure msg ->
if raise_exception then
failwith msg
else begin
prerr_endline msg;
make_element "parse error" ["file",a; "msg", msg] []
end in
let parse = fun a ->
List.map
sw/lib/ocaml/expr_syntax.ml
+16 -16
View File
@@ -40,18 +40,18 @@ type expression =
let c_var_of_ident = fun x -> "_var_" ^ x
let rec sprint = function
Ident i when i.[0] = '$' -> sprintf "%s" (c_var_of_ident (String.sub i 1 (String.length i - 1)))
Ident i when i.[0] = '$' -> sprintf "%s" (c_var_of_ident (String.sub i 1 (String.length i - 1)))
| Ident i -> sprintf "%s" i
| Int i -> sprintf "%d" i
| Float i -> sprintf "%f" i
| CallOperator (op, [e1;e2]) ->
sprintf "(%s%s%s)" (sprint e1) op (sprint e2)
sprintf "(%s%s%s)" (sprint e1) op (sprint e2)
| CallOperator (op, [e1]) ->
sprintf "%s(%s)" op (sprint e1)
sprintf "%s(%s)" op (sprint e1)
| CallOperator (_,_) -> failwith "Operator should be binary or unary"
| Call (i, es) ->
let ses = List.map sprint es in
sprintf "%s(%s)" i (String.concat "," ses)
let ses = List.map sprint es in
sprintf "%s(%s)" i (String.concat "," ses)
| Index (i,e) -> sprintf "%s[%s]" i (sprint e)
| Field (i,f) -> sprintf "%s.%s" i f
| Deref (e,f) -> sprintf "(%s)->%s" (sprint e) f
@@ -97,21 +97,21 @@ let unexpected = fun kind x ->
let rec check_expression = fun e ->
match e with
Ident i when i.[0] = '$' -> ()
| Ident i ->
Ident i when i.[0] = '$' -> ()
| Ident i ->
if not (List.mem i variables) then
unexpected "ident" i
| Int _ | Float _ | CallOperator _ -> ()
| Call (i, es) ->
unexpected "ident" i
| Int _ | Float _ | CallOperator _ -> ()
| Call (i, es) ->
if not (List.mem i functions) then
unexpected "function" i;
unexpected "function" i;
List.iter check_expression es
| Index (i,e) ->
| Index (i,e) ->
if not (List.mem i variables) then
unexpected "ident" i;
unexpected "ident" i;
check_expression e
| Field (i, _field) ->
| Field (i, _field) ->
if not (List.mem i variables) then
unexpected "ident" i
| Deref (e, _field) ->
unexpected "ident" i
| Deref (e, _field) ->
check_expression e
sw/lib/ocaml/extXml.ml
+93 -93
View File
@@ -30,14 +30,14 @@ let sep = Str.regexp "\\."
let child xml ?select c =
let rec find = function
Xml.Element (tag, _attributes, _children) as elt :: elts ->
if tag = c then
match select with
None -> elt
| Some p ->
if p elt then elt else find elts
else
find elts
Xml.Element (tag, _attributes, _children) as elt :: elts ->
if tag = c then
match select with
None -> elt
| Some p ->
if p elt then elt else find elts
else
find elts
| _ :: elts -> find elts
| [] -> raise Not_found in
@@ -46,14 +46,14 @@ let child xml ?select c =
(* Let's try with a numeric index *)
try (Array.of_list children).(int_of_string c) with
Failure "int_of_string" -> (* Bad luck. Go through the children *)
find children
Failure "int_of_string" -> (* Bad luck. Go through the children *)
find children
let get xml path =
let p = Str.split sep path in
let rec iter xml = function
[] -> failwith "ExtXml.get: empty path"
[] -> failwith "ExtXml.get: empty path"
| [x] -> ( try if Xml.tag xml <> x then raise Not_found else xml with _ -> raise Not_found )
| x::xs -> iter (child xml x) xs in
iter xml p
@@ -63,14 +63,14 @@ let get_attrib xml path attr =
let sprint_fields = fun () l ->
"<"^
List.fold_right (fun (a, b) -> (^) (Printf.sprintf "%s=\"%s\" " a b)) l ">"
List.fold_right (fun (a, b) -> (^) (Printf.sprintf "%s=\"%s\" " a b)) l ">"
let attrib = fun x a ->
try
Xml.attrib x a
with
Xml.No_attribute _ ->
raise (Error (Printf.sprintf "Error: Attribute '%s' expected in <%a>" a sprint_fields (Xml.attribs x)))
Xml.No_attribute _ ->
raise (Error (Printf.sprintf "Error: Attribute '%s' expected in <%a>" a sprint_fields (Xml.attribs x)))
let tag_is = fun x v ->
String.lowercase (Xml.tag x) = String.lowercase v
@@ -92,52 +92,52 @@ let buffer_attr = fun indent tab (n,v) ->
let l = String.length v in
for p = 0 to l-1 do
match v.[p] with
| '\\' -> Buffer.add_string tmp "\\\\"
| '"' -> Buffer.add_string tmp "\\\""
| '\\' -> Buffer.add_string tmp "\\\\"
| '"' -> Buffer.add_string tmp "\\\""
| c -> Buffer.add_char tmp c
done;
Buffer.add_char tmp '"';
if indent then
Buffer.add_char tmp '\n'
if indent then
Buffer.add_char tmp '\n'
let buffer_pcdata = Buffer.add_string tmp
let my_to_string_fmt = fun tab_attribs x ->
let rec loop ?(newl=false) tab = function
| Xml.Element (tag,alist,[]) ->
Buffer.add_string tmp tab;
Buffer.add_char tmp '<';
Buffer.add_string tmp tag;
if tab_attribs then Buffer.add_char tmp '\n';
List.iter (buffer_attr tab_attribs tab) alist;
if tab_attribs then Buffer.add_string tmp tab;
Buffer.add_string tmp "/>";
if newl then Buffer.add_char tmp '\n';
Buffer.add_string tmp tab;
Buffer.add_char tmp '<';
Buffer.add_string tmp tag;
if tab_attribs then Buffer.add_char tmp '\n';
List.iter (buffer_attr tab_attribs tab) alist;
if tab_attribs then Buffer.add_string tmp tab;
Buffer.add_string tmp "/>";
if newl then Buffer.add_char tmp '\n';
| Xml.Element (tag,alist,[Xml.PCData text]) ->
Buffer.add_string tmp tab;
Buffer.add_char tmp '<';
Buffer.add_string tmp tag;
List.iter (buffer_attr tab_attribs tab) alist;
Buffer.add_string tmp ">";
buffer_pcdata text;
Buffer.add_string tmp "</";
Buffer.add_string tmp tag;
Buffer.add_char tmp '>';
if newl then Buffer.add_char tmp '\n';
Buffer.add_string tmp tab;
Buffer.add_char tmp '<';
Buffer.add_string tmp tag;
List.iter (buffer_attr tab_attribs tab) alist;
Buffer.add_string tmp ">";
buffer_pcdata text;
Buffer.add_string tmp "</";
Buffer.add_string tmp tag;
Buffer.add_char tmp '>';
if newl then Buffer.add_char tmp '\n';
| Xml.Element (tag,alist,l) ->
Buffer.add_string tmp tab;
Buffer.add_char tmp '<';
Buffer.add_string tmp tag;
List.iter (buffer_attr tab_attribs tab) alist;
Buffer.add_string tmp ">\n";
List.iter (loop ~newl:true (tab^" ")) l;
Buffer.add_string tmp tab;
Buffer.add_string tmp "</";
Buffer.add_string tmp tag;
Buffer.add_char tmp '>';
if newl then Buffer.add_char tmp '\n';
Buffer.add_string tmp tab;
Buffer.add_char tmp '<';
Buffer.add_string tmp tag;
List.iter (buffer_attr tab_attribs tab) alist;
Buffer.add_string tmp ">\n";
List.iter (loop ~newl:true (tab^" ")) l;
Buffer.add_string tmp tab;
Buffer.add_string tmp "</";
Buffer.add_string tmp tag;
Buffer.add_char tmp '>';
if newl then Buffer.add_char tmp '\n';
| Xml.PCData text ->
buffer_pcdata text;
if newl then Buffer.add_char tmp '\n';
buffer_pcdata text;
if newl then Buffer.add_char tmp '\n';
in
Buffer.reset tmp;
loop "" x;
@@ -150,11 +150,11 @@ let my_to_string_fmt = fun tab_attribs x ->
let to_string_fmt = fun ?(tab_attribs = false) xml ->
let l = String.lowercase in
let rec lower = function
Xml.PCData _ as x -> x
Xml.PCData _ as x -> x
| Xml.Element (t, ats, cs) ->
Xml.Element(l t,
List.map (fun (a,v) -> (l a, v)) ats,
List.map lower cs) in
Xml.Element(l t,
List.map (fun (a,v) -> (l a, v)) ats,
List.map lower cs) in
my_to_string_fmt tab_attribs (lower xml)
@@ -162,47 +162,47 @@ let subst_attrib = fun attrib value xml ->
let u = String.uppercase in
let uattrib = u attrib in
match xml with
Xml.Element (tag, attrs, children) ->
let rec loop = function
[] -> [(attrib, value)]
| (a,_v) as c::ats ->
if u a = uattrib then loop ats else c::loop ats in
Xml.Element (tag,
loop attrs,
children)
| Xml.PCData _ -> xml
Xml.Element (tag, attrs, children) ->
let rec loop = function
[] -> [(attrib, value)]
| (a,_v) as c::ats ->
if u a = uattrib then loop ats else c::loop ats in
Xml.Element (tag,
loop attrs,
children)
| Xml.PCData _ -> xml
let subst_child = fun ?(select= fun _ -> true) t x xml ->
match xml with
Xml.Element (tag, attrs, children) ->
let found = ref false in
let new_children =
List.map
(fun xml -> if tag_is xml t && select xml then (found := true; x) else xml)
children in
if !found then
Xml.Element (tag, attrs, new_children)
else
raise Not_found
| Xml.PCData _ -> xml
Xml.Element (tag, attrs, children) ->
let found = ref false in
let new_children =
List.map
(fun xml -> if tag_is xml t && select xml then (found := true; x) else xml)
children in
if !found then
Xml.Element (tag, attrs, new_children)
else
raise Not_found
| Xml.PCData _ -> xml
let subst_or_add_child = fun t x xml ->
try subst_child t x xml with Not_found ->
match xml with
Xml.Element (tag, attrs, children) ->
Xml.Element (tag, attrs, x::children)
| Xml.PCData _ -> xml
Xml.Element (tag, attrs, children) ->
Xml.Element (tag, attrs, x::children)
| Xml.PCData _ -> xml
let remove_child = fun ?(select= fun _ -> true) t xml ->
match xml with
Xml.Element (tag, attrs, children) ->
Xml.Element (tag,
attrs,
List.fold_right (fun xml rest -> if tag_is xml t && select xml then rest else xml::rest) children [])
| Xml.PCData _ -> xml
Xml.Element (tag, attrs, children) ->
Xml.Element (tag,
attrs,
List.fold_right (fun xml rest -> if tag_is xml t && select xml then rest else xml::rest) children [])
| Xml.PCData _ -> xml
let float_attrib = fun xml a ->
@@ -210,19 +210,19 @@ let float_attrib = fun xml a ->
try
float_of_string v
with
_ -> failwith (Printf.sprintf "Error: float expected in '%s'" v)
_ -> failwith (Printf.sprintf "Error: float expected in '%s'" v)
let int_attrib = fun xml a ->
let v = attrib xml a in
try
int_of_string v
with
_ -> failwith (Printf.sprintf "Error: integer expected in '%s'" v)
_ -> failwith (Printf.sprintf "Error: integer expected in '%s'" v)
(* When an .xml is coming through http, the dtd is not available. We disable
the DTD proving feature in this case. FIXME: We should use the resolve
feature *)
the DTD proving feature in this case. FIXME: We should use the resolve
feature *)
let my_xml_parse_file =
let parser = XmlParser.make () in
XmlParser.prove parser false;
@@ -234,11 +234,11 @@ let parse_file = fun ?(noprovedtd = false) file ->
try
(if noprovedtd then my_xml_parse_file else Xml.parse_file) file
with
Xml.Error e -> failwith (sprintf "%s: %s" file (Xml.error e))
| Xml.File_not_found f -> failwith (sprintf "File not found: %s" f)
| Dtd.Prove_error e -> failwith (sprintf "%s: %s" file (Dtd.prove_error e))
| Dtd.Check_error e -> failwith (sprintf "%s: %s" file (Dtd.check_error e))
| Dtd.Parse_error e -> failwith (sprintf "%s: %s" file (Dtd.parse_error e))
Xml.Error e -> failwith (sprintf "%s: %s" file (Xml.error e))
| Xml.File_not_found f -> failwith (sprintf "File not found: %s" f)
| Dtd.Prove_error e -> failwith (sprintf "%s: %s" file (Dtd.prove_error e))
| Dtd.Check_error e -> failwith (sprintf "%s: %s" file (Dtd.check_error e))
| Dtd.Parse_error e -> failwith (sprintf "%s: %s" file (Dtd.parse_error e))
@@ -262,9 +262,9 @@ module Gconf = struct
let entry = fun application name value ->
Xml.Element ("entry", ["name", name;
"value", value;
"application", application],
[])
"value", value;
"application", application],
[])
let add_entry = fun xml appli name value ->
let entry = entry appli name value in
sw/lib/ocaml/fig.ml
+327 -327
View File
File diff suppressed because it is too large Load Diff
sw/lib/ocaml/geometry_2d.ml
+266 -266
View File
File diff suppressed because it is too large Load Diff
sw/lib/ocaml/geometry_3d.ml
+156 -156
View File
File diff suppressed because it is too large Load Diff
sw/lib/ocaml/gm.ml
+34 -34
View File
@@ -34,16 +34,16 @@ let zoom_min = 18
let cache_path = ref "/var/tmp"
type tile_t = {
key : string;
sw_corner : Latlong.geographic;
width : float; (* Longitude difference *)
height : float (* Latitude difference *)
}
key : string;
sw_corner : Latlong.geographic;
width : float; (* Longitude difference *)
height : float (* Latitude difference *)
}
type maps_source = Google | OSM | MS | MQ | MQ_Aerial
let maps_sources = [Google; OSM; MS; MQ; MQ_Aerial]
let string_of_maps_source = function
Google -> "Google" | OSM -> "OpenStreetMap" | MS -> "Bing" | MQ -> "MapQuest OSM" | MQ_Aerial -> "MapQuest Open Aerial"
Google -> "Google" | OSM -> "OpenStreetMap" | MS -> "Bing" | MQ -> "MapQuest OSM" | MQ_Aerial -> "MapQuest Open Aerial"
let maps_source = ref Google
let set_maps_source = fun s -> maps_source := s
@@ -78,7 +78,7 @@ let gm_pos_and_scale = fun keyholeString tLat latHeight tLon lonWidth ->
(** Returns a keyhole string for a longitude (x), latitude (y), and zoom
for Google Maps (http://www.ponies.me.uk/maps/GoogleTileUtils.java) *)
for Google Maps (http://www.ponies.me.uk/maps/GoogleTileUtils.java) *)
let tile_of_geo = fun ?level wgs84 zoom ->
let max = match level with
| None -> zoom_max
@@ -135,13 +135,13 @@ let tile_of_key = fun keyholeStr ->
latLonSize /.= 2.;
match keyholeStr.[i] with
's' -> lon +.= !latLonSize
| 'r' ->
's' -> lon +.= !latLonSize
| 'r' ->
lat +.= !latLonSize;
lon +.= !latLonSize
| 'q' -> lat +.= !latLonSize
| 't' -> ()
| _ -> invalid_arg ("gm_get_lat_long " ^ keyholeStr)
| 'q' -> lat +.= !latLonSize
| 't' -> ()
| _ -> invalid_arg ("gm_get_lat_long " ^ keyholeStr)
done;
gm_pos_and_scale keyholeStr !lat !latLonSize !lon !latLonSize
@@ -149,7 +149,7 @@ let tile_of_key = fun keyholeStr ->
let is_prefix = fun a b ->
String.length b >= String.length a &&
a = String.sub b 0 (String.length a)
a = String.sub b 0 (String.length a)
(** Get the tile or one which contains it from the cache *)
@@ -168,7 +168,7 @@ let get_from_cache = fun dir f ->
loop (i+1)
else
raise Not_found
in
in
loop 0
(** Translate the old quadtree naming policy into new (x,y) coordinates
@@ -181,11 +181,11 @@ let xyz_of_qsrt = fun s ->
x := !x * 2;
y := !y * 2;
match s.[i] with
'q' -> ()
| 'r' -> incr x
| 's' -> incr x; incr y
| 't' -> incr y
| _ -> failwith "xyz_of_qsrt"
'q' -> ()
| 'r' -> incr x
| 's' -> incr x; incr y
| 't' -> incr y
| _ -> failwith "xyz_of_qsrt"
done;
(!x, !y, n-1)
@@ -196,11 +196,11 @@ let ms_key = fun key ->
for i = 1 to n - 1 do
ms_key.[i-1] <-
match key.[i] with
'q' -> '0'
| 'r' -> '1'
| 's' -> '3'
| 't' -> '2'
| _ -> invalid_arg "Gm.ms_key"
'q' -> '0'
| 'r' -> '1'
| 's' -> '3'
| 't' -> '2'
| _ -> invalid_arg "Gm.ms_key"
done;
(ms_key, ms_key.[n-2])
@@ -209,22 +209,22 @@ let google_version = Maps_support.google_version
let url_of_tile_key = fun maps_source s ->
let (x, y, z) = xyz_of_qsrt s in
match maps_source with
Google -> sprintf "http://khm0.google.com/kh/v=%d&x=%d&s=&y=%d&z=%d" google_version x y z
| OSM -> sprintf "http://tile.openstreetmap.org/%d/%d/%d.png" z x y
| MQ -> sprintf "http://otile1.mqcdn.com/tiles/1.0.0/osm/%d/%d/%d.png" z x y
| MQ_Aerial -> sprintf "http://otile1.mqcdn.com/tiles/1.0.0/sat/%d/%d/%d.png" z x y
| MS ->
Google -> sprintf "http://khm0.google.com/kh/v=%d&x=%d&s=&y=%d&z=%d" google_version x y z
| OSM -> sprintf "http://tile.openstreetmap.org/%d/%d/%d.png" z x y
| MQ -> sprintf "http://otile1.mqcdn.com/tiles/1.0.0/osm/%d/%d/%d.png" z x y
| MQ_Aerial -> sprintf "http://otile1.mqcdn.com/tiles/1.0.0/sat/%d/%d/%d.png" z x y
| MS ->
let (key, last_char) = ms_key s in
(* That's the old naming scheme, that still works as of 1st August 2010
sprintf "http://a0.ortho.tiles.virtualearth.net/tiles/a%s.jpeg?g=%d" key (z+32)
sprintf "http://a0.ortho.tiles.virtualearth.net/tiles/a%s.jpeg?g=%d" key (z+32)
*)
(* That's the new code, which conforms to MS naming scheme as of 1st August 2010 *)
sprintf "http://ecn.t%c.tiles.virtualearth.net/tiles/a%s.jpeg?g=516" last_char key
(**)
(**)
let get_cache_dir = function
Google -> !cache_path (* Historic ! Should be // Google *)
Google -> !cache_path (* Historic ! Should be // Google *)
| OSM -> !cache_path // "OSM"
| MQ -> !cache_path // "MapQuest"
| MQ_Aerial -> !cache_path // "MapQuestAerial"
@@ -235,7 +235,7 @@ exception Not_available
type policy = CacheOrHttp | NoHttp | NoCache
let string_of_policy = function
CacheOrHttp -> "CacheOrHttp"
CacheOrHttp -> "CacheOrHttp"
| NoHttp -> "NoHttp"
| NoCache -> "NoCache"
let policies = [CacheOrHttp; NoHttp; NoCache]
@@ -277,7 +277,7 @@ let get_image = fun key ->
try get_from_http key with _ -> (t, f)
else (t, f)
with
| Not_found ->
| Not_found ->
if !policy = NoHttp then raise Not_available;
get_from_http key
sw/lib/ocaml/gm.mli
+7 -7
View File
@@ -29,11 +29,11 @@ val tile_coverage : float -> int -> float * float
(** [tile_coverage wgs84_lat zoom] Returns (width,height) *)
type tile_t = {
key : string; (* [qrst] string *)
sw_corner : Latlong.geographic;
width : float; (* Longitude difference *)
height : float (* Latitude difference *)
}
key : string; (* [qrst] string *)
sw_corner : Latlong.geographic;
width : float; (* Longitude difference *)
height : float (* Latitude difference *)
}
type maps_source = Google | OSM | MS | MQ | MQ_Aerial
val string_of_maps_source : maps_source -> string
@@ -44,11 +44,11 @@ val get_maps_source : unit -> maps_source
val tile_of_geo : ?level:int -> Latlong.geographic -> int -> tile_t
(** [tile_string geo zoom] Returns the tile description containing a
given point with a the smallest available zoom greater or equal to [zoom]. *)
given point with a the smallest available zoom greater or equal to [zoom]. *)
val tile_of_key : string -> tile_t
(** [tile_of_key google_maps_tile_key] Returns tile description of a
named tile. *)
named tile. *)
val cache_path : string ref
sw/lib/ocaml/gtk_3d.ml
+699 -699
View File
File diff suppressed because it is too large Load Diff
sw/lib/ocaml/gtk_draw.ml
+10 -10
View File
@@ -52,7 +52,7 @@ let gd_color_of_rgb (r, g, b) = `RGB (r, g, b)
(* = Creation d'une GDraw.color a partir de ces composantes (r, g, b) = *)
(* ============================================================================= *)
let gd_color_of_float_rgb (r, g, b) = `RGB (int_of_float r, int_of_float g,
int_of_float b)
int_of_float b)
(* ============================================================================= *)
(* = Mise a jour de la couleur de dessin = *)
(* ============================================================================= *)
@@ -82,7 +82,7 @@ let gd_set_style_double_dash p =
(* = Modification du mode de trace = *)
(* ============================================================================= *)
(*let gd_set_mode_xor p = (gd_do_cast p)#set_gc_xor
let gd_set_mode_std p = (gd_do_cast p)#set_gc_copy*)
let gd_set_mode_std p = (gd_do_cast p)#set_gc_copy*)
(* ============================================================================= *)
(* = Modification de l'epaisseur du trace = *)
@@ -152,31 +152,31 @@ let gd_draw_filled_circle p (x, y) r =
(* ============================================================================= *)
let gd_draw_rect p (x1, y1, x2, y2) =
(gd_do_cast p)#rectangle ~filled:false ~x:x1 ~y:y1
~width:(x2-x1) ~height:(y2-y1) ()
~width:(x2-x1) ~height:(y2-y1) ()
(* ============================================================================= *)
(* = Dessin d'un rectangle plein = *)
(* ============================================================================= *)
let gd_draw_filled_rect p (x1, y1, x2, y2) =
(gd_do_cast p)#rectangle ~filled:true ~x:x1 ~y:y1 ~width:(x2-x1) ~height:(y2-y1) ()
(gd_do_cast p)#rectangle ~filled:true ~x:x1 ~y:y1 ~width:(x2-x1) ~height:(y2-y1) ()
(* ============================================================================= *)
(* = Dessin d'un triangle = *)
(* ============================================================================= *)
let gd_draw_triangle p (x, y) size =
let size0 = int_of_float ((float_of_int size) *. 1.5) and
size1 = int_of_float ((float_of_int size) *. 0.5) in
size1 = int_of_float ((float_of_int size) *. 0.5) in
(gd_do_cast p)#polygon ~filled:false
[(x, y-size); (x-size0, y+size1); (x+size0, y+size1)]
[(x, y-size); (x-size0, y+size1); (x+size0, y+size1)]
(* ============================================================================= *)
(* = Dessin d'un triangle plein = *)
(* ============================================================================= *)
let gd_draw_filled_triangle p (x, y) size =
let size0 = int_of_float ((float_of_int size) *. 1.5) and
size1 = int_of_float ((float_of_int size) *. 0.5) in
size1 = int_of_float ((float_of_int size) *. 0.5) in
(gd_do_cast p)#polygon ~filled:true
[(x, y-size); (x-size0, y+size1); (x+size0, y+size1)]
[(x, y-size); (x-size0, y+size1); (x+size0, y+size1)]
(* ============================================================================= *)
(* = Efface une pixmap = *)
@@ -196,8 +196,8 @@ let gd_set_background_pixmap p dest = (gd_do_cast dest)#put_pixmap ~x:0 ~y:0 p
let gd_put_transp_pixmap p dest x y =
(* Indispensable d'utiliser le masque pour la transparence *)
(match p#mask with
None -> () |
Some m -> (gd_do_cast dest)#set_clip_origin ~x:x ~y:y; dest#set_clip_mask m) ;
None -> () |
Some m -> (gd_do_cast dest)#set_clip_origin ~x:x ~y:y; dest#set_clip_mask m) ;
(* Mise en place du pixmap transparent *)
dest#put_pixmap ~x:x ~y:y p#pixmap ;

Some files were not shown because too many files have changed in this diff Show More