Google maps display

sw/ground_segment/cockpit/map2d.ml

@@ -106,12 +106,24 @@ let load_map = fun (geomap:G.widget) (vertical_display:MapCanvas.basic_widget) x
   geomap#moveto en0
 
 
-let file_of_url = fun url ->
+let set_geo_ref = fun geomap wgs84 ->
+  let utm_ref = utm_of WGS84 wgs84 in
+  let wgs84_of_en = fun en ->
+    of_utm WGS84 {utm_x = utm_ref.utm_x +. en.G.east; utm_y = utm_ref.utm_y +. en.G.north; utm_zone = utm_ref.utm_zone} in
+
+  geomap#set_wgs84_of_en wgs84_of_en;
+  geomap#set_world_unit 1.;
+  assert (!map_ref = None);
+  map_ref := Some utm_ref
+
+
+
+let file_of_url = fun ?(extension=".xml") url ->
   if String.sub url 0 7 = "file://" then
     String.sub url 7 (String.length url - 7)
   else
-    let tmp_file = Filename.temp_file "fp" ".xml" in
-    let c = sprintf "wget --cache=off -O %s %s" tmp_file url in
+    let tmp_file = Filename.temp_file "fp" extension in
+    let c = sprintf "wget --cache=off -O %s '%s'" tmp_file url in
     if Sys.command c = 0 then
       tmp_file
     else
@@ -467,13 +479,56 @@ let listen_flight_params = fun () ->
   in
   ignore (Ground_Pprz.message_bind "AP_STATUS" get_ap_status);;
 
+let en_of_wgs84 = fun geomap wgs84 ->
+  let ref = geomap#wgs84_of_en {G.east=0.; north = 0.} in
+  let utm_ref = utm_of WGS84 ref in
+
+  let utm = utm_of WGS84 wgs84 in
+  {G.east=utm.utm_x -. utm_ref.utm_x; north=utm.utm_y -. utm_ref.utm_y}
+
+
+
+let rec get_gm_tile = fun wgs84 zoom ->
+  if zoom < 10 then
+    try
+      let (gm_string, sw, scale) = Latlong.gm_tile_string wgs84 zoom in
+      let url = sprintf "http://kh1.google.com/kh?n=404&v=3&t=%s" gm_string in
+      let jpg_file = file_of_url ~extension:".jpg" url in
+
+      (jpg_file, sw, scale)
+    with (** Error, let's try a lower zoom *)
+      _ -> get_gm_tile wgs84 (zoom+1)
+  else
+    failwith "get_gm_tile"
+
+
+
+let button_press = fun (geomap:MapCanvas.widget) ev ->
+  let xc = GdkEvent.Button.x ev 
+  and yc = GdkEvent.Button.y ev in
+  let (xw, yw) = geomap#window_to_world xc yc in
+  let en = geomap#en_of_world xw yw in
+  let wgs84 = geomap#wgs84_of_en en in
+
+  let (jpg_file, sw, scale) = get_gm_tile wgs84 1 in
+
+  let en_sw = en_of_wgs84 geomap sw in
+  let en_nw = { en_sw with G.north = en_sw.G.north +. 256. *. scale } in
+  let scale = scale /. geomap#get_world_unit () in
+  let map = geomap#display_map ~scale en_nw (GdkPixbuf.from_file jpg_file) in
+  map#raise 1;
+  true
+  
+
 
 let _ =
   let ivy_bus = ref "127.255.255.255:2010"
+  and geo_ref = ref ""
   and map_file = ref ""
   and mission_file = ref "" in
   let options =
     [ "-b", Arg.String (fun x -> ivy_bus := x), "Bus\tDefault is 127.255.255.25:2010";
+      "-ref", Arg.Set_string geo_ref, "Geographic ref (default '')";
       "-m", Arg.String (fun x -> map_file := x), "Map description file"] in
   Arg.parse (options)
     (fun x -> Printf.fprintf stderr "Warning: Don't do anythig with %s\n" x)
@@ -495,6 +550,8 @@ let _ =
   let geomap = new MapCanvas.widget ~height:400 () in
   let accel_group = geomap#menu_fact#accel_group in
 
+  ignore (geomap#canvas#event#connect#button_press (button_press geomap));
+
   (** widget displaying aircraft vertical position  *)
   let vertical_display = new MapCanvas.basic_widget ~height:400 () in
   let ac_vertical_fact = new GMenu.factory vertical_display#file_menu in
@@ -518,8 +575,10 @@ let _ =
   vertical_vbox#pack ~expand:true vertical_display#frame#coerce;
 
   (* Loading an initial map *)
-  if !map_file <> "" then begin
-    let xml_map_file = Filename.concat default_path_maps !map_file in
+  if !geo_ref <> "" then
+    set_geo_ref geomap (Latlong.of_string !geo_ref)
+  else if !map_file <> "" then begin
+    let xml_map_file = if !map_file.[0] <> '/' then Filename.concat default_path_maps !map_file else !map_file in
     load_map geomap vertical_display xml_map_file
   end;
 

sw/lib/ocaml/latlong.ml

@@ -57,7 +57,7 @@ let (>>) u1 u2 x = (x *. piradian u2) /. piradian u1;;
 let deg_string_of_rad = fun r -> Printf.sprintf "%.6f" ((Rad>>Deg)r)
 
 let sprint_degree_of_radian x =
-  Printf.sprintf "%.4f" ((Rad>>Deg) x)
+  Printf.sprintf "%.6f" ((Rad>>Deg) x)
 
 let string_degrees_of_geographic sm =
   Printf.sprintf "%s\t%s"
@@ -353,3 +353,97 @@ let of_string = fun s ->
   | ["LBT2e";x;y] ->
       wgs84_of_lambertIIe (ios x) (ios y)
   | _ -> invalid_arg (Printf.sprintf "Latlong.of_string: %s" s)
+
+
+
+let (/.=) r x = r := !r /. x
+let (+.=) r x = r := !r +. x
+let (-.=) r x = r := !r -. x
+
+(** Returns a keyhole string for a longitude (x), latitude (y), and zoom 
+   for Google Maps (http://www.ponies.me.uk/maps/GoogleTileUtils.java) *)
+let gm_tile_string = fun wgs84 zoom ->
+  let zoom = 18 - zoom in
+  
+  (* first convert the lat lon to transverse mercator coordintes.*)
+  let lon = (Rad>>Deg)wgs84.posn_long in
+  let lon = if lon > 180. then lon -. 180. else lon in
+  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 tLat = ref (-1.)
+  and tLon      = ref (-1.)
+  and lonWidth  = ref 2.
+  and latHeight = ref 2. (** Always identical to lonWidth !!! *)
+  and keyholeString = Buffer.create 3 in
+  Buffer.add_char keyholeString 't';
+
+  for i = 0 to zoom - 1 do
+    lonWidth /.= 2.;
+    latHeight /.= 2.;
+
+    if !tLat +. !latHeight > lat then
+      if ((!tLon +. !lonWidth) > lon) then begin
+        Buffer.add_char keyholeString 't';
+      end else begin
+        tLon +.= !lonWidth;
+        Buffer.add_char keyholeString 's';
+      end
+    else begin
+      tLat +.= !latHeight;
+
+      if ((!tLon +. !lonWidth) > lon) then begin
+        Buffer.add_char keyholeString 'q';
+      end
+      else begin  
+        tLon +.= !lonWidth;
+        Buffer.add_char keyholeString 'r';
+      end
+    end
+  done;
+  let tmp_lat = fun l -> 2. *. atan (exp (l *. pi)) -. pi/.2. in
+  let bl_lat = tmp_lat !tLat in
+  let tr_lat = tmp_lat (!tLat +. !latHeight) in
+  let bottom_left = {posn_lat = bl_lat ; posn_long = !tLon *. pi} in
+  let top_right = {posn_lat = tr_lat;
+		   posn_long = bottom_left.posn_long +. !lonWidth *. pi } in
+
+  let utm_bottom_left = utm_of WGS84 bottom_left
+  and utm_top_right = utm_of WGS84 top_right in
+  Printf.fprintf stderr "%fx%f\n" (utm_bottom_left.utm_x -. utm_top_right.utm_x) (utm_bottom_left.utm_y -. utm_top_right.utm_y);
+  let scale = utm_distance utm_bottom_left utm_top_right  /. sqrt (2. *. 256.*.256.) in
+  (Buffer.contents keyholeString, bottom_left, scale)
+
+
+let gm_lat_long_of_tile = fun keyholeStr ->
+  assert(keyholeStr.[0] = 't');
+  
+  let lon  = ref (-180.)
+  and lonWidth = ref 360. 
+  and  lat       = ref (-1.) 
+  and latHeight = ref 2. in
+  
+  for i = 1 to String.length keyholeStr - 1  do
+    lonWidth /.= 2.;
+    latHeight /.= 2.;
+
+    match keyholeStr.[i] with
+      's' -> lon +.= !lonWidth
+    | 'r' -> 
+        lat +.= !latHeight;
+        lon +.= !lonWidth
+    | 'q' -> lat +.= !latHeight
+    | 't' -> ()
+    | _ -> invalid_arg ("gm_get_lat_long " ^ keyholeStr)
+  done;
+
+  latHeight +.= !lat;
+  latHeight := (2. *. atan (exp (pi *. !latHeight))) -. (pi /. 2.);
+
+  lat := (2. *. atan (exp (pi *. !lat))) -. (pi /. 2.);
+
+  latHeight -.= !lat;
+  lon := (Deg>>Rad)!lon;
+  { posn_lat = !lat; posn_long = !lon }

sw/lib/ocaml/latlong.mli

@@ -114,3 +114,11 @@ 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 gm_tile_string : geographic -> int -> string * geographic * float
+(** [gm_tile_string geo zoom] Returns the string code designing the
+   Google Map tile *)
+
+val gm_lat_long_of_tile : string -> geographic
+(** [gm_lat_long_of_tile google_maps_tile_key] Returns the coordinates of the
+South West corner of the given tile. *)

sw/lib/ocaml/mapCanvas.ml

@@ -141,6 +141,11 @@ class basic_widget = fun ?(height=800) ?width ?wgs84_of_en () ->
 	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 moveto = fun en ->
       let (xw, yw) = self#world_of_en en in