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

*** empty log message ***

Browse Source
This commit is contained in:
Pascal Brisset
2006-06-12 17:55:11 +00:00
parent d9fd198517
commit ef79e29558
20 changed files with 543 additions and 203 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/twinstar1.xml
-1
View File
@@ -108,7 +108,6 @@
<define name="THETA0" value="0" unit="deg"/>
<define name="PHI0" value="30" unit="deg"/>
</section>
<makefile>
# Virtual AC
conf/control_panel.xml.example
+13
View File
@@ -79,6 +79,19 @@
<program name="gaia"/>
</session>
<session name="Generic sim">
<program name="server"/>
<program name="map 2d ml">
<arg flag="-mercator" constant=""/>
<arg flag="-google_fill" constant=""/>
<arg flag="-center_ac" constant=""/>
<arg flag="-no_alarm" constant=""/>
<!-- <arg flag="-mplayer" constant="rtsp://localhost:7070/webcam"/> -->
</program>
<program name="sim"><arg flag="-a" constant="Plaster"/></program>
</session>
</section>
</control_panel>
conf/flight_plans/generic.xml
+29 -9
View File
@@ -1,22 +1,42 @@
<flight_plan SECURITY_HEIGHT="25" lat0="42.000000" lon0="3" ground_alt="420" qfu="270" alt="450" max_dist_from_home="500" name="Generic">
<dl_settings>
<dl_setting VAR="ir_roll_neutral" MAX="0.2" STEP="0.01" MIN="-0.2"/>
<dl_setting VAR="ir_pitch_neutral" MAX="0.2" STEP="0.01" MIN="-0.2"/>
<dl_setting VAR="roll_pgain" MAX="15000" STEP="100" MIN="5000"/>
<dl_setting VAR="pitch_pgain" MAX="20000" STEP="100" MIN="10000"/>
<dl_setting VAR="course_pgain" MAX="-0.1" STEP="0.05" MIN="-1"/>
<dl_setting VAR="pprz_mode" MAX="2" STEP="1" MIN="0"/>
</dl_settings>
<waypoints>
<waypoint Y="0" NAME="HOME" X="0"/>
<waypoint Y="0" NAME="1" X="200"/>
<waypoint Y="200" NAME="2" X="0"/>
</waypoints>
<include Y="100" NAME="up_and_down" PROCEDURE="circles.xml" X="00" ROTATE="0">
<arg NAME="bottom" VALUE="GROUND_ALT+50"/>
<with FROM="event1" TO="circlehome"/>
</include>
<blocks>
<block NAME="init">
<block NAME="wait GPS">
<while COND="!GPS_FIX_VALID(gps_mode)"/>
<set var="nav_utm_east0" value="gps_utm_east"/>
<set var="nav_utm_north0" value="gps_utm_north"/>
</block>
<block name="init">
<while COND="10>block_time"/>
<set var="nav_utm_east0" value="gps_utm_east/100"/>
<set var="nav_utm_north0" value="gps_utm_north/100"/>
<set var="nav_utm_zone0" value="gps_utm_zone"/>
<set var="waypoints[WP_HOME].a" value="gps_alt/100+50"/>
<set var="moved_waypoints[WP_HOME]" value="TRUE"/>
<set var="waypoints[WP_1].a" value="gps_alt/100+50"/>
<set var="moved_waypoints[WP_1]" value="TRUE"/>
<set var="waypoints[WP_2].a" value="gps_alt/100+50"/>
<set var="moved_waypoints[WP_2]" value="TRUE"/>
<deroute BLOCK="circlehome"/>
</block>
<block NAME="circlehome">
<exception COND="(RcEvent1())" DEROUTE="up_and_down.climb"/>
<circle WP="HOME" ALT="GROUND_ALT+50" RADIUS="75"/>
<circle WP="HOME" RADIUS="75"/>
</block>
<block NAME="stack 1">
<circle WP="1" RADIUS="75"/>
</block>
<block NAME="stack 2">
<circle WP="2" RADIUS="75"/>
</block>
</blocks>
</flight_plan>
sw/airborne/gps.c
+3 -2
View File
@@ -34,12 +34,13 @@
#include "estimator.h"
#include "ap_downlink.h"
#include "infrared.h"
#include "nav.h"
uint16_t last_gps_msg_t; /** cputime of the last gps message */
void estimator_update_state_gps( void ) {
float gps_east = gps_utm_east / 100. - NAV_UTM_EAST0;
float gps_north = gps_utm_north / 100. - NAV_UTM_NORTH0;
float gps_east = gps_utm_east / 100. - nav_utm_east0;
float gps_north = gps_utm_north / 100. - nav_utm_north0;
float falt = gps_alt / 100.;
EstimatorSetPos(gps_east, gps_north, falt);
float fspeed = gps_gspeed / 100.;
sw/airborne/main_ap.c
+1 -1
View File
@@ -77,7 +77,7 @@ uint8_t rc_settings_mode = RC_SETTINGS_MODE_NONE;
uint8_t fatal_error_nb = 0;
static const uint16_t version = 1;
uint8_t pprz_mode = PPRZ_MODE_MANUAL;
uint8_t pprz_mode = PPRZ_MODE_AUTO2;
uint8_t vertical_mode = VERTICAL_MODE_MANUAL;
uint8_t lateral_mode = LATERAL_MODE_MANUAL;
sw/airborne/nav.c
-1
View File
@@ -377,7 +377,6 @@ void nav_home(void) {
*/
void nav_update(void) {
compute_dist2_to_home();
auto_nav();
}
sw/ground_segment/cockpit/horizon.ml
+110 -14
View File
@@ -1,3 +1,4 @@
open Printf
open Latlong
let affine_pos_and_angle xw yw angle =
@@ -5,6 +6,7 @@ let affine_pos_and_angle xw yw angle =
let sin_a = sin angle in
[| cos_a ; sin_a ; ~-. sin_a; cos_a; xw ; yw |]
let affine_pos xw yw = affine_pos_and_angle xw yw 0.
let arc = fun n r start stop ->
let s = (stop -. start) /. float (n-1) in
@@ -20,39 +22,82 @@ let floats_of_points = fun ps ->
done;
a
let ruler = fun ?(index_on_right=false) ~text_props ~max ~scale ~w ~index_width ~step ~h root ->
let r = GnoCanvas.group root in
let height = scale *. float max in
let _ = GnoCanvas.rect ~x1:0. ~y1:(-.height) ~x2:w ~y2:height ~fill_color:"#808080" r in
for i = 0 to max/step do
let i = i * step in
let y = -. scale *. float i in
let text = Printf.sprintf "%d" i in
let _ = GnoCanvas.text ~text ~props:(text_props@[`ANCHOR `EAST]) ~y ~x:(w*.0.75) r in
let _ = GnoCanvas.line ~points:[|w*.0.8;y;w;y|] ~fill_color:"white" r in
let y = y -. float step /. 2. *. scale in
let _ = GnoCanvas.line ~points:[|w*.0.8;y;w;y|] ~fill_color:"white" r in
()
done;
(** Yellow index *)
let _ = GnoCanvas.line ~points:[|0.;0.;w;0.|] ~fill_color:"yellow" root in
let s = index_width in
let idx = GnoCanvas.polygon ~points:[|0.;0.;-.s;s/.2.;-.s;-.s/.2.|] ~fill_color:"yellow" root in
if index_on_right then
idx#affine_absolute (affine_pos_and_angle w 0. pi);
(** Mask (bottom & top) *)
let _ = GnoCanvas.rect ~x1:0. ~y1:(-.height) ~x2:w ~y2:(-.h) ~fill_color:"black" root in
let _ = GnoCanvas.rect ~x1:0. ~y1:height ~x2:w ~y2:h ~fill_color:"black" root in
r
class h = fun ?packing size ->
let canvas = GnoCanvas.canvas ~aa:true ~width:size ~height:size ?packing () in
let _ =
canvas#set_center_scroll_region false;
in
let size = float size in
let size2 = size /. 2. in
let left_margin = size2 /. 10. in
let pitch_scale = fun pitch -> pitch *. size2 *. 2. in
let speed_scale = size2 /. 10. in
let alt_scale = size2 /. 50. in
let speed_width = size2/.5. in
let alt_width = size2/.2.5 in
let index_width = size2 /. 15. in
let xc = left_margin +. speed_width +. size2
and yc = size2*.1.1 in
let text_props = [`FONT "Sans 10"; `ANCHOR `CENTER; `FILL_COLOR "white"] in
let disc = GnoCanvas.group canvas#root in
let top = GnoCanvas.rect ~x1:(-.size2) ~y1:(-.size2*.5.) ~x2:size2 ~y2:0. ~fill_color:"#0099cb" disc
and bottom = GnoCanvas.rect ~x1:(-.size2) ~y1:0. ~x2:size2 ~y2:(size2*.5.) ~fill_color:"#986701" disc
and line = GnoCanvas.line ~props:[`WIDTH_PIXELS 4] ~points:[|-.size2;0.;size2;0.|] ~fill_color:"white" disc in
and line = GnoCanvas.line ~props:[`WIDTH_PIXELS 4] ~points:[|-.size2;0.;size2;0.|] ~fill_color:"white" disc
and _ = GnoCanvas.line ~points:[|0.;-.size2;0.;size2|] ~fill_color:"white" disc
in
let grads = fun ?(text=false) n s a b ->
for i = 0 to n do
let a = float i *. a in
let y = pitch_scale ((Deg>>Rad)(a+.b)) in
let deg = float i *. a +. b in
let y = pitch_scale ((Deg>>Rad)deg) in
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 a)
let text = Printf.sprintf "%d" (truncate deg)
and x = 2.*.s in
ignore (GnoCanvas.text ~props:[`ANCHOR `CENTER; `FILL_COLOR "white"] ~text ~y ~x disc);
ignore (GnoCanvas.text ~props:[`ANCHOR `CENTER; `FILL_COLOR "white"] ~text ~y ~x:(-.x) disc);
let y = -. y
and text = Printf.sprintf "-%d" (truncate a) in
ignore (GnoCanvas.text ~props:[`ANCHOR `CENTER; `FILL_COLOR "white"] ~text ~y ~x disc);
ignore (GnoCanvas.text ~props:[`ANCHOR `CENTER; `FILL_COLOR "white"] ~text ~y ~x:(-.x) disc);
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 _ =
grads 10 (size2/.10.) 5. 2.5;
grads 5 (size2/.7.) 10. 5.;
grads ~text:true 5 (size2/.5.) 10. 10. in
let mask = GnoCanvas.group canvas#root in
let mask = GnoCanvas.group ~x:xc ~y:yc canvas#root in
let center = GnoCanvas.ellipse ~x1:(-3.) ~y1:(-.3.) ~x2:3. ~y2:3. ~fill_color:"black" mask in
let pi6 = pi/.6. in
let n = 20 in
@@ -68,9 +113,60 @@ class h = fun ?packing size ->
ignore (GnoCanvas.polygon ~fill_color:"black" ~points mask);
let s = size2/. 5. in
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) in
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 ->
let l = GnoCanvas.line~props:[`WIDTH_PIXELS 2] ~fill_color:"white" ~points:[|0.;-.size2;0.;-.1.2*.size2|] mask in
l#affine_relative (affine_pos_and_angle 0. 0. ((Deg>>Rad)a)) in
for i = 0 to 4 do
let a = float (i*10) in
g a; g (-.a)
done;
let _30 = fun a ->
let t = GnoCanvas.text ~text:"30" ~props:text_props ~x:0. ~y:(-1.1*.size2) mask in
t#affine_relative (affine_pos_and_angle 0. 0. ((Deg>>Rad)a)) in
_30 30.; _30 (-30.)
in
let speed =
let g = GnoCanvas.group ~x:left_margin ~y:yc canvas#root in
let r = ruler ~text_props ~index_on_right:true ~max:50 ~scale:speed_scale ~w:speed_width ~step:2 ~index_width ~h:(0.75*.size2) g in
let mx =
GnoCanvas.text ~x:(speed_width/.2.) ~y:(-0.85*.size2) ~props:text_props g
and mi =
GnoCanvas.text ~x:(speed_width/.2.) ~y:(0.80*.size2) ~props:text_props g in
mx#set [`FILL_COLOR "yellow"];
mi#set [`FILL_COLOR "yellow"];
r, mi, mx
and alt =
let g = GnoCanvas.group ~x:(xc+.size2) ~y:yc canvas#root in
ruler ~text_props ~max:3000 ~scale:alt_scale ~w:alt_width ~step:10
~index_width ~h:(0.75*.size2) g
in
object (self)
method draw = fun roll pitch ->
disc#affine_absolute (affine_pos_and_angle 0. (pitch_scale pitch) (-.roll))
method set_attitude = fun roll pitch ->
disc#affine_absolute (affine_pos_and_angle xc (yc+.pitch_scale pitch) (-.roll))
val mutable max_speed = 0.
val mutable min_speed = max_float
method set_speed = fun (s:float) ->
let (r, mi, mx) = speed in
r#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
let (r, _, _) = speed in
ignore (r#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. (alt_scale*.s))
end
sw/ground_segment/cockpit/map2d.ml
+151 -97
View File
File diff suppressed because it is too large Load Diff
sw/ground_segment/cockpit/pages.ml
+30 -3
View File
@@ -130,14 +130,41 @@ object (this)
method set_roll r =
roll <- r;
horizon#draw ((Deg>>Rad)roll) ((Deg>>Rad)pitch)
horizon#set_attitude ((Deg>>Rad)roll) ((Deg>>Rad)pitch)
method set_pitch p =
pitch <- p;
horizon#draw ((Deg>>Rad)roll) ((Deg>>Rad)pitch)
method set_alt (a:float) = ()
horizon#set_attitude ((Deg>>Rad)roll) ((Deg>>Rad)pitch)
method set_alt (a:float) = horizon#set_alt a
method set_climb (c:float) = ()
method set_speed (c:float) = horizon#set_speed c
end
(*****************************************************************************)
(* Misc page *)
(*****************************************************************************)
class misc ~packing (widget: GBin.frame) =
let table = GPack.table
~rows: 2
~columns: 2
~row_spacings: 5
~col_spacings: 40
~packing
() in
let label = fun text i j ->GMisc.label ~text ~packing:(table#attach ~top:i ~left:j) () in
let _init =
ignore (label "Wind speed" 0 0);
ignore (label "Wind direction" 1 0) in
let wind_speed = label "" 0 1
and wind_dir = label "" 1 1 in
object
method set_wind_speed s = wind_speed#set_text s
method set_wind_dir s = wind_dir#set_text s
end
(*****************************************************************************)
(* Dataling settings paged *)
(*****************************************************************************)
class settings = fun xml_settings callback ->
let sw = GBin.scrolled_window ~hpolicy:`AUTOMATIC ~vpolicy:`AUTOMATIC () in
let vbox = GPack.vbox ~packing:sw#add_with_viewport () in
sw/ground_segment/cockpit/pages.mli
+8
View File
@@ -15,6 +15,7 @@ class gps : GBin.frame ->
end
class pfd : GBin.frame ->
object
method set_speed : float -> unit
method set_alt : float -> unit
method set_climb : float -> unit
method set_pitch : float -> unit
@@ -26,3 +27,10 @@ class settings : Xml.xml list -> (int -> float -> unit) ->
method set : int -> float -> unit
method widget : GObj.widget
end
class misc :
packing:(GObj.widget -> unit) ->
GBin.frame ->
object
method set_wind_dir : string -> unit
method set_wind_speed : string -> unit
end
sw/ground_segment/tmtc/server.ml
+1 -1
View File
@@ -305,7 +305,7 @@ let send_cam_status = fun a ->
match a.nav_ref with
None -> () (* No geo ref for camera target *)
| Some nav_ref ->
let h = a.alt -. float (Srtm.of_utm a.pos) in
let h = a.alt -. float (try Srtm.of_utm a.pos with _ -> 0) in
let dx = h *. tan (a.cam.phi -. a.roll)
and dy = h *. tan (a.cam.theta +. a.pitch) in
let alpha = -. a.course in
sw/lib/ocaml/mapCanvas.ml
+142 -44
View File
@@ -43,6 +43,10 @@ let distance = fun (x1,y1) (x2,y2) -> sqrt ((x1-.x2)**2.+.(y1-.y2)**2.)
let _ = Srtm.add_path "SRTM"
let affine_pos_and_angle xw yw angle =
let cos_a = cos angle in
let sin_a = sin angle in
[| cos_a ; sin_a ; ~-. sin_a; cos_a; xw ; yw |]
type projection =
Mercator (* 1e-6 = 1 world unit, y axis reversed *)
@@ -82,6 +86,38 @@ let set_opacity = fun pixbuf opacity ->
pixbuf
type drawing = NotDrawing | Rectangle of float*float | Polygon of (float*float) list
let float_array_of_points = fun l ->
Array.of_list (List.fold_right (fun (x,y) r -> x::y::r) l [])
let pvect (x1,y1) (x2,y2) = x1*.y2-.y1*.x2
let rec convexify = fun l ->
match l with
[] | [_] | [_;_] -> l
| (x1,y1)::(x2,y2)::(x3,y3)::l ->
if pvect (x3-.x2, y3-.y2) (x1-.x2,y1-.y2) < 0.
then convexify ((x1,y1)::(x3,y3)::l)
else (x1,y1)::convexify ((x2,y2)::(x3,y3)::l)
let convex = fun l ->
match convexify l with
[] -> []
| (x3,y3)::ps ->
(** Remove last bad points *)
let rec loop = fun l ->
match l with
[] | [_] -> l
| (x2,y2)::(x1,y1)::pts ->
if pvect (x3-.x2, y3-.y2) (x1-.x2,y1-.y2) < 0.
then loop ((x1,y1)::pts)
else l in
(x3,y3)::List.rev (loop (List.rev ps))
(** basic canvas with menubar **************************************
* (the vertical display in map2.ml is an instance of basic_widget)*
*******************************************************************)
@@ -103,15 +139,32 @@ class basic_widget = fun ?(height=800) ?width ?(projection = Mercator) ?georef (
let spin_button = GEdit.spin_button ~rate:0. ~digits:2 ~width:50 (*** ~height:20 ***) ~packing:top_bar#pack () in
let canvas = GnoCanvas.canvas ~packing:(frame#pack ~expand:true) () in
let background = GnoCanvas.group canvas#root in
let background = GnoCanvas.group canvas#root
and still = GnoCanvas.group canvas#root in
let view_cbs = Hashtbl.create 3 in (* Store for view event callback *)
let region_rectangle = GnoCanvas.rect canvas#root ~props:[`WIDTH_PIXELS 2; `OUTLINE_COLOR "red"] in
let region_polygon = GnoCanvas.polygon canvas#root ~props:[`WIDTH_PIXELS 2; `OUTLINE_COLOR "red"] in
let s = 50. in
let s2 = s/.2. and s4=s/.4. in
let points = [|0.;0.; s2;s2; s4;s2; s4;s; -.s4;s; -.s4;s2; -.s2;s2|] in
let props = [`FILL_COLOR "#a0a0ff"; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] in
let arrow = fun x y angle ->
let a = GnoCanvas.polygon still ~points ~props in
a#affine_relative (affine_pos_and_angle x y angle);
a in
let north_arrow = arrow (1.5*.s) 0. 0.
and south_arrow = arrow (1.5*.s) (3.*.s) LL.pi
and west_arrow = arrow 0. (1.5*.s) (-.LL.pi/.2.)
and east_arrow = arrow (3.*.s) (1.5*.s) (LL.pi/.2.) in
object (self)
(** GUI attributes *)
val background = background
method still = still
method top_still = 3.5*.s
val adj = GData.adjustment
~value:1. ~lower:0.05 ~upper:10.
@@ -126,10 +179,12 @@ class basic_widget = fun ?(height=800) ?width ?(projection = Mercator) ?georef (
val mutable projection = projection
val mutable georef = georef
val mutable dragging = None
val mutable grouping = None
val mutable drawing = NotDrawing
val mutable region = None (* Rectangle selected region *)
val mutable polygon = None (* Polygon selected region *)
method region = region
method polygon = polygon
(** initialization of instance attributes *)
@@ -140,9 +195,10 @@ class basic_widget = fun ?(height=800) ?width ?(projection = Mercator) ?georef (
(** callback bindings *)
canvas#coerce#misc#modify_bg [`NORMAL, `NAME "black"];
ignore (canvas#event#connect#motion_notify (self#mouse_motion));
ignore (canvas#event#connect#button_press (self#button_press));
ignore (canvas#event#connect#button_press self#button_press);
ignore (canvas#event#connect#button_release self#button_release);
ignore (canvas#event#connect#motion_notify self#mouse_motion);
ignore (canvas#event#connect#button_press self#button_press);
ignore (canvas#event#connect#after#key_press self#key_press) ;
ignore (canvas#event#connect#enter_notify (fun _ -> self#canvas#misc#grab_focus () ; false));
ignore (canvas#event#connect#any self#any_event);
@@ -264,22 +320,32 @@ class basic_widget = fun ?(height=800) ?width ?(projection = Mercator) ?georef (
method zoom = fun value ->
adj#set_value value
(** Mouse button events *******************************************)
method button_press = fun ev ->
let xc = GdkEvent.Button.x ev in
let yc = GdkEvent.Button.y ev in
match GdkEvent.Button.button ev with
1 when Gdk.Convert.test_modifier `SHIFT (GdkEvent.Button.state ev) && not (Gdk.Convert.test_modifier `CONTROL (GdkEvent.Button.state ev)) ->
let (x1,y1) = self#window_to_world xc yc in
grouping <- Some (x1,y1);
region_rectangle#set [`X1 x1; `Y1 y1; `X2 x1; `Y2 y1];
true
| 2 when Gdk.Convert.test_modifier `SHIFT (GdkEvent.Button.state ev) ->
dragging <- Some (xc, yc);
true
| _ -> false
(** events *******************************************)
method button_press = fun ev ->
let xc = GdkEvent.Button.x ev
and yc = GdkEvent.Button.y ev
and state = GdkEvent.Button.state ev in
let (x1,y1) = self#window_to_world xc yc in
try let _ = canvas#get_item_at x1 y1 in false with
Not_found ->
begin
match GdkEvent.Button.button ev with
1 ->
begin
match Gdk.Convert.modifier state with
[`SHIFT] ->
drawing <- Rectangle (x1,y1);
region_rectangle#set [`X1 x1; `Y1 y1; `X2 x1; `Y2 y1];
true
| [] ->
drawing <- Polygon [(x1, y1)];
true;
| _ -> false
end
| _ -> false
end
method mouse_motion = fun ev ->
if georef <> None then begin
let xc = GdkEvent.Motion.x ev
@@ -288,38 +354,42 @@ class basic_widget = fun ?(height=800) ?width ?(projection = Mercator) ?georef (
self#display_geo (self#geo_string (self#of_world (xw,yw)));
self#display_alt (self#of_world (xw,yw));
begin
match dragging with
Some (x0, y0 ) ->
let (x, y) = self#canvas#get_scroll_offsets in
self#canvas#scroll_to (x+truncate (x0-.xc)) (y+truncate (y0-.yc))
| None -> ()
end;
begin
match grouping with
Some starting_point ->
let starting_point = self#of_world starting_point in
match drawing with
Rectangle (x1,y1) ->
let starting_point = self#of_world (x1,y1) in
let starting_point = LL.utm_of LL.WGS84 starting_point in
let current_point = LL.utm_of LL.WGS84 (self#of_world (xw, yw)) in
let (east, north) = LL.utm_sub current_point starting_point in
region_rectangle#set [`X2 xw; `Y2 yw];
self#display_group (sprintf "[%.1fkm %.1fkm]" (east/.1000.) (north/.1000.))
| None -> ()
end
| Polygon ps ->
let points = convex ((xw,yw)::ps) in
drawing <- Polygon points;
region_polygon#set [`POINTS (float_array_of_points points)]
| _ -> ()
end;
end;
false
method button_release = fun ev ->
match GdkEvent.Button.button ev, grouping with
2, _ ->
dragging <- None; false
| 1, Some starting_point ->
let xc = GdkEvent.Button.x ev in
let yc = GdkEvent.Button.y ev in
let current_point = self#window_to_world xc yc in
region <- Some (starting_point, current_point);
self#display_group "";
grouping <- None;
false
match GdkEvent.Button.button ev with
1 ->
begin
let xc = GdkEvent.Button.x ev in
let yc = GdkEvent.Button.y ev in
let current_point = self#window_to_world xc yc in
match drawing with
Rectangle (x1,y1) ->
region <- Some ((x1,y1), current_point);
self#display_group "";
drawing <- NotDrawing;
true
| Polygon points ->
drawing <- NotDrawing;
polygon <- Some points;
true
| _ -> false
end
| _ -> false
method key_press = fun ev ->
@@ -414,7 +484,35 @@ class basic_widget = fun ?(height=800) ?width ?(projection = Mercator) ?georef (
let t = GnoCanvas.text ~x:x1 ~y:y1 ~text:text ~props:[`FILL_COLOR fill_color; `X_OFFSET x_offset; `Y_OFFSET y_offset] group in
t#show ();
t
initializer
let replace_still = fun _ ->
let (x, y) = canvas#get_scroll_offsets in
let (xc, yc) = canvas#window_to_world (float x) (float y) in
let z = 1./.self#current_zoom in
still#affine_absolute [|z;0.;0.;z;xc;yc|]
in
self#connect_view replace_still;
let move_timer = ref (Glib.Timeout.add 0 (fun _ -> false)) in
let move dx dy = function
`BUTTON_PRESS _ ->
let scroll = fun _ ->
let (x, y) = canvas#get_scroll_offsets in
canvas#scroll_to (x+dx) (y+dy) ; true in
move_timer := Glib.Timeout.add 50 scroll;
true
| `BUTTON_RELEASE _ ->
Glib.Timeout.remove !move_timer;
true
| _ -> false in
let up = move 0 (-pan_step)
and down = move 0 pan_step
and left = move (-pan_step) 0
and right = move pan_step 0 in
ignore (north_arrow#connect#event up);
ignore (south_arrow#connect#event down);
ignore (west_arrow#connect#event left);
ignore (east_arrow#connect#event right)
end
sw/lib/ocaml/mapFP.ml
+2 -2
View File
@@ -34,7 +34,7 @@ let rec assoc_nocase at = function
if String.uppercase at = String.uppercase a then v else assoc_nocase at avs
(** Connect a change in the XML editor to the graphical rep *)
let update_wp utm_ref wp = function
let update_wp utm_ref (wp:MapWaypoints.waypoint) = function
XmlEdit.Deleted -> wp#delete
| XmlEdit.New_child _ -> failwith "update_wp"
| XmlEdit.Modified attribs ->
@@ -216,7 +216,7 @@ class flight_plan = fun ?edit geomap color fp_dtd xml ->
)
path
method connect_selection = fun cb -> XmlEdit.connect_selection xml_tree_view cb
method connect_activated = fun cb -> XmlEdit.connect_activated xml_tree_view cb
initializer (
(** Create a graphic waypoint when it is created from the xml editor *)
sw/lib/ocaml/mapFP.mli
+1 -1
View File
@@ -43,7 +43,7 @@ class flight_plan :
method xml : Xml.xml
method insert_path : (MapWaypoints.waypoint * float) list -> unit
method highlight_stage : int -> int -> unit
method connect_selection : (XmlEdit.node->unit) -> unit
method connect_activated : (XmlEdit.node->unit) -> unit
end
(** Extracts [lat0] and [Lon0] attributes *)
sw/lib/ocaml/mapWaypoints.ml
+23 -11
View File
@@ -71,7 +71,7 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) wgs84 ->
end
method alt = alt
method label = label
method xy = let a = item#i2w_affine in (a.(4), a.(5)) (*** item#i2w 0. 0. causes Seg Fault !***)
method xy = let a = item#i2w_affine in (a.(4), a.(5))
method move dx dy = item#move dx dy; label#move dx dy
method edit =
let dialog = GWindow.window ~border_width:10 ~title:"Waypoint Edit" () in
@@ -81,7 +81,14 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) wgs84 ->
let s = sprintf "WGS84 %s" (geomap#geo_string wgs84) in
let ename = GEdit.entry ~text:name ~editable:false ~packing:dvbx#add () in
let e_pos = GEdit.entry ~text:s ~packing:dvbx#add () in
let ea = GEdit.entry ~text:(string_of_float alt) ~packing:dvbx#add () in
let ha = GPack.hbox ~packing:dvbx#add () in
let minus10= GButton.button ~label:"-10" ~packing:ha#add () in
let ea = GEdit.entry ~text:(string_of_float alt) ~packing:ha#add () in
let plus10= GButton.button ~label:"+10" ~packing:ha#add () in
let change_alt = fun x ->
ea#set_text (string_of_float (float_of_string ea#text +. x)) in
ignore(minus10#connect#pressed (fun _ -> change_alt (-10.)));
ignore(plus10#connect#pressed (fun _ -> change_alt (10.)));
let callback = fun _ ->
self#set_name ename#text;
@@ -100,21 +107,18 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) wgs84 ->
(fun e -> ignore (e#connect#activate ~callback))
[ename; e_pos; ea];
ok#grab_default ();
ignore(ok#connect#clicked ~callback:dialog#destroy);
ignore(ok#connect#clicked ~callback:(fun _ -> callback (); dialog#destroy ()));
dialog#show ()
val mutable motion = false
method event (ev : GnoCanvas.item_event) =
begin
match ev with
| `BUTTON_PRESS ev ->
begin
match GdkEvent.Button.button ev with
| 2 -> self#edit
| 3 ->
if (GToolbox.question_box ~title:"Confirm delete" ~buttons:["Cancel";"Delete"] ~default:2 (sprintf "Delete '%s' ?" name)) = 2 then
self#delete
| 1 ->
motion <- false;
let x = GdkEvent.Button.x ev
and y = GdkEvent.Button.y ev in
x0 <- x; y0 <- y;
@@ -126,6 +130,7 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) wgs84 ->
| `MOTION_NOTIFY ev ->
let state = GdkEvent.Motion.state ev in
if Gdk.Convert.test_modifier `BUTTON1 state then begin
motion <- true;
let x = GdkEvent.Motion.x ev
and y = GdkEvent.Motion.y ev in
let dx = geomap#current_zoom *. (x-. x0)
@@ -138,7 +143,8 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) wgs84 ->
| `BUTTON_RELEASE ev ->
if GdkEvent.Button.button ev = 1 then begin
item#ungrab (GdkEvent.Button.time ev);
moved <- true
if not motion then
self#edit
end
| _ -> ()
end;
@@ -149,11 +155,17 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) wgs84 ->
method deleted = deleted
method item = item
method pos = geomap#of_world self#xy
method set wgs84 =
method set ?altitude ?(update=false) wgs84 =
let (xw, yw) = geomap#world_of wgs84
and (xw0, yw0) = self#xy
and z = geomap#zoom_adj#value in
self#move ((xw-.xw0)*.z) ((yw-.yw0)*.z)
self#move ((xw-.xw0)*.z) ((yw-.yw0)*.z);
begin
match altitude with
Some a -> alt <- a
| _ -> ()
end;
if update then updated ()
method delete =
deleted <- true; (* BOF *)
item#destroy ();
sw/lib/ocaml/mapWaypoints.mli
+1 -1
View File
@@ -50,7 +50,7 @@ class waypoint :
method label : GnoCanvas.text
method move : float -> float -> unit
method name : string
method set : Latlong.geographic -> unit
method set : ?altitude:float -> ?update:bool -> Latlong.geographic -> unit
method set_name : string -> unit
method xy : float * float
method zoom : float -> unit
sw/lib/ocaml/xmlEdit.ml
+10 -7
View File
@@ -379,10 +379,10 @@ let connect = fun ((model, path):node) cb ->
let current_cb = try model#get ~row ~column:event with _ -> fun _ -> () in
model#set ~row ~column:event (fun e -> cb e; current_cb e)
let selection_cbs = Hashtbl.create 3
let connect_selection = fun (model,_) cb ->
let l = try Hashtbl.find selection_cbs model with Not_found -> [] in
Hashtbl.replace selection_cbs model (cb::l)
let activated_cbs = Hashtbl.create 3
let connect_activated = fun (model,_) cb ->
let l = try Hashtbl.find activated_cbs model with Not_found -> [] in
Hashtbl.replace activated_cbs model (cb::l)
@@ -483,13 +483,16 @@ let create = fun ?(edit=true) dtd xml ->
let attribs = tree_model#get ~row ~column:attributes in
attribs_model#clear ();
tag_of_last_selection := tree_model#get ~row ~column:tag_col;
set_attributes attribs_model attribs;
let cbs = try (Hashtbl.find selection_cbs tree_model) with Not_found -> [] in
List.iter (fun cb -> cb (tree_model, path)) cbs
set_attributes attribs_model attribs
| _ -> () in
let _c = tree_view#selection#connect#after#changed ~callback:selection_changed in
let _ = tree_view#connect#after#row_activated ~callback:
(fun path vcol ->
let cbs = try (Hashtbl.find activated_cbs tree_model) with Not_found -> [] in
List.iter (fun cb -> cb (tree_model, path)) cbs) in
if edit then begin
let _c = add_context_menu tree_model tree_view (tree_menu_popup dtd) in
let _c = add_context_menu attribs_model attribs_view ~noselection_menu:(add_one_menu dtd tree_view) (attribs_menu_popup dtd tree_view) in
sw/lib/ocaml/xmlEdit.mli
+1 -1
View File
@@ -65,7 +65,7 @@ val add_child : node -> tag -> attributes -> node
(** Modifications *)
val connect : node -> (event -> unit) -> unit
val connect_selection : t -> (node -> unit) -> unit
val connect_activated : t -> (node -> unit) -> unit
(** To be kept informed about modifications *)
val selection : t -> node
sw/simulator/sitl.ml
+2
View File
@@ -107,6 +107,8 @@ module Make(A:Data.MISSION) = struct
rc_channels;
ignore (on_off#connect#toggled (fun () -> sliders#coerce#misc#set_sensitive on_off#active));
on_off#set_active false;
let send_ppm = fun () ->
if on_off#active then send_ppm () in
sw/tools/gen_flight_plan.ml
+15 -7
View File
@@ -374,7 +374,7 @@ let rec print_stage = fun index_of_waypoints sectors x ->
lprintf "return;\n"
| "set" ->
stage ();
let var = parsed_attrib x "var" in
let var = ExtXml.attrib x "var" in
let valuee = parsed_attrib x "value" in
lprintf "%s = %s;\n" var valuee;
begin
@@ -474,16 +474,23 @@ let print_blocks = fun index_of_waypoints sectors bs ->
List.iter (fun b -> incr block; print_block index_of_waypoints sectors b !block) bs
let define_home = fun waypoints ->
let rec loop i = function
let define_waypoint_ids = fun waypoints ->
let i = ref 0 in
List.iter (fun w ->
Xml2h.define (sprintf "WP_%s" (name_of w)) (string_of_int !i);
incr i)
waypoints
let home = fun waypoints ->
let rec loop = function
[] -> failwith "Waypoint 'HOME' required"
| w::ws ->
if name_of w = "HOME" then begin
Xml2h.define "WP_HOME" (string_of_int i);
(float_attrib w "x", float_attrib w "y")
end else
loop (i+1) ws in
loop 0 waypoints
loop ws in
loop waypoints
let check_distance = fun (hx, hy) max_d wp ->
let x = float_attrib wp "x"
@@ -717,7 +724,8 @@ let _ =
let waypoints = dummy_waypoint :: Xml.children waypoints in
let (hx, hy) = define_home waypoints in
define_waypoint_ids waypoints;
let (hx, hy) = home waypoints in
List.iter (check_distance (hx, hy) mdfh) waypoints;
Xml2h.define "WAYPOINTS" "{ \\";