mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-10 15:09:25 +08:00
Map Display rewritten
This commit is contained in:
Pascal Brisset
+16
-16
@@ -324,8 +324,8 @@
|
||||
<field name="ac_id" type="string"/>
|
||||
<field name="roll" type="float" unit="deg"/>
|
||||
<field name="pitch" type="float" unit="deg"/>
|
||||
<field name="east" type="float" unit="m"/>
|
||||
<field name="north" type="float" unit="m"/>
|
||||
<field name="lat" type="float" unit="deg"/>
|
||||
<field name="long" type="float" unit="deg"/>
|
||||
<field name="speed" type="float" unit="m/s"/>
|
||||
<field name="course" type="float" unit="deg" format="%.1f"/>
|
||||
<field name="alt" type="float" unit="m"/>
|
||||
@@ -348,8 +348,8 @@
|
||||
<field name="cur_stage" type="uint8"/>
|
||||
<field name="block_time" type="uint32"/>
|
||||
<field name="stage_time" type="uint32"/>
|
||||
<field name="target_east" type="float" unit="m"/>
|
||||
<field name="target_north" type="float" unit="m"/>
|
||||
<field name="target_lat" type="float" unit="deg"/>
|
||||
<field name="target_long" type="float" unit="deg"/>
|
||||
<field name="target_climb" type="float" unit="m/s"/>
|
||||
<field name="target_alt" type="float" unit="m"/>
|
||||
<field name="target_course" type="float" unit="deg"/>
|
||||
@@ -357,10 +357,10 @@
|
||||
|
||||
<message name="CAM_STATUS" ID="14">
|
||||
<field name="ac_id" type="string"/>
|
||||
<field name="cam_east" type="float" unit="m" format="%.1f"/>
|
||||
<field name="cam_north" type="float" unit="m" format="%.1f"/>
|
||||
<field name="target_east" type="float" unit="m"/>
|
||||
<field name="target_north" type="float" unit="m"/>
|
||||
<field name="cam_lat" type="float" unit="deg"/>
|
||||
<field name="cam_long" type="float" unit="deg"/>
|
||||
<field name="cam_target_lat" type="float" unit="deg"/>
|
||||
<field name="cam_target_long" type="float" unit="deg"/>
|
||||
</message>
|
||||
|
||||
<message name="ENGINE_STATUS" ID="15">
|
||||
@@ -417,24 +417,24 @@
|
||||
|
||||
<message name="CIRCLE_STATUS" ID="22">
|
||||
<field name="ac_id" type="string"/>
|
||||
<field name="circle_east" type="float" unit="m"/>
|
||||
<field name="circle_north" type="float" unit="m"/>
|
||||
<field name="circle_lat" type="float" unit="deg"/>
|
||||
<field name="circle_long" type="float" unit="deg"/>
|
||||
<field name="radius" type="int16" unit="m"/>
|
||||
</message>
|
||||
|
||||
<message name="SEGMENT_STATUS" ID="23">
|
||||
<field name="ac_id" type="string"/>
|
||||
<field name="segment1_east" type="float" unit="m"/>
|
||||
<field name="segment1_north" type="float" unit="m"/>
|
||||
<field name="segment2_east" type="float" unit="m"/>
|
||||
<field name="segment2_north" type="float" unit="m"/>
|
||||
<field name="segment1_lat" type="float" unit="deg"/>
|
||||
<field name="segment1_long" type="float" unit="deg"/>
|
||||
<field name="segment2_lat" type="float" unit="deg"/>
|
||||
<field name="segment2_long" type="float" unit="deg"/>
|
||||
</message>
|
||||
|
||||
<message name="MOVE_WAYPOINT" ID="24">
|
||||
<field name="ac_id" type="string"/>
|
||||
<field name="wp_id" type="uint8"/>
|
||||
<field name="utm_east" type="float" unit="m"></field>
|
||||
<field name="utm_north" type="float" unit="m"></field>
|
||||
<field name="lat" type="float" unit="deg"></field>
|
||||
<field name="long" type="float" unit="deg"></field>
|
||||
<field name="alt" type="float" unit="m"></field>
|
||||
</message>
|
||||
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
<map file="muret_UTM.gif" projection="UTM" scale="2.5" approx_ground_altitude="185">
|
||||
<point x="0" y="800" utm_x="359000" utm_y="4813000"/>
|
||||
<point x="0" y="0" utm_x="359000" utm_y="4815000"/>
|
||||
<map file="muret_UTM.gif" projection="UTM" scale="2.5" approx_ground_altitude="185" utm_zone="31">
|
||||
<point x="0" y="0" utm_x="359000" utm_y="4815000" geo="WGS84 43.474630 1.256652"/>
|
||||
<point x="0" y="800" utm_x="359000" utm_y="4813000" geo="WGS84 43.456629 1.257169"/>
|
||||
<point x="800" y="800" utm_x="361000" utm_y="4813000"/>
|
||||
</map>
|
||||
|
||||
@@ -10,7 +10,7 @@ all : map2d
|
||||
opt : map2d.opt
|
||||
|
||||
|
||||
map2d : map2d.ml
|
||||
map2d : map2d.ml ../../lib/ocaml/lib-pprz.cma ../../lib/ocaml/xlib-pprz.cma
|
||||
$(OCAMLC) -thread -custom $(INCLUDES) $(LIBS) threads.cma gtkThread.cmo gtkInit.cmo $< -o $@
|
||||
|
||||
map2d.opt : map2d.cmx
|
||||
|
||||
+119
-247
File diff suppressed because it is too large
Load Diff
@@ -77,7 +77,7 @@ type aircraft = {
|
||||
mutable pos : Latlong.utm;
|
||||
mutable roll : float;
|
||||
mutable pitch : float;
|
||||
mutable nav_ref : Latlong.utm;
|
||||
mutable nav_ref : Latlong.utm option;
|
||||
mutable desired_east : float;
|
||||
mutable desired_north : float;
|
||||
mutable desired_altitude : float;
|
||||
|
||||
@@ -59,7 +59,7 @@ type aircraft = {
|
||||
mutable pos : Latlong.utm;
|
||||
mutable roll : float;
|
||||
mutable pitch : float;
|
||||
mutable nav_ref : Latlong.utm;
|
||||
mutable nav_ref : Latlong.utm option;
|
||||
mutable desired_east : float;
|
||||
mutable desired_north : float;
|
||||
mutable desired_altitude : float;
|
||||
|
||||
@@ -24,6 +24,7 @@
|
||||
*
|
||||
*)
|
||||
|
||||
open Latlong
|
||||
open Printf
|
||||
module W = Wavecard
|
||||
module Tm_Pprz = Pprz.Protocol(struct let name = "telemetry_ap" end)
|
||||
@@ -146,13 +147,15 @@ let move_wp = fun ac _sender vs ->
|
||||
let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
|
||||
if ac_id = ac.id then
|
||||
let f = fun a -> Pprz.float_assoc a vs in
|
||||
let ux = f "utm_east"
|
||||
and uy = f "utm_north"
|
||||
let lat = f "lat"
|
||||
and long = f "long"
|
||||
and alt = f "alt"
|
||||
and wp_id = Pprz.int_assoc "wp_id" vs in
|
||||
let wgs84 = {posn_lat=(Deg>>Rad)lat;posn_long=(Deg>>Rad)long} in
|
||||
let utm = Latlong.utm_of WGS84 wgs84 in
|
||||
let vs = ["wp_id", Pprz.Int wp_id;
|
||||
"utm_east", cm_of_m ux;
|
||||
"utm_north", cm_of_m uy;
|
||||
"utm_east", cm_of_m utm.utm_x;
|
||||
"utm_north", cm_of_m utm.utm_y;
|
||||
"alt", cm_of_m alt] in
|
||||
let msg_id, _ = Dl_Pprz.message_of_name "MOVE_WP" in
|
||||
let s = Dl_Pprz.payload_of_values msg_id ground_id vs in
|
||||
|
||||
@@ -176,7 +176,7 @@ let log_and_parse = fun logging ac_name a msg values ->
|
||||
a.desired_altitude <- fvalue "desired_altitude";
|
||||
a.desired_climb <- fvalue "desired_climb"
|
||||
| "NAVIGATION_REF" ->
|
||||
a.nav_ref <- { utm_x = fvalue "utm_east"; utm_y = fvalue "utm_north"; utm_zone = a.pos.utm_zone }
|
||||
a.nav_ref <- Some { utm_x = fvalue "utm_east"; utm_y = fvalue "utm_north"; utm_zone = ivalue "utm_zone" }
|
||||
| "ATTITUDE" ->
|
||||
a.roll <- (Deg>>Rad) (fvalue "phi");
|
||||
a.pitch <- (Deg>>Rad) (fvalue "theta")
|
||||
@@ -240,11 +240,21 @@ let log_and_parse = fun logging ac_name a msg values ->
|
||||
azim = ivalue "Azim";
|
||||
}
|
||||
| "CIRCLE" ->
|
||||
a.horiz_mode <- Circle (Latlong.utm_add a.nav_ref (fvalue "center_east") (fvalue "center_north"), ivalue "radius")
|
||||
begin
|
||||
match a.nav_ref with
|
||||
Some nav_ref ->
|
||||
a.horiz_mode <- Circle (Latlong.utm_add nav_ref (fvalue "center_east", fvalue "center_north"), ivalue "radius")
|
||||
| None -> ()
|
||||
end
|
||||
| "SEGMENT" ->
|
||||
let p1 = Latlong.utm_add a.nav_ref (fvalue "segment_east_1") (fvalue "segment_north_1")
|
||||
and p2 = Latlong.utm_add a.nav_ref (fvalue "segment_east_2") (fvalue "segment_north_2") in
|
||||
begin
|
||||
match a.nav_ref with
|
||||
Some nav_ref ->
|
||||
let p1 = Latlong.utm_add nav_ref (fvalue "segment_east_1", fvalue "segment_north_1")
|
||||
and p2 = Latlong.utm_add nav_ref (fvalue "segment_east_2", fvalue "segment_north_2") in
|
||||
a.horiz_mode <- Segment (p1, p2)
|
||||
| None -> ()
|
||||
end
|
||||
| "CALIBRATION" ->
|
||||
a.throttle_accu <- fvalue "climb_sum_err"
|
||||
| _ -> ()
|
||||
@@ -267,9 +277,13 @@ let send_cam_status = fun a ->
|
||||
let dx = h *. tan (a.cam.phi -. a.roll)
|
||||
and dy = h *. tan (a.cam.theta +. a.pitch) in
|
||||
let alpha = -. a.course in
|
||||
let east = a.pos.utm_x +. dx *. cos alpha -. dy *. sin alpha
|
||||
and north = a.pos.utm_y +. dx *. sin alpha +. dy *. cos alpha in
|
||||
let values = ["ac_id", Pprz.String a.id; "cam_east", Pprz.Float east; "cam_north", Pprz.Float north] in
|
||||
let east = dx *. cos alpha -. dy *. sin alpha
|
||||
and north = dx *. sin alpha +. dy *. cos alpha in
|
||||
let utm = Latlong.utm_add a.pos (east, north) in
|
||||
let wgs84 = Latlong.of_utm WGS84 utm in
|
||||
let values = ["ac_id", Pprz.String a.id;
|
||||
"cam_lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat);
|
||||
"cam_long", Pprz.Float ((Rad>>Deg)wgs84.posn_long)] in
|
||||
Ground_Pprz.message_send my_id "CAM_STATUS" values
|
||||
|
||||
let send_if_calib = fun a ->
|
||||
@@ -322,17 +336,20 @@ let send_svsinfo = fun a ->
|
||||
let send_horiz_status = fun a ->
|
||||
match a.horiz_mode with
|
||||
Circle (utm, r) ->
|
||||
let wgs84 = Latlong.of_utm WGS84 utm in
|
||||
let vs = [ "ac_id", Pprz.String a.id;
|
||||
"circle_east", Pprz.Float utm.utm_x;
|
||||
"circle_north", Pprz.Float utm.utm_y;
|
||||
"circle_lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat);
|
||||
"circle_long", Pprz.Float ((Rad>>Deg)wgs84.posn_long);
|
||||
"radius", Pprz.Int r ] in
|
||||
Ground_Pprz.message_send my_id "CIRCLE_STATUS" vs
|
||||
| Segment (u1, u2) ->
|
||||
let geo1 = Latlong.of_utm WGS84 u1 in
|
||||
let geo2 = Latlong.of_utm WGS84 u2 in
|
||||
let vs = [ "ac_id", Pprz.String a.id;
|
||||
"segment1_east", Pprz.Float u1.utm_x;
|
||||
"segment1_north", Pprz.Float u1.utm_y;
|
||||
"segment2_east", Pprz.Float u2.utm_x;
|
||||
"segment2_north", Pprz.Float u2.utm_y ] in
|
||||
"segment1_lat", Pprz.Float ((Rad>>Deg)geo1.posn_lat);
|
||||
"segment1_long", Pprz.Float ((Rad>>Deg)geo1.posn_long);
|
||||
"segment2_lat", Pprz.Float ((Rad>>Deg)geo2.posn_lat);
|
||||
"segment2_long", Pprz.Float ((Rad>>Deg)geo2.posn_long) ] in
|
||||
Ground_Pprz.message_send my_id "SEGMENT_STATUS" vs
|
||||
| UnknownHorizMode -> ()
|
||||
|
||||
@@ -356,29 +373,37 @@ let send_aircraft_msg = fun ac ->
|
||||
try
|
||||
let a = Hashtbl.find aircrafts ac in
|
||||
let f = fun x -> Pprz.Float x in
|
||||
let wgs84 = Latlong.of_utm WGS84 a.pos in
|
||||
let values = ["ac_id", Pprz.String ac;
|
||||
"roll", f (Geometry_2d.rad2deg a.roll);
|
||||
"pitch", f (Geometry_2d.rad2deg a.pitch);
|
||||
"east", f a.pos.utm_x;
|
||||
"north", f a.pos.utm_y;
|
||||
"lat", f ((Rad>>Deg)wgs84.posn_lat);
|
||||
"long", f ((Rad>>Deg) wgs84.posn_long);
|
||||
"speed", f a.gspeed;
|
||||
"course", f (Geometry_2d.rad2deg a.course);
|
||||
"alt", f a.alt;
|
||||
"climb", f a.climb] in
|
||||
Ground_Pprz.message_send my_id "FLIGHT_PARAM" values;
|
||||
|
||||
let values = ["ac_id", Pprz.String ac;
|
||||
"cur_block", Pprz.Int a.cur_block;
|
||||
"cur_stage", Pprz.Int a.cur_stage;
|
||||
"stage_time", Pprz.Int a.stage_time;
|
||||
"block_time", Pprz.Int a.block_time;
|
||||
"target_east", f (a.nav_ref.utm_x+.a.desired_east);
|
||||
"target_north", f (a.nav_ref.utm_y+.a.desired_north);
|
||||
"target_alt", Pprz.Float a.desired_altitude;
|
||||
"target_climb", Pprz.Float a.desired_climb;
|
||||
"target_course", Pprz.Float ((Rad>>Deg)a.desired_course)
|
||||
] in
|
||||
Ground_Pprz.message_send my_id "NAV_STATUS" values;
|
||||
begin
|
||||
match a.nav_ref with
|
||||
Some nav_ref ->
|
||||
let target_utm = Latlong.utm_add nav_ref (a.desired_east, a.desired_north) in
|
||||
let target_wgs84 = Latlong.of_utm WGS84 target_utm in
|
||||
let values = ["ac_id", Pprz.String ac;
|
||||
"cur_block", Pprz.Int a.cur_block;
|
||||
"cur_stage", Pprz.Int a.cur_stage;
|
||||
"stage_time", Pprz.Int a.stage_time;
|
||||
"block_time", Pprz.Int a.block_time;
|
||||
"target_lat", f ((Rad>>Deg)target_wgs84.posn_lat);
|
||||
"target_long", f ((Rad>>Deg)target_wgs84.posn_long);
|
||||
"target_alt", Pprz.Float a.desired_altitude;
|
||||
"target_climb", Pprz.Float a.desired_climb;
|
||||
"target_course", Pprz.Float ((Rad>>Deg)a.desired_course)
|
||||
] in
|
||||
Ground_Pprz.message_send my_id "NAV_STATUS" values
|
||||
| None -> () (* No nav_ref yet *)
|
||||
end;
|
||||
|
||||
let values = ["ac_id", Pprz.String ac;
|
||||
"throttle", f a.throttle;
|
||||
@@ -426,7 +451,7 @@ let new_aircraft = fun id ->
|
||||
desired_altitude = 0.;
|
||||
desired_climb = 0.;
|
||||
pos = { utm_x = 0.; utm_y = 0.; utm_zone = 0 };
|
||||
nav_ref = { utm_x = 0.; utm_y = 0.; utm_zone = 0 };
|
||||
nav_ref = None;
|
||||
cam = { phi = 0.; theta = 0. };
|
||||
inflight_calib = { if_mode = 1 ; if_val1 = 0.; if_val2 = 0.};
|
||||
infrared = infrared_init;
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
(*
|
||||
* $Id$
|
||||
*
|
||||
* Multi aircrafts receiver, logger and broadcaster
|
||||
* Connection of a wavecard to the Ivy bus
|
||||
*
|
||||
* Copyright (C) 2004 CENA/ENAC, Pascal Brisset, Antoine Drouin
|
||||
*
|
||||
@@ -24,6 +24,7 @@
|
||||
*
|
||||
*)
|
||||
|
||||
open Latlong
|
||||
open Printf
|
||||
module W = Wavecard
|
||||
module Tm_Pprz = Pprz.Protocol(struct let name = "telemetry_ap" end)
|
||||
@@ -98,13 +99,15 @@ let move_wp = fun ac _sender vs ->
|
||||
let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
|
||||
if ac_id = ac.id then
|
||||
let f = fun a -> Pprz.float_assoc a vs in
|
||||
let ux = f "utm_east"
|
||||
and uy = f "utm_north"
|
||||
let lat = f "lat"
|
||||
and long = f "long"
|
||||
and alt = f "alt"
|
||||
and wp_id = Pprz.int_assoc "wp_id" vs in
|
||||
let wgs84 = {posn_lat=(Deg>>Rad)lat;posn_long=(Deg>>Rad)long} in
|
||||
let utm = Latlong.utm_of WGS84 wgs84 in
|
||||
let vs = ["wp_id", Pprz.Int wp_id;
|
||||
"utm_east", cm_of_m ux;
|
||||
"utm_north", cm_of_m uy;
|
||||
"utm_east", cm_of_m utm.utm_x;
|
||||
"utm_north", cm_of_m utm.utm_y;
|
||||
"alt", cm_of_m alt] in
|
||||
let msg_id, _ = Dl_Pprz.message_of_name "MOVE_WP" in
|
||||
let s = Dl_Pprz.payload_of_values msg_id ground_id vs in
|
||||
|
||||
@@ -30,7 +30,7 @@ SRC = debug.ml env.ml serial.ml ocaml_tools.ml extXml.ml xml2h.ml latlong.ml srt
|
||||
CMO = $(SRC:.ml=.cmo)
|
||||
CMX = $(SRC:.ml=.cmx)
|
||||
|
||||
XSRC = platform.ml gtkgl_Hack.ml ml_gtkgl_hack.o gtk_image.ml gtk_tools_icons.ml gtk_tools.ml gtk_draw.ml gtk_tools_GL.ml gtk_3d.ml mapCanvas.ml mapWaypoints.ml mapTrack.ml ml_gtk_drag.o xmlEdit.ml
|
||||
XSRC = platform.ml gtkgl_Hack.ml ml_gtkgl_hack.o gtk_image.ml gtk_tools_icons.ml gtk_tools.ml gtk_draw.ml gtk_tools_GL.ml gtk_3d.ml mapCanvas.ml mapWaypoints.ml mapTrack.ml mapGoogle.ml ml_gtk_drag.o xmlEdit.ml
|
||||
XCMO = $(XSRC:.ml=.cmo)
|
||||
XCMX = $(XSRC:.ml=.cmx)
|
||||
|
||||
|
||||
+22
-10
@@ -44,11 +44,20 @@ let (/.=) r x = r := !r /. x
|
||||
let (+.=) r x = r := !r +. x
|
||||
let (-.=) r x = r := !r -. x
|
||||
|
||||
let inv_norm_lat = fun l -> Latlong.inv_mercator_lat (l *. pi)
|
||||
let norm_lat = fun l -> Latlong.mercator_lat l /. pi
|
||||
|
||||
let tile_coverage = fun lat zoom ->
|
||||
let normed_size = 2. /. (2. ** (float (18-zoom))) in
|
||||
let normed_lat = norm_lat lat in
|
||||
let normed_lat' = normed_lat +. normed_size in
|
||||
let lat' = inv_norm_lat normed_lat' in
|
||||
(normed_size, lat' -. lat)
|
||||
|
||||
|
||||
let gm_pos_and_scale = fun keyholeString tLat latHeight tLon lonWidth ->
|
||||
let tmp_lat = fun l -> 2. *. atan (exp (l *. pi)) -. pi/.2. in
|
||||
let bot_lat = tmp_lat tLat in
|
||||
let top_lat = tmp_lat (tLat +. latHeight) in
|
||||
let bot_lat = inv_norm_lat tLat in
|
||||
let top_lat = inv_norm_lat (tLat +. latHeight) in
|
||||
let bottom_left = {posn_lat = bot_lat ; posn_long = tLon *. pi} in
|
||||
{ key = keyholeString;
|
||||
sw_corner = bottom_left;
|
||||
@@ -67,7 +76,8 @@ let tile_of_geo = fun wgs84 zoom ->
|
||||
let lon = lon /. 180. in
|
||||
|
||||
(* convert latitude to a range -1..+1 *)
|
||||
let lat = log (tan (pi/.4. +. 0.5*. wgs84.posn_lat)) /. pi in
|
||||
let lat = norm_lat wgs84.posn_lat in
|
||||
(*** log (tan (pi/.4. +. 0.5*. wgs84.posn_lat)) /. pi in ***)
|
||||
|
||||
let tLat = ref (-1.)
|
||||
and tLon = ref (-1.)
|
||||
@@ -151,10 +161,12 @@ let google_maps_url = fun s ->
|
||||
|
||||
exception Not_available
|
||||
|
||||
let get_image = fun no_http tile ->
|
||||
let no_http = ref false
|
||||
|
||||
let get_image = fun tile ->
|
||||
try get_from_cache tile.key with
|
||||
Not_found ->
|
||||
if no_http then raise Not_available;
|
||||
if !no_http then raise Not_available;
|
||||
let url = google_maps_url tile.key in
|
||||
let jpg_file = !cache_path // (tile.key ^ ".jpg") in
|
||||
try
|
||||
@@ -164,13 +176,13 @@ let get_image = fun no_http tile ->
|
||||
Http.Failure _ -> raise Not_available
|
||||
|
||||
|
||||
let rec get_tile = fun ?(no_http=false) wgs84 zoom ->
|
||||
let rec get_tile = fun wgs84 zoom ->
|
||||
if zoom < 10 then
|
||||
let tile = tile_of_geo wgs84 zoom in
|
||||
try get_image no_http tile with
|
||||
try get_image tile with
|
||||
(** Error, let's try a lower zoom *)
|
||||
Not_available when not no_http -> get_tile ~no_http wgs84 (zoom+1)
|
||||
Not_available when not !no_http -> get_tile wgs84 (zoom+1)
|
||||
else
|
||||
failwith "download_gm_tile"
|
||||
raise Not_available
|
||||
|
||||
|
||||
|
||||
+5
-1
@@ -25,6 +25,8 @@
|
||||
*)
|
||||
|
||||
val tile_size : int * int
|
||||
val tile_coverage : float -> int -> float * float
|
||||
(** [tile_coverage wgs84_lat zoom] Returns (width,height) *)
|
||||
|
||||
type tile_t = {
|
||||
key : string; (* [qrst] string *)
|
||||
@@ -43,8 +45,10 @@ val tile_of_key : string -> tile_t
|
||||
|
||||
val cache_path : string ref
|
||||
|
||||
val get_tile : ?no_http:bool -> Latlong.geographic -> int -> tile_t*string
|
||||
val get_tile : Latlong.geographic -> int -> tile_t*string
|
||||
(** May raise [Not_available] *)
|
||||
|
||||
exception Not_available
|
||||
|
||||
val no_http : bool ref
|
||||
|
||||
|
||||
+11
-1
@@ -335,9 +335,15 @@ let utm_distance = fun utm1 utm2 ->
|
||||
if utm1.utm_zone <> utm2.utm_zone then invalid_arg "utm_distance";
|
||||
sqrt ((utm1.utm_x -. utm2.utm_x)**2. +. (utm1.utm_y -. utm2.utm_y)**2.)
|
||||
|
||||
let utm_add = fun u x y ->
|
||||
let utm_add = fun u (x, y) ->
|
||||
{utm_x = u.utm_x +. x; utm_y = u.utm_y +. y; utm_zone = u.utm_zone }
|
||||
|
||||
let utm_sub = fun u1 u2 ->
|
||||
if u1.utm_zone <> u2.utm_zone then
|
||||
invalid_arg (Printf.sprintf "utm_sub: %d %d" u1.utm_zone u2.utm_zone);
|
||||
(u1.utm_x -. u2.utm_x, u1.utm_y -. u2.utm_y)
|
||||
|
||||
|
||||
let wgs84_of_lambertIIe = fun x y -> (WGS84<<NTF)(of_lambert lambertIIe {lbt_x = x; lbt_y = y})
|
||||
|
||||
let space = Str.regexp "[ \t]+"
|
||||
@@ -354,3 +360,7 @@ let of_string = fun s ->
|
||||
wgs84_of_lambertIIe (ios x) (ios y)
|
||||
| _ -> invalid_arg (Printf.sprintf "Latlong.of_string: %s" s)
|
||||
|
||||
|
||||
let mercator_lat = fun l -> log (tan (pi/.4. +. 0.5*. l))
|
||||
let inv_mercator_lat = fun l -> 2. *. atan (exp l) -. pi/.2.
|
||||
|
||||
|
||||
@@ -107,10 +107,20 @@ geographic coordinates and altitude expressed in geodesic referential
|
||||
|
||||
val utm_distance : utm -> utm -> fmeter
|
||||
|
||||
val utm_add : utm -> fmeter -> fmeter -> utm
|
||||
(** [add_utm utm east north] *)
|
||||
val utm_add : utm -> (fmeter * fmeter) -> utm
|
||||
(** [add_utm utm (east,north)] *)
|
||||
|
||||
val utm_sub : utm -> utm -> (fmeter * fmeter)
|
||||
(** [utm_sub u1 u2] Raises Invalid_arg if [u1] and [u2] are not in the same
|
||||
UTM zone *)
|
||||
|
||||
val wgs84_of_lambertIIe : meter -> meter -> geographic
|
||||
|
||||
val of_string : string -> geographic
|
||||
(** [of_string pos] Parses [pos] as "WGS84 45.678 1.2345", "UTM 500123 4500300 31" or "LBT2e 544945 1755355" *)
|
||||
|
||||
val mercator_lat : float -> float
|
||||
(** wgs84 -> [-pi; pi] *)
|
||||
|
||||
val inv_mercator_lat : float -> float
|
||||
(** [-pi; pi] -> wgs84 *)
|
||||
|
||||
+158
-110
@@ -1,13 +1,25 @@
|
||||
open Latlong
|
||||
module LL = Latlong
|
||||
open Printf
|
||||
|
||||
let zoom_factor = 1.5
|
||||
|
||||
let pan_step = 50
|
||||
|
||||
type meter = float
|
||||
type en = { east : meter; north : meter }
|
||||
|
||||
let distance = fun (x1,y1) (x2,y2) -> sqrt ((x1-.x2)**2.+.(y1-.y2)**2.)
|
||||
|
||||
let _ = Srtm.add_path "SRTM"
|
||||
|
||||
type utm_zone = int
|
||||
type projection =
|
||||
Mercator (* 1e-6 = 1 world unit, y axis reversed *)
|
||||
| UTM (* 1m = 1 world unit, y axis reversed *)
|
||||
| Lambert2 (* 1m = 1 world unit, y axis reversed *)
|
||||
|
||||
let default_georef = { LL.posn_lat = 0.; LL.posn_long = 0. }
|
||||
|
||||
let mercator_coeff = 5e6
|
||||
|
||||
|
||||
(** basic canvas with menubar **************************************
|
||||
@@ -15,12 +27,15 @@ let _ = Srtm.add_path "SRTM"
|
||||
*******************************************************************)
|
||||
|
||||
(* world_unit: m:pixel at scale 1. *)
|
||||
class basic_widget = fun ?(height=800) ?width ?wgs84_of_en () ->
|
||||
|
||||
class basic_widget = fun ?(height=800) ?width ?(projection = Mercator) ?georef () ->
|
||||
let canvas = GnoCanvas.canvas () in
|
||||
let background = GnoCanvas.group canvas#root in
|
||||
object (self)
|
||||
|
||||
(** GUI attributes *)
|
||||
|
||||
val background = background
|
||||
|
||||
val frame = GPack.vbox ~height ?width ()
|
||||
|
||||
val menubar = GMenu.menu_bar ()
|
||||
@@ -29,30 +44,24 @@ class basic_widget = fun ?(height=800) ?width ?wgs84_of_en () ->
|
||||
~value:1. ~lower:0.05 ~upper:10.
|
||||
~step_incr:0.25 ~page_incr:1.0 ~page_size:1.0 ()
|
||||
|
||||
val canvas = GnoCanvas.canvas ()
|
||||
|
||||
val bottom = GPack.hbox ~height:30 ()
|
||||
|
||||
val _w = GEdit.spin_button ~rate:0. ~digits:2 ~width:50
|
||||
~height:20 ()
|
||||
val _w = GEdit.spin_button ~rate:0. ~digits:2 ~width:50 ~height:20 ()
|
||||
|
||||
(***) val mutable factory = new GMenu.factory (GMenu.menu_bar ())
|
||||
val mutable factory = new GMenu.factory (GMenu.menu_bar ())
|
||||
|
||||
val mutable file_menu = GMenu.menu ()
|
||||
|
||||
val mutable lbl_x_axis = GMisc.label ~height:50 ()
|
||||
|
||||
|
||||
(** other attributes *)
|
||||
|
||||
val mutable current_zoom = 1.
|
||||
|
||||
val mutable projection = projection
|
||||
val mutable georef = georef
|
||||
val mutable dragging = None
|
||||
val mutable grouping = None
|
||||
val mutable rectangle = None
|
||||
val mutable world_unit = 1.
|
||||
val mutable wgs84_of_en = wgs84_of_en
|
||||
(***) val mutable background = GnoCanvas.pixbuf (GnoCanvas.canvas ())#root
|
||||
val mutable vertical_factor = 10.0
|
||||
val mutable vertical_max_level = 0.0
|
||||
|
||||
|
||||
method pack =
|
||||
@@ -70,11 +79,6 @@ class basic_widget = fun ?(height=800) ?width ?wgs84_of_en () ->
|
||||
|
||||
_w#set_adjustment adj;
|
||||
|
||||
background#destroy ();
|
||||
background <- GnoCanvas.pixbuf canvas#root;
|
||||
|
||||
(*** factory#destroy (); ***)
|
||||
|
||||
factory <- new GMenu.factory menubar;
|
||||
|
||||
file_menu#destroy ();
|
||||
@@ -90,7 +94,7 @@ class basic_widget = fun ?(height=800) ?width ?wgs84_of_en () ->
|
||||
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);
|
||||
ignore (adj#connect#value_changed (fun () -> self#zoom adj#value));
|
||||
ignore (adj#connect#value_changed (fun () -> canvas#set_pixels_per_unit adj#value));
|
||||
|
||||
canvas#set_center_scroll_region false ;
|
||||
canvas#set_scroll_region (-2500000.) (-2500000.) 2500000. 2500000.;
|
||||
@@ -101,21 +105,10 @@ class basic_widget = fun ?(height=800) ?width ?wgs84_of_en () ->
|
||||
|
||||
(** methods *)
|
||||
|
||||
|
||||
method set_wgs84_of_en = fun x -> wgs84_of_en <- Some x
|
||||
|
||||
method set_world_unit = fun x -> world_unit <- x
|
||||
|
||||
method get_world_unit = fun () -> world_unit
|
||||
|
||||
method set_lbl_x_axis = fun s -> lbl_x_axis#set_text s
|
||||
|
||||
(** accessors to instance variables *)
|
||||
method current_zoom = current_zoom
|
||||
method get_vertical_factor = vertical_factor
|
||||
method get_vertical_max_level = vertical_max_level
|
||||
method set_vertical_factor = fun x -> vertical_factor <- x
|
||||
method set_vertical_max_level = fun x -> vertical_max_level <- x
|
||||
method current_zoom = adj#value
|
||||
method canvas = canvas
|
||||
method frame = frame
|
||||
method factory = factory
|
||||
@@ -126,55 +119,98 @@ class basic_widget = fun ?(height=800) ?width ?wgs84_of_en () ->
|
||||
|
||||
(** following display functions can be redefined by subclasses.
|
||||
they do nothing in the basic_widget *)
|
||||
method display_xy = fun s -> ()
|
||||
method display_geo = fun s -> ()
|
||||
method display_alt = fun en -> ()
|
||||
method display_alt = fun wgs84 -> ()
|
||||
method display_group = fun s -> ()
|
||||
|
||||
(** converts relative utm coordinates into world (ie map) coordinates *)
|
||||
method world_of_en = fun en ->
|
||||
en.east /. world_unit, -. en.north /. world_unit
|
||||
method en_of_world = fun wx wy -> { east = wx *. world_unit;
|
||||
north = -. wy *. world_unit }
|
||||
method geo_string = fun en ->
|
||||
match wgs84_of_en with
|
||||
None -> ""
|
||||
| Some f -> string_degrees_of_geographic (f en)
|
||||
|
||||
method wgs84_of_en =
|
||||
match wgs84_of_en with
|
||||
None -> raise Not_found
|
||||
| Some f -> f
|
||||
|
||||
method georef = georef
|
||||
method set_georef = fun wgs84 -> georef <- Some wgs84
|
||||
|
||||
method world_of = fun wgs84 ->
|
||||
match georef with
|
||||
Some georef -> begin
|
||||
match projection with
|
||||
UTM ->
|
||||
let utmref = LL.utm_of LL.WGS84 georef
|
||||
and utm = LL.utm_of LL.WGS84 wgs84 in
|
||||
let (wx, y) = LL.utm_sub utm utmref in
|
||||
(wx, -.y)
|
||||
| Mercator ->
|
||||
let mlref = LL.mercator_lat georef.LL.posn_lat
|
||||
and ml = LL.mercator_lat wgs84.LL.posn_lat in
|
||||
let xw = (wgs84.LL.posn_long -. georef.LL.posn_long) *. mercator_coeff
|
||||
and yw = -. (ml -. mlref) *. mercator_coeff in
|
||||
(xw, yw)
|
||||
| _ -> failwith "#world_of : unknown projection"
|
||||
end
|
||||
| None -> failwith "#world_of : no georef"
|
||||
|
||||
method of_world = fun (wx, wy) ->
|
||||
match georef with
|
||||
Some georef -> begin
|
||||
match projection with
|
||||
UTM ->
|
||||
let utmref = LL.utm_of LL.WGS84 georef in
|
||||
LL.of_utm LL.WGS84 (LL.utm_add utmref (wx, -.wy))
|
||||
| Mercator ->
|
||||
let mlref = LL.mercator_lat georef.LL.posn_lat in
|
||||
let ml = mlref -. wy /. mercator_coeff in
|
||||
let lat = LL.inv_mercator_lat ml
|
||||
and long = wx /. mercator_coeff +. georef.LL.posn_long in
|
||||
{ LL.posn_lat = lat; posn_long = long }
|
||||
| _ -> failwith "#of_world : unknown projection"
|
||||
end
|
||||
| None -> failwith "#of_world : no georef"
|
||||
|
||||
|
||||
method geo_string = fun wgs84 ->
|
||||
LL.string_degrees_of_geographic wgs84
|
||||
|
||||
|
||||
method moveto = fun en ->
|
||||
let (xw, yw) = self#world_of_en en in
|
||||
method moveto = fun wgs84 ->
|
||||
let (xw, yw) = self#world_of wgs84 in
|
||||
let (xc, yc) = canvas#world_to_window xw yw in
|
||||
canvas#scroll_to (truncate xc) (truncate yc)
|
||||
|
||||
method center = fun wgs84 ->
|
||||
self#moveto wgs84;
|
||||
let sx_w, sy_w = Gdk.Drawable.get_size canvas#misc#window
|
||||
and (x, y) = canvas#get_scroll_offsets in
|
||||
canvas#scroll_to (x-sx_w/2) (y-sy_w/2)
|
||||
|
||||
method display_map = fun ?(scale = 1.) ?(anchor = (`ANCHOR `NW)) en image ->
|
||||
background <- GnoCanvas.pixbuf ~pixbuf:image ~props:[anchor] self#root;
|
||||
background#lower_to_bottom ();
|
||||
let wx, wy = self#world_of_en en in
|
||||
background#move wx wy;
|
||||
let a = background#i2w_affine in
|
||||
method display_map = fun ?(scale = 1.) ?(anchor = (`ANCHOR `NW)) wgs84 image ->
|
||||
let pix = GnoCanvas.pixbuf ~pixbuf:image ~props:[anchor] background in
|
||||
pix#lower_to_bottom ();
|
||||
let wx, wy = self#world_of wgs84 in
|
||||
pix#move wx wy;
|
||||
let a = pix#i2w_affine in
|
||||
a.(0) <- scale; a.(3) <- scale;
|
||||
background#affine_absolute a;
|
||||
background
|
||||
pix#affine_absolute a;
|
||||
pix
|
||||
|
||||
method display_pixbuf = fun ((x1,y1), geo1) ((x2,y2), geo2) image ->
|
||||
let x1 = float x1 and x2 = float x2
|
||||
and y1 = float y1 and y2 = float y2 in
|
||||
let pix = GnoCanvas.pixbuf ~x:(-.x1) ~y:(-.y1)~pixbuf:image ~props:[`ANCHOR `NW] background in
|
||||
let xw1, yw1 = self#world_of geo1
|
||||
and xw2, yw2 = self#world_of geo2 in
|
||||
let scale = distance (xw1, yw1) (xw2, yw2) /. distance (x1,y1) (x2,y2) in
|
||||
let a = atan2 (yw2-.yw1) (xw2-.xw1) -. atan2 (y2-.y1) (x2-.x1) in
|
||||
let cos_a = cos a *. scale and sin_a = sin a *. scale in
|
||||
pix#move xw1 yw1;
|
||||
pix#affine_relative [| cos_a; sin_a; -. sin_a; cos_a; 0.;0.|];
|
||||
pix
|
||||
|
||||
method zoom = fun value ->
|
||||
canvas#set_pixels_per_unit value;
|
||||
current_zoom <- value
|
||||
|
||||
adj#set_value value
|
||||
|
||||
|
||||
method mouse_motion = fun ev ->
|
||||
let xc = GdkEvent.Motion.x ev
|
||||
and yc = GdkEvent.Motion.y ev in
|
||||
let (xw, yw) = self#window_to_world xc yc in
|
||||
let en = self#en_of_world xw yw in
|
||||
self#display_xy (sprintf "%.0fm %.0fm\t" en.east en.north);
|
||||
self#display_geo (self#geo_string en);
|
||||
self#display_alt en;
|
||||
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 ) ->
|
||||
@@ -184,9 +220,11 @@ class basic_widget = fun ?(height=800) ?width ?wgs84_of_en () ->
|
||||
end;
|
||||
begin
|
||||
match grouping with
|
||||
Some (xw1, yw1) ->
|
||||
let en1 = self#en_of_world xw1 yw1 in
|
||||
self#display_group (sprintf "[%.1fkm %.1fkm]" ((en1.east -. en.east)/.1000.) ((en1.north-.en.north)/.1000.))
|
||||
Some starting_point ->
|
||||
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
|
||||
self#display_group (sprintf "[%.1fkm %.1fkm]" (east/.1000.) (north/.1000.))
|
||||
| None -> ()
|
||||
end;
|
||||
false
|
||||
@@ -195,11 +233,12 @@ class basic_widget = fun ?(height=800) ?width ?wgs84_of_en () ->
|
||||
match GdkEvent.Button.button ev, grouping with
|
||||
2, _ ->
|
||||
dragging <- None; false
|
||||
| 1, Some (xw1, yw1) ->
|
||||
| 1, Some starting_point ->
|
||||
let xc = GdkEvent.Button.x ev in
|
||||
let yc = GdkEvent.Button.y ev in
|
||||
let (xw2, yw2) = self#window_to_world xc yc in
|
||||
rectangle <- Some ((xw1, yw1), (xw2, yw2));
|
||||
let current_point = self#of_world (xw2, yw2) in
|
||||
rectangle <- Some (starting_point, current_point);
|
||||
self#display_group "";
|
||||
grouping <- None;
|
||||
false
|
||||
@@ -211,7 +250,7 @@ class basic_widget = fun ?(height=800) ?width ?wgs84_of_en () ->
|
||||
match GdkEvent.Button.button ev with
|
||||
1 ->
|
||||
let xyw = self#window_to_world xc yc in
|
||||
grouping <- Some xyw;
|
||||
grouping <- Some (self#of_world xyw);
|
||||
true
|
||||
| 2 when Gdk.Convert.test_modifier `SHIFT (GdkEvent.Button.state ev) ->
|
||||
dragging <- Some (xc, yc);
|
||||
@@ -232,31 +271,54 @@ class basic_widget = fun ?(height=800) ?width ?wgs84_of_en () ->
|
||||
method any_event = fun ev ->
|
||||
match GdkEvent.get_type ev with
|
||||
| `SCROLL -> begin
|
||||
match GdkEvent.Scroll.direction (GdkEvent.Scroll.cast ev) with
|
||||
let scroll_event = GdkEvent.Scroll.cast ev in
|
||||
let (x, y) = canvas#get_scroll_offsets in
|
||||
let xr = GdkEvent.Scroll.x_root scroll_event in
|
||||
let yr = GdkEvent.Scroll.y_root scroll_event -. 35. in
|
||||
match GdkEvent.Scroll.direction scroll_event with
|
||||
`UP ->
|
||||
adj#set_value (adj#value+.adj#step_increment) ;
|
||||
canvas#scroll_to (x+truncate xr) (y+truncate yr);
|
||||
|
||||
adj#set_value (adj#value*.zoom_factor);
|
||||
|
||||
let (x, y) = canvas#get_scroll_offsets in
|
||||
canvas#scroll_to (x-truncate (xr)) (y-truncate (yr));
|
||||
true
|
||||
| `DOWN ->
|
||||
canvas#scroll_to (x+truncate xr) (y+truncate yr);
|
||||
|
||||
adj#set_value (adj#value/.zoom_factor);
|
||||
|
||||
let (x, y) = canvas#get_scroll_offsets in
|
||||
canvas#scroll_to (x-truncate (xr)) (y-truncate (yr));
|
||||
true
|
||||
| `DOWN -> adj#set_value (adj#value-.adj#step_increment) ; true
|
||||
| _ -> false
|
||||
end
|
||||
| _ -> false
|
||||
|
||||
|
||||
method segment = fun ?(group = canvas#root) ?(width=1) ?fill_color en1 en2 ->
|
||||
let (x1, y1) = self#world_of_en en1
|
||||
and (x2, y2) = self#world_of_en en2 in
|
||||
method segment = fun ?(group = canvas#root) ?(width=1) ?fill_color geo1 geo2 ->
|
||||
let (x1, y1) = self#world_of geo1
|
||||
and (x2, y2) = self#world_of geo2 in
|
||||
let l = GnoCanvas.line ?fill_color ~props:[`WIDTH_PIXELS width] ~points:[|x1;y1;x2;y2|] group in
|
||||
l#show ();
|
||||
l
|
||||
|
||||
method circle = fun ?(group = canvas#root) ?(width=1) ?fill_color ?(color="black") en rad ->
|
||||
let (x, y) = self#world_of_en en in
|
||||
let rad = rad /. world_unit in
|
||||
method circle = fun ?(group = canvas#root) ?(width=1) ?fill_color ?(color="black") geo radius ->
|
||||
let (x, y) = self#world_of geo in
|
||||
|
||||
(** Compute the actual radius in a UTM projection *)
|
||||
let utm = LL.utm_of LL.WGS84 geo in
|
||||
let geo_east = LL.of_utm LL.WGS84 (LL.utm_add utm (radius, 0.)) in
|
||||
let (xe, _) = self#world_of geo_east in
|
||||
let rad = xe -. x in
|
||||
|
||||
let l = GnoCanvas.ellipse ?fill_color ~props:[`WIDTH_PIXELS width; `OUTLINE_COLOR color] ~x1:(x-.rad) ~y1:(y -.rad) ~x2:(x +.rad) ~y2:(y+.rad) group in
|
||||
l#show ();
|
||||
l
|
||||
method text = fun ?(group = canvas#root) ?(fill_color = "blue") ?(x_offset = 0.0) ?(y_offset = 0.0) en1 text ->
|
||||
let (x1, y1) = self#world_of_en en1 in
|
||||
|
||||
method text = fun ?(group = canvas#root) ?(fill_color = "blue") ?(x_offset = 0.0) ?(y_offset = 0.0) geo text ->
|
||||
let (x1, y1) = self#world_of geo in
|
||||
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
|
||||
@@ -271,15 +333,15 @@ class basic_widget = fun ?(height=800) ?width ?wgs84_of_en () ->
|
||||
****************************************************************)
|
||||
|
||||
|
||||
class widget = fun ?(height=800) ?width ?wgs84_of_en () ->
|
||||
class widget = fun ?(height=800) ?width ?projection ?georef () ->
|
||||
object(self)
|
||||
inherit (basic_widget ~height:height ?width ?wgs84_of_en ())
|
||||
inherit (basic_widget ~height ?width ?projection ?georef ())
|
||||
|
||||
val mutable lbl_xy = GMisc.label ~height:50 ()
|
||||
val mutable lbl_geo = GMisc.label ~height:50 ()
|
||||
val mutable lbl_alt = GMisc.label ~height:50 ()
|
||||
val mutable lbl_group = GMisc.label ~height:50 ()
|
||||
(***) val mutable menu_fact = new GMenu.factory (GMenu.menu ())
|
||||
val mutable menu_fact = new GMenu.factory (GMenu.menu ())
|
||||
val mutable srtm = GMenu.check_menu_item ()
|
||||
|
||||
method pack_labels =
|
||||
@@ -314,13 +376,9 @@ class widget = fun ?(height=800) ?width ?wgs84_of_en () ->
|
||||
|
||||
method display_xy = fun s -> lbl_xy#set_text s
|
||||
method display_geo = fun s -> lbl_geo#set_text s
|
||||
method display_alt = fun en ->
|
||||
begin
|
||||
match wgs84_of_en, srtm#active with
|
||||
Some wgs84_of_en, true ->
|
||||
lbl_alt#set_text (sprintf "\t%dm"(self#altitude (wgs84_of_en en)))
|
||||
| _ -> ()
|
||||
end
|
||||
method display_alt = fun wgs84 ->
|
||||
if srtm#active then
|
||||
lbl_alt#set_text (sprintf "\t%dm"(self#altitude wgs84))
|
||||
|
||||
method display_group = fun s -> lbl_group#set_text s
|
||||
|
||||
@@ -328,20 +386,10 @@ class widget = fun ?(height=800) ?width ?wgs84_of_en () ->
|
||||
method switch_background = fun x -> if x then background#show () else background#hide ()
|
||||
|
||||
method goto = fun () ->
|
||||
let dialog = GWindow.window ~border_width:10 ~title:"Geo ref" () in
|
||||
let dvbx = GPack.box `VERTICAL ~packing:dialog#add () in
|
||||
let lat = GEdit.entry ~packing:dvbx#add () in
|
||||
let lon = GEdit.entry ~packing:dvbx#add () in
|
||||
let cancel = GButton.button ~label:"Cancel" ~packing: dvbx#add () in
|
||||
let ok = GButton.button ~label:"OK" ~packing: dvbx#add () in
|
||||
ignore(cancel#connect#clicked ~callback:dialog#destroy);
|
||||
ignore(ok#connect#clicked ~callback:
|
||||
begin fun _ ->
|
||||
let x = float_of_string lat#text in
|
||||
let y = float_of_string lon#text in
|
||||
self#moveto {east=x; north=y};
|
||||
dialog#destroy ()
|
||||
end);
|
||||
dialog#show ()
|
||||
match GToolbox.input_string ~title:"Geo ref" ~text:"WGS84 " "Geo ref" with
|
||||
Some s ->
|
||||
let wgs84 = Latlong.of_string s in
|
||||
self#moveto wgs84
|
||||
| None -> ()
|
||||
|
||||
end
|
||||
|
||||
@@ -0,0 +1,110 @@
|
||||
(*
|
||||
* $Id$
|
||||
*
|
||||
* Displaying Google Maps on a MapCanvas object
|
||||
*
|
||||
* Copyright (C) 2004-2006 ENAC, Pascal Brisset, Antoine Drouin
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*
|
||||
*)
|
||||
|
||||
module LL = Latlong
|
||||
|
||||
(** Quadtreee of displayed tiles *)
|
||||
type tiles_tree =
|
||||
Empty
|
||||
| Tile
|
||||
| Node of tiles_tree array
|
||||
let gm_tiles = Node (Array.create 4 Empty)
|
||||
|
||||
(** Google Maps paths in the quadtree are coded with q,r,s and t*)
|
||||
let index_of = function
|
||||
'q' -> 0 | 'r' -> 1 | 's' -> 2 | 't' -> 3
|
||||
| _ -> invalid_arg "index_of"
|
||||
|
||||
(** Checking that a tile is already displayed *)
|
||||
let mem_tile = fun tile_key ->
|
||||
let rec loop = fun i tree ->
|
||||
tree = Tile ||
|
||||
i < String.length tile_key &&
|
||||
match tree with
|
||||
Empty -> false
|
||||
| Tile -> true
|
||||
| Node sons -> loop (i+1) sons.(index_of tile_key.[i]) in
|
||||
loop 0 gm_tiles
|
||||
|
||||
(** Adding a tile to the store *)
|
||||
let add_tile = fun tile_key ->
|
||||
let rec loop = fun i tree j ->
|
||||
if i < String.length tile_key then
|
||||
match tree.(j) with
|
||||
Empty ->
|
||||
let sons = Array.create 4 Empty in
|
||||
tree.(j) <- Node sons;
|
||||
loop (i+1) sons (index_of tile_key.[i])
|
||||
| Tile -> () (* Already there *)
|
||||
| Node sons ->
|
||||
loop (i+1) sons (index_of tile_key.[i])
|
||||
else
|
||||
tree.(j) <- Tile in
|
||||
loop 0 [|gm_tiles|] 0
|
||||
|
||||
|
||||
(** Displaying the tile around the given point *)
|
||||
let display_tile = fun (geomap:MapCanvas.widget) wgs84 ->
|
||||
let desired_tile = Gm.tile_of_geo wgs84 1 in
|
||||
|
||||
let key = desired_tile.Gm.key in
|
||||
if not (mem_tile key) then
|
||||
let (tile, jpg_file) = Gm.get_tile wgs84 1 in
|
||||
let south_lat = tile.Gm.sw_corner.LL.posn_lat
|
||||
and west_long = tile.Gm.sw_corner.LL.posn_long in
|
||||
let north_lat = south_lat +. tile.Gm.height
|
||||
and east_long = west_long +. tile.Gm.width in
|
||||
let ne = { LL.posn_lat = north_lat; posn_long = east_long } in
|
||||
|
||||
|
||||
let map = geomap#display_pixbuf ((0,256), tile.Gm.sw_corner) ((256,0),ne) (GdkPixbuf.from_file jpg_file) in
|
||||
map#raise 1;
|
||||
add_tile key
|
||||
|
||||
|
||||
(** Filling the window with tiles *)
|
||||
let fill_window = fun (geomap:MapCanvas.widget) ->
|
||||
let width_c, height_c = Gdk.Drawable.get_size geomap#canvas#misc#window
|
||||
and (xc0, yc0) = geomap#canvas#get_scroll_offsets in
|
||||
let (xw0, yw0) = geomap#window_to_world (float xc0) (float yc0)
|
||||
and (xw1, yw1) = geomap#window_to_world (float (xc0+width_c)) (float (yc0+height_c)) in
|
||||
let nw = geomap#of_world (xw0, yw0)
|
||||
and se = geomap#of_world (xw1, yw1) in
|
||||
|
||||
(* Hypothesis: no strong variation of the height of the tiles on the whole area *)
|
||||
let (width_tile, height_tile) = Gm.tile_coverage se.LL.posn_lat 1 in
|
||||
for ilong = 0 to truncate ((se.LL.posn_long -. nw.LL.posn_long) /. width_tile) do
|
||||
let long = nw.LL.posn_long +. float ilong *. width_tile in
|
||||
for ilat = 0 to truncate ((nw.LL.posn_lat -. se.LL.posn_lat) /. height_tile) do
|
||||
let lat = nw.LL.posn_lat -. float ilat *. height_tile in
|
||||
let wgs84 = { LL.posn_lat = lat; posn_long = long } in
|
||||
try
|
||||
display_tile geomap wgs84
|
||||
with
|
||||
Gm.Not_available -> ()
|
||||
done
|
||||
done
|
||||
|
||||
@@ -0,0 +1,31 @@
|
||||
(*
|
||||
* $Id$
|
||||
*
|
||||
* Displaying Google Maps on a MapCanvas object
|
||||
*
|
||||
* Copyright (C) 2004-2006 ENAC, Pascal Brisset, Antoine Drouin
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*
|
||||
*)
|
||||
|
||||
val display_tile : MapCanvas.widget -> Latlong.geographic -> unit
|
||||
(** Displaying the Google Maps tile around the given point (zoom=1) *)
|
||||
|
||||
val fill_window : MapCanvas.widget -> unit
|
||||
(** Filling the canvas window with Google Maps tiles *)
|
||||
+34
-84
@@ -25,7 +25,8 @@
|
||||
*)
|
||||
|
||||
open Printf
|
||||
open Geometry_2d
|
||||
module G2d = Geometry_2d
|
||||
module LL = Latlong
|
||||
|
||||
module G = MapCanvas
|
||||
|
||||
@@ -47,15 +48,14 @@ let fixed_cam_targeted_yw = 500.0
|
||||
|
||||
(** variables used for handling cam moves: *)
|
||||
|
||||
let cam_half_aperture = m_pi /. 6.0
|
||||
let half_pi = m_pi /. 2.0
|
||||
let cam_half_aperture = LL.pi /. 6.0
|
||||
let half_pi = LL.pi /. 2.0
|
||||
let sqrt_2_div_2 = sqrt 2.0
|
||||
|
||||
|
||||
class track = fun ?(name="coucou") ?(size = 500) ?(color="red") (geomap:MapCanvas.widget) (vertical_display:MapCanvas.basic_widget) ->
|
||||
class track = fun ?(name="coucou") ?(size = 500) ?(color="red") (geomap:MapCanvas.widget) ->
|
||||
let group = GnoCanvas.group geomap#canvas#root in
|
||||
let v_group = GnoCanvas.group vertical_display#canvas#root in
|
||||
let empty = ({ G.east = 0.; north = 0. }, GnoCanvas.line group) in
|
||||
let empty = ({LL.posn_lat=0.; LL.posn_long=0.}, GnoCanvas.line group) in
|
||||
|
||||
let aircraft = GnoCanvas.group group
|
||||
and track = GnoCanvas.group group in
|
||||
@@ -82,24 +82,12 @@ class track = fun ?(name="coucou") ?(size = 500) ?(color="red") (geomap:MapCanva
|
||||
ignore ( GnoCanvas.ellipse ~x1: (-5.) ~y1: (-5.) ~x2: 5. ~y2: 5. ~fill_color:"red" ~props:[`WIDTH_UNITS 1.; `OUTLINE_COLOR "red"; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] mission_target ) in
|
||||
|
||||
(** data at map scale *)
|
||||
let max_cam_half_height_scaled = 10000.0 /. (geomap#get_world_unit ()) in
|
||||
let max_oblic_distance_scaled = 10000.0 /. (geomap#get_world_unit ()) in
|
||||
let min_distance_scaled = 10. /. (geomap#get_world_unit ()) in
|
||||
let min_height_scaled = 0.1 /. (geomap#get_world_unit ()) in
|
||||
let max_cam_half_height_scaled = 10000.0 in
|
||||
let max_oblic_distance_scaled = 10000.0 in
|
||||
let min_distance_scaled = 10. in
|
||||
let min_height_scaled = 0.1 in
|
||||
|
||||
|
||||
(** vertical display items *)
|
||||
|
||||
let vertical_group = GnoCanvas.group vertical_display#canvas#root in
|
||||
let vertical_aircraft = GnoCanvas.group vertical_group in
|
||||
let vertical_plot =
|
||||
ignore ( GnoCanvas.ellipse ~x1: (-. 5.0) ~y1: (-. 5.0) ~x2: 5.0 ~y2: 5.0 ~fill_color:color ~props:[`WIDTH_UNITS 1.; `OUTLINE_COLOR color; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] vertical_aircraft);
|
||||
(*** ignore (GnoCanvas.line ~fill_color:color ~props:[`WIDTH_PIXELS 2;`CAP_STYLE `ROUND] ~points:[|0.;0.;0.; -10.|] vertical_aircraft); ***)
|
||||
in
|
||||
let ac_v_label =
|
||||
GnoCanvas.text v_group ~props:[`TEXT name; `X 25.; `Y 25.; `ANCHOR `SW; `FILL_COLOR color]
|
||||
in
|
||||
|
||||
let top = ref 0
|
||||
and v_top = ref 0 in
|
||||
object (self)
|
||||
@@ -107,7 +95,6 @@ let ac_v_label =
|
||||
val mutable segments = Array.create size empty
|
||||
val mutable v_segments = Array.create size empty
|
||||
val mutable last = None
|
||||
val mutable v_last = None
|
||||
val mutable last_heading = 0.0
|
||||
val mutable last_altitude = 0.0
|
||||
val mutable last_speed = 0.0
|
||||
@@ -118,7 +105,6 @@ let ac_v_label =
|
||||
val mutable last_flight_time = 0.0
|
||||
val mutable last_x_val = 0.0
|
||||
val mutable cam_on = false
|
||||
val mutable vertical_time_axis_on = false
|
||||
val mutable params_on = false
|
||||
val mutable v_params_on = false
|
||||
val mutable desired_track = ((GnoCanvas.ellipse group) :> GnoCanvas.base_item)
|
||||
@@ -143,40 +129,31 @@ let ac_v_label =
|
||||
top := 0
|
||||
method set_cam_state = fun b -> cam_on <- b
|
||||
|
||||
(** switches time and longitude on the vertical display x axis.
|
||||
tracks are cleared *)
|
||||
method set_vertical_time_axis = fun b ->
|
||||
vertical_time_axis_on <- b;
|
||||
if vertical_time_axis_on then vertical_display#set_lbl_x_axis "x-axis: time"
|
||||
else vertical_display#set_lbl_x_axis "x-axis: longitude";
|
||||
self#clear v_segments v_top;
|
||||
v_last <- None
|
||||
|
||||
method update_ap_status = fun time ->
|
||||
last_flight_time <- time
|
||||
method set_params_state = fun b -> params_on <- b
|
||||
method set_v_params_state = fun b -> v_params_on <- b
|
||||
method set_last = fun x -> last <- x
|
||||
method set_v_last = fun x -> v_last <- x
|
||||
method last = last
|
||||
|
||||
(** add track points on map2D or vertical display, according to the
|
||||
(** add track points on map2D, according to the
|
||||
track parameter *)
|
||||
method add_point = fun en seg set_last_point last_point top track ->
|
||||
method add_point = fun geo seg set_last_point last_point top track ->
|
||||
self#clear_one (!top) seg ;
|
||||
begin
|
||||
match last_point with
|
||||
None ->
|
||||
seg.((!top)) <- (en, geomap#segment ~group:track ~fill_color:color en en)
|
||||
| Some pt ->
|
||||
seg.((!top)) <- (en, geomap#segment ~group:track ~width:2 ~fill_color:color pt en);
|
||||
seg.((!top)) <- (geo, geomap#segment ~group:track ~fill_color:color geo geo)
|
||||
| Some last_geo ->
|
||||
seg.((!top)) <- (geo, geomap#segment ~group:track ~width:2 ~fill_color:color last_geo geo);
|
||||
end;
|
||||
self#incr seg;
|
||||
set_last_point (Some en)
|
||||
(set_last_point (Some geo) : unit)
|
||||
|
||||
method clear_map2D = self#clear segments top
|
||||
|
||||
method move_icon = fun en heading altitude relief_height speed climb ->
|
||||
let (xw,yw) = geomap#world_of_en en in
|
||||
method move_icon = fun wgs84 heading altitude relief_height speed climb ->
|
||||
let (xw,yw) = geomap#world_of wgs84 in
|
||||
aircraft#affine_absolute (affine_pos_and_angle geomap#zoom_adj#value xw yw heading);
|
||||
last_heading <- heading;
|
||||
last_yw <- yw;
|
||||
@@ -188,37 +165,10 @@ let ac_v_label =
|
||||
ac_label#set [`TEXT ( name^" \n"^(string_of_float last_height)^" m\n"^(string_of_float last_speed)^" m/s\n" ); `Y 70. ] else
|
||||
ac_label#set [`TEXT name; `Y 25.];
|
||||
ac_label#affine_absolute (affine_pos_and_angle geomap#zoom_adj#value xw yw 0.);
|
||||
self#add_point wgs84 segments (self#set_last) last top group;
|
||||
|
||||
let y_val = ((vertical_display#get_vertical_max_level -. altitude) *. (vertical_display#get_vertical_factor) /. ( vertical_display#get_world_unit ()) ) in
|
||||
let x_val = if vertical_time_axis_on then
|
||||
last_flight_time /. ( vertical_display#get_world_unit () )
|
||||
else xw in
|
||||
vertical_aircraft#affine_absolute (affine_pos_and_angle vertical_display#zoom_adj#value x_val y_val 0.0 );
|
||||
|
||||
let v_en = { MapCanvas.east = x_val *. ( vertical_display#get_world_unit () ); MapCanvas.north = y_val *. ( -. vertical_display#get_world_unit () ) } in
|
||||
|
||||
|
||||
(** on the vertical_display,
|
||||
params displayed are different if we have time or longitude on x-axis *)
|
||||
|
||||
if vertical_time_axis_on then
|
||||
begin
|
||||
self#add_point v_en v_segments (self#set_v_last) v_last v_top v_group;
|
||||
if v_params_on then ac_v_label#set [`TEXT ( name^" \n alt: "^(string_of_float altitude)^" m\n"^" height: "^(string_of_float last_height)^" m\n flight_time: "^( sprintf"%.2f sec\n" last_flight_time)); `Y 70. ]
|
||||
else ac_v_label#set [`TEXT name; `Y 25.]
|
||||
end
|
||||
else
|
||||
begin
|
||||
if v_params_on then ac_v_label#set [`TEXT ( name^" \n alt: "^(string_of_float altitude)^" m\n"^" height: "^(string_of_float last_height)^" m\n long(utm_world): "^(sprintf"%.2f m\n" en.MapCanvas.east)); `Y 70. ]
|
||||
else ac_v_label#set [`TEXT name; `Y 25.]
|
||||
end;
|
||||
ac_v_label#affine_absolute (affine_pos_and_angle vertical_display#zoom_adj#value x_val y_val 0.);
|
||||
self#add_point en segments (self#set_last) last top group;
|
||||
last_altitude <- altitude;
|
||||
last_xw <- xw;
|
||||
last_x_val <- x_val;
|
||||
method move_carrot = fun en ->
|
||||
let (xw,yw) = geomap#world_of_en en in
|
||||
method move_carrot = fun wgs84 ->
|
||||
let (xw,yw) = geomap#world_of wgs84 in
|
||||
carrot#affine_absolute (affine_pos_and_angle geomap#zoom_adj#value xw yw 0.);
|
||||
|
||||
(** draws the circular path to be followed by the aircraft in circle mode *)
|
||||
@@ -232,30 +182,30 @@ let ac_v_label =
|
||||
desired_track <- ((geomap#segment ~fill_color:"green" en en2) :> GnoCanvas.base_item)
|
||||
|
||||
(** moves the rectangle representing the field covered by the camera *)
|
||||
method move_cam = fun en mission_target_en ->
|
||||
method move_cam = fun wgs84 mission_target_wgs84 ->
|
||||
if not cam_on then
|
||||
cam#hide ()
|
||||
else
|
||||
let (xw,yw) = geomap#world_of_en en in
|
||||
let (mission_target_xw, mission_target_yw) = geomap#world_of_en mission_target_en in
|
||||
let last_height_scaled = last_height /. (geomap#get_world_unit ()) in
|
||||
let (xw,yw) = geomap#world_of wgs84 in
|
||||
let (mission_target_xw, mission_target_yw) = geomap#world_of mission_target_wgs84 in
|
||||
let last_height_scaled = last_height in
|
||||
|
||||
(** all data are at map scale *)
|
||||
|
||||
begin
|
||||
let pt1 = { x2D = last_xw; y2D = last_yw} in
|
||||
let pt2 = { x2D = xw ; y2D = yw } in
|
||||
let pt1 = { G2d.x2D = last_xw; y2D = last_yw} in
|
||||
let pt2 = { G2d.x2D = xw ; y2D = yw } in
|
||||
|
||||
(** y axis is downwards so North vector is as follows: *)
|
||||
let vect_north = (vect_make { x2D = 0.0 ; y2D = 0.0 } { x2D = 0.0 ; y2D = -1.0 } ) in
|
||||
let d = distance pt1 pt2 in
|
||||
let vect_north = (G2d.vect_make { G2d.x2D = 0.0 ; y2D = 0.0 } { G2d.x2D = 0.0 ; y2D = -1.0 } ) in
|
||||
let d = G2d.distance pt1 pt2 in
|
||||
begin
|
||||
let cam_heading =
|
||||
if d > min_distance_scaled then
|
||||
let cam_vect_normalized = (vect_normalize (vect_make pt1 pt2)) in
|
||||
if (dot_product vect_north cam_vect_normalized) > 0.0 then
|
||||
norm_angle_360 ( rad2deg (asin (cross_product vect_north cam_vect_normalized)))
|
||||
else norm_angle_360 ( rad2deg (m_pi -. asin (cross_product vect_north cam_vect_normalized)))
|
||||
let cam_vect_normalized = (G2d.vect_normalize (G2d.vect_make pt1 pt2)) in
|
||||
if (G2d.dot_product vect_north cam_vect_normalized) > 0.0 then
|
||||
norm_angle_360 ( G2d.rad2deg (asin (G2d.cross_product vect_north cam_vect_normalized)))
|
||||
else norm_angle_360 ( G2d.rad2deg (G2d.m_pi -. asin (G2d.cross_product vect_north cam_vect_normalized)))
|
||||
else last_heading in
|
||||
let (angle_of_view, oblic_distance) =
|
||||
if last_height < min_height_scaled then
|
||||
|
||||
@@ -24,6 +24,7 @@
|
||||
*
|
||||
*)
|
||||
|
||||
module LL = Latlong
|
||||
open Printf
|
||||
|
||||
let s = 5.
|
||||
@@ -38,11 +39,11 @@ class group = fun ?(color="red") ?(editable=true) (geomap:MapCanvas.widget) ->
|
||||
method editable=editable
|
||||
end
|
||||
|
||||
class waypoint = fun (group:group) (name :string) ?(alt=0.) en ->
|
||||
class waypoint = fun (group:group) (name :string) ?(alt=0.) wgs84 ->
|
||||
let geomap=group#geomap
|
||||
and color = group#color
|
||||
and editable = group#editable in
|
||||
let xw, yw = geomap#world_of_en en in
|
||||
let xw, yw = geomap#world_of wgs84 in
|
||||
object (self)
|
||||
val mutable x0 = 0.
|
||||
val mutable y0 = 0.
|
||||
@@ -66,10 +67,10 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) en ->
|
||||
method edit =
|
||||
let dialog = GWindow.window ~border_width:10 ~title:"Waypoint Edit" () in
|
||||
let dvbx = GPack.box `VERTICAL ~packing:dialog#add () in
|
||||
let en = self#en in
|
||||
let wgs84 = self#pos in
|
||||
let s = sprintf "WGS84 %s" (geomap#geo_string wgs84) in
|
||||
let ename = GEdit.entry ~text:name ~packing:dvbx#add () in
|
||||
let ex = GEdit.entry ~text:(string_of_float en.MapCanvas.east) ~packing:dvbx#add () in
|
||||
let ey = GEdit.entry ~text:(string_of_float en.MapCanvas.north) ~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 cancel = GButton.button ~label:"Cancel" ~packing: dvbx#add () in
|
||||
let ok = GButton.button ~label:"OK" ~packing: dvbx#add () in
|
||||
@@ -79,8 +80,7 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) en ->
|
||||
self#set_name ename#text;
|
||||
alt <- float_of_string ea#text;
|
||||
label#set [`TEXT name];
|
||||
self#set {MapCanvas.east = float_of_string ex#text;
|
||||
north = float_of_string ey#text};
|
||||
self#set (LL.of_string e_pos#text);
|
||||
dialog#destroy ()
|
||||
end);
|
||||
dialog#show ()
|
||||
@@ -127,11 +127,9 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) en ->
|
||||
initializer ignore(if editable then ignore (item#connect#event self#event))
|
||||
method moved = moved
|
||||
method item = item
|
||||
method en =
|
||||
let (dx, dy) = self#xy in
|
||||
geomap#en_of_world dx dy
|
||||
method set en =
|
||||
let (xw, yw) = geomap#world_of_en en
|
||||
method pos = geomap#of_world self#xy
|
||||
method set wgs84 =
|
||||
let (xw, yw) = geomap#world_of wgs84
|
||||
and (xw0, yw0) = self#xy in
|
||||
self#move (xw-.xw0) (yw-.yw0)
|
||||
method delete =
|
||||
|
||||
@@ -39,18 +39,18 @@ class waypoint :
|
||||
group ->
|
||||
string ->
|
||||
?alt:float ->
|
||||
MapCanvas.en ->
|
||||
Latlong.geographic ->
|
||||
object
|
||||
method alt : float
|
||||
method delete : unit
|
||||
method edit : unit
|
||||
method en : MapCanvas.en
|
||||
method pos : Latlong.geographic
|
||||
method event : GnoCanvas.item_event -> bool
|
||||
method item : GnoCanvas.polygon
|
||||
method label : GnoCanvas.text
|
||||
method move : float -> float -> unit
|
||||
method name : string
|
||||
method set : MapCanvas.en -> unit
|
||||
method set : Latlong.geographic -> unit
|
||||
method set_name : string -> unit
|
||||
method xy : float * float
|
||||
method zoom : float -> unit
|
||||
@@ -58,4 +58,4 @@ class waypoint :
|
||||
end
|
||||
|
||||
|
||||
val waypoint : group -> ?name:string -> ?alt:float -> MapCanvas.en -> waypoint
|
||||
val waypoint : group -> ?name:string -> ?alt:float -> Latlong.geographic -> waypoint
|
||||
|
||||
+8
-7
@@ -61,21 +61,22 @@ end
|
||||
let (//) = Filename.concat
|
||||
|
||||
let ubx_xml =
|
||||
Xml.parse_file (Env.paparazzi_src // "conf" // "ubx.xml")
|
||||
lazy (Xml.parse_file (Env.paparazzi_src // "conf" // "ubx.xml"))
|
||||
|
||||
let ubx_get_class = fun name ->
|
||||
let ubx_xml = Lazy.force ubx_xml in
|
||||
ExtXml.child ubx_xml ~select:(fun x -> ExtXml.attrib x "name" = name) "class"
|
||||
|
||||
let ubx_nav = ubx_get_class "NAV"
|
||||
let ubx_nav_id = int_of_string (ExtXml.attrib ubx_nav "ID")
|
||||
let ubx_nav () = ubx_get_class "NAV"
|
||||
let ubx_nav_id () = int_of_string (ExtXml.attrib (ubx_nav ()) "ID")
|
||||
let ubx_get_msg = fun ubx_class name ->
|
||||
ExtXml.child ubx_class ~select:(fun x -> ExtXml.attrib x "name" = name) "message"
|
||||
|
||||
let ubx_get_nav_msg = fun name -> ubx_get_msg ubx_nav name
|
||||
let ubx_get_nav_msg = fun name -> ubx_get_msg (ubx_nav ()) name
|
||||
|
||||
let nav_posutm = ubx_nav_id, ubx_get_nav_msg "POSUTM"
|
||||
let nav_status = ubx_nav_id, ubx_get_nav_msg "STATUS"
|
||||
let nav_velned = ubx_nav_id, ubx_get_nav_msg "VELNED"
|
||||
let nav_posutm () = ubx_nav_id (), ubx_get_nav_msg "POSUTM"
|
||||
let nav_status () = ubx_nav_id (), ubx_get_nav_msg "STATUS"
|
||||
let nav_velned () = ubx_nav_id (), ubx_get_nav_msg "VELNED"
|
||||
|
||||
|
||||
let send_start_sequence = fun gps ->
|
||||
|
||||
@@ -34,7 +34,7 @@ module Protocol :
|
||||
val checksum : string -> int -> string -> bool
|
||||
end
|
||||
|
||||
val nav_posutm : int * Xml.xml
|
||||
val nav_status : int * Xml.xml
|
||||
val nav_velned : int * Xml.xml
|
||||
val nav_posutm : unit -> int * Xml.xml
|
||||
val nav_status : unit -> int * Xml.xml
|
||||
val nav_velned : unit -> int * Xml.xml
|
||||
val send : out_channel -> int * Xml.xml -> (string * int) list -> unit
|
||||
|
||||
@@ -24,6 +24,7 @@
|
||||
*
|
||||
*)
|
||||
|
||||
|
||||
type cmd_name =
|
||||
ACK
|
||||
| NAK
|
||||
@@ -60,18 +61,32 @@ type cmd_name =
|
||||
| REQ_SEND_SERVICE
|
||||
| RES_SEND_SERVICE
|
||||
| SERVICE_RESPONSE
|
||||
(** available commands *)
|
||||
|
||||
|
||||
val code_of_cmd : cmd_name -> int
|
||||
(** Code of command *)
|
||||
|
||||
type data = string
|
||||
type cmd = cmd_name * data
|
||||
|
||||
val send : Unix.file_descr -> cmd -> unit
|
||||
|
||||
val receive : ?ack:(unit -> unit) -> (cmd -> 'a) -> (Unix.file_descr -> unit)
|
||||
|
||||
val code_of_cmd : cmd_name -> int
|
||||
(** A command is composed of a command name and some untyped data *)
|
||||
|
||||
type addr
|
||||
val addr_of_string : string -> addr
|
||||
(** [addr_of_string address] where [address] is a 64 bits number, for example
|
||||
[0x011804c0012d] *)
|
||||
|
||||
val send : Unix.file_descr -> cmd -> unit
|
||||
(** Send a command on the channel connected to the serial port of the wavecard *)
|
||||
|
||||
val send_addressed : Unix.file_descr -> (cmd_name*addr*data) -> unit
|
||||
(** [send_addressed fd (cmd, a, data)] Sends [cmd] with data obtained by
|
||||
concatenation of codinf of [a] and [data] *)
|
||||
|
||||
val receive : ?ack:(unit -> unit) -> (cmd -> 'a) -> (Unix.file_descr -> unit)
|
||||
(** [receive ?acknowledger callbkack] Returns a listener for wavecard messages *)
|
||||
|
||||
|
||||
val compute_checksum : string -> int
|
||||
(** [compute_checksum buf] Computes the checksum of a complete message buffer,
|
||||
including the header of the message *)
|
||||
|
||||
@@ -41,8 +41,8 @@ sub populate {
|
||||
|
||||
roll => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
pitch => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
east => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
north => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
lat => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
long => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
speed => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
course => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
alt => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
@@ -62,13 +62,13 @@ sub populate {
|
||||
flight_time => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
block_time => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
stage_time => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
target_east => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
target_north => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
target_lat => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
target_long => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
|
||||
cam_east => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
cam_north => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
target_east => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
target_north => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
cam_lat => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
cam_long => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
cam_target_lat => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
cam_target_long => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],
|
||||
|
||||
-engine_status => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, undef],
|
||||
-svsinfo => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, undef],
|
||||
|
||||
+10
-6
@@ -143,12 +143,14 @@ module Make(A:Data.MISSION) = struct
|
||||
let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
|
||||
if ac_id <> !my_id then
|
||||
let f = fun a -> Pprz.float_assoc a vs in
|
||||
let ux = f "east"
|
||||
and uy = f "north"
|
||||
let lat = f "lat"
|
||||
and long = f "long"
|
||||
and course = (Deg>>Rad)(f "course")
|
||||
and alt = f "alt"
|
||||
and gspeed = f "speed" in
|
||||
set_ac_info ac_id ux uy course alt gspeed
|
||||
let wgs84 = {posn_lat=(Deg>>Rad)lat;posn_long=(Deg>>Rad)long} in
|
||||
let utm = Latlong.utm_of WGS84 wgs84 in
|
||||
set_ac_info ac_id utm.utm_x utm.utm_y course alt gspeed
|
||||
|
||||
external move_waypoint : int -> float -> float -> float -> unit = "move_waypoint"
|
||||
let get_move_waypoint = fun _sender vs ->
|
||||
@@ -156,10 +158,12 @@ module Make(A:Data.MISSION) = struct
|
||||
if ac_id = !my_id then
|
||||
let f = fun a -> Pprz.float_assoc a vs in
|
||||
let wp_id = Pprz.int_assoc "wp_id" vs
|
||||
and ux = f "utm_east"
|
||||
and uy = f "utm_north"
|
||||
and lat = f "lat"
|
||||
and long = f "long"
|
||||
and alt = f "alt" in
|
||||
move_waypoint wp_id ux uy alt
|
||||
let wgs84 = {posn_lat=(Deg>>Rad)lat;posn_long=(Deg>>Rad)long} in
|
||||
let utm = Latlong.utm_of WGS84 wgs84 in
|
||||
move_waypoint wp_id utm.utm_x utm.utm_y alt
|
||||
|
||||
external send_event : int -> unit = "send_event"
|
||||
let get_send_event = fun _sender vs ->
|
||||
|
||||
Reference in New Issue
Block a user