diff --git a/conf/messages.xml b/conf/messages.xml index 0b427d0278..e607603a7a 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -324,8 +324,8 @@ - - + + @@ -348,8 +348,8 @@ - - + + @@ -357,10 +357,10 @@ - - - - + + + + @@ -417,24 +417,24 @@ - - + + - - - - + + + + - - + + diff --git a/data/maps/muret_UTM.xml b/data/maps/muret_UTM.xml index af0a0f39a9..296fd93ec1 100644 --- a/data/maps/muret_UTM.xml +++ b/data/maps/muret_UTM.xml @@ -1,5 +1,5 @@ - - - + + + diff --git a/sw/ground_segment/cockpit/Makefile b/sw/ground_segment/cockpit/Makefile index d826363b57..f2072db372 100644 --- a/sw/ground_segment/cockpit/Makefile +++ b/sw/ground_segment/cockpit/Makefile @@ -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 diff --git a/sw/ground_segment/cockpit/map2d.ml b/sw/ground_segment/cockpit/map2d.ml index 3697ebb42a..623ec28211 100644 --- a/sw/ground_segment/cockpit/map2d.ml +++ b/sw/ground_segment/cockpit/map2d.ml @@ -29,6 +29,9 @@ open Latlong module Ground_Pprz = Pprz.Protocol(struct let name = "ground" end) +let float_attr = fun xml a -> float_of_string (ExtXml.attrib xml a) +let int_attr = fun xml a -> int_of_string (ExtXml.attrib xml a) + type color = string let fos = float_of_string @@ -69,87 +72,70 @@ type aircraft = { let live_aircrafts = Hashtbl.create 3 -let map_ref = ref None +let set_georef_if_none = fun geomap wgs84 -> + match geomap#georef with + None -> geomap#set_georef wgs84 + | Some _ -> () + -let float_attr = fun xml a -> float_of_string (ExtXml.attrib xml a) -let load_map = fun (geomap:G.widget) (vertical_display:MapCanvas.basic_widget) xml_map -> +let load_map = fun (geomap:G.widget) xml_map -> let dir = Filename.dirname xml_map in let xml_map = Xml.parse_file xml_map in let image = dir // ExtXml.attrib xml_map "file" and scale = float_attr xml_map "scale" - and utm_zone = - try int_of_string (Xml.attrib xml_map "utm_zone") with - _ -> 31 in - geomap#set_world_unit scale; - approx_ground_altitude := ( try (float_attr xml_map "approx_ground_altitude") - with _ -> 0.0); - vertical_display#set_world_unit scale; - let one_ref = ExtXml.child xml_map "point" in - let x = float_attr one_ref "x" and y = float_attr one_ref "y" - and utm_x = float_attr one_ref "utm_x" and utm_y = float_attr one_ref "utm_y" in - let utm_x0 = utm_x -. x *. scale - and utm_y0 = utm_y +. y *. scale in + and utm_zone = int_attr xml_map "utm_zone" in + assert (ExtXml.attrib xml_map "projection" = "UTM"); - let utm_ref = - match !map_ref with - None -> - let utm0 = {utm_x = utm_x0; utm_y = utm_y0; utm_zone = utm_zone } in - map_ref := Some utm0; - utm0 - | Some utm -> - assert (utm_zone = utm.utm_zone); - utm 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_zone} in - - geomap#set_wgs84_of_en wgs84_of_en; - let en0 = {G.east=utm_x0 -. utm_ref.utm_x; north=utm_y0 -. utm_ref.utm_y} in - ignore (geomap#display_map en0 (GdkPixbuf.from_file image)); - geomap#moveto en0 + match Xml.children xml_map with + p1::p2::_ -> + let x1 = truncate (float_attr p1 "x") + and y1 = truncate (float_attr p1 "y") + and x2 = truncate (float_attr p2 "x") + and y2 = truncate (float_attr p2 "y") + and utm_x1 = float_attr p1 "utm_x" + and utm_y1 = float_attr p1 "utm_y" + and utm_x2 = float_attr p2 "utm_x" + and utm_y2 = float_attr p2 "utm_y" in + + let utm1 = {utm_x = utm_x1; utm_y = utm_y1; utm_zone = utm_zone } + and utm2 = {utm_x = utm_x2; utm_y = utm_y2; utm_zone = utm_zone } in + let geo1 = Latlong.of_utm WGS84 utm1 + and geo2 = Latlong.of_utm WGS84 utm2 in + + (* Take this point as a reference for the display if none currently *) + set_georef_if_none geomap geo1; + + ignore (geomap#display_pixbuf ((x1,y1),geo1) ((x2,y2),geo2) (GdkPixbuf.from_file image)); + geomap#moveto geo1 + | _ -> failwith (sprintf "load_map: two ref points required") -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 load_mission = fun color geomap xml -> let lat0 = float_attr xml "lat0" and lon0 = float_attr xml "lon0" and alt0 = float_attr xml "alt" in - let utm0 = utm_of WGS84 {posn_lat = (Deg>>Rad)lat0; posn_long = (Deg>>Rad)lon0 } in + let ref_wgs84 = {posn_lat = (Deg>>Rad)lat0; posn_long = (Deg>>Rad)lon0 } in + let utm0 = utm_of WGS84 ref_wgs84 in let waypoints = ExtXml.child xml "waypoints" in let max_dist_from_home = float_attr xml "MAX_DIST_FROM_HOME" in - let utm_ref = - match !map_ref with - None -> - map_ref := Some utm0; - utm0 - | Some utm -> - assert (utm0.utm_zone = utm.utm_zone); - utm in - let en_of_xy = fun x y -> - {G.east = x +. utm0.utm_x -. utm_ref.utm_x; - G.north = y +. utm0.utm_y -. utm_ref.utm_y } in + set_georef_if_none geomap ref_wgs84; + + let wgs84_of_xy = fun x y -> + Latlong.of_utm WGS84 (utm_add utm0 (x, y) ) in let fp = new MapWaypoints.group ~color ~editable:true geomap in let i = ref 0 in let wpts = List.map (fun wp -> - let en = en_of_xy (float_attr wp "x") (float_attr wp "y") in + let wgs84 = wgs84_of_xy (float_attr wp "x") (float_attr wp "y") in let alt = try float_attr wp "alt" with _ -> alt0 in - let w = MapWaypoints.waypoint fp ~name:(ExtXml.attrib wp "name") ~alt en in + let w = MapWaypoints.waypoint fp ~name:(ExtXml.attrib wp "name") ~alt wgs84 in if ExtXml.attrib wp "name" = "HOME" then - ignore (geomap#circle ~color en max_dist_from_home); + ignore (geomap#circle ~color wgs84 max_dist_from_home); incr i; !i, w ) @@ -157,55 +143,25 @@ let load_mission = fun color geomap xml -> fp, wpts -let aircraft_pos_msg = fun track utm_x_ utm_y_ heading altitude speed climb -> - match !map_ref with - None -> () - | Some utm0 -> - let en = {G.east = utm_x_ -. utm0.utm_x; north = utm_y_ -. utm0.utm_y } in - let h = - try - Srtm.of_utm { utm_zone = utm0.utm_zone; utm_x = utm_x_; utm_y = utm_y_} - with - _ -> truncate altitude - in - track#move_icon en heading altitude (float_of_int h) speed climb +let aircraft_pos_msg = fun track wgs84 heading altitude speed climb -> + let h = + try + Srtm.of_wgs84 wgs84 + with + _ -> truncate altitude in + track#move_icon wgs84 heading altitude (float_of_int h) speed climb -let carrot_pos_msg = fun track utm_x utm_y -> - match !map_ref with - None -> () - | Some utm0 -> - let en = {G.east = utm_x -. utm0.utm_x; north = utm_y -. utm0.utm_y } in - track#move_carrot en +let carrot_pos_msg = fun track wgs84 -> + track#move_carrot wgs84 -let cam_pos_msg = fun track utm_x utm_y target_utm_x target_utm_y -> - match !map_ref with - None -> () - | Some utm0 -> - let en = {G.east = utm_x -. utm0.utm_x; north = utm_y -. utm0.utm_y } in - let target_en = {G.east = target_utm_x -. utm0.utm_x; north = target_utm_y -. utm0.utm_y } in - track#move_cam en target_en +let cam_pos_msg = fun track wgs84 target_wgs84 -> + track#move_cam wgs84 target_wgs84 -let circle_status_msg = fun track utm_x utm_y radius -> - match !map_ref with - None -> () - | Some utm0 -> - let en = {G.east = utm_x -. utm0.utm_x; north = utm_y -. utm0.utm_y } in - track#draw_circle en radius +let circle_status_msg = fun track wgs84 radius -> + track#draw_circle wgs84 radius -let segment_status_msg = fun track utm_x utm_y utm2_x utm2_y -> - match !map_ref with - None -> () - | Some utm0 -> - let en = {G.east = utm_x -. utm0.utm_x; north = utm_y -. utm0.utm_y } in - let en2 = {G.east = utm2_x -. utm0.utm_x; north = utm2_y -. utm0.utm_y } in - track#draw_segment en en2 - -let circle_status_msg = fun track utm_x utm_y radius -> - match !map_ref with - None -> () - | Some utm0 -> - let en = {G.east = utm_x -. utm0.utm_x; north = utm_y -. utm0.utm_y } in - track#draw_circle en radius +let segment_status_msg = fun track geo1 geo2 -> + track#draw_segment geo1 geo2 let ap_status_msg = fun track flight_time -> track#update_ap_status flight_time @@ -232,16 +188,16 @@ let show_mission = fun geomap ac on_off -> let commit_changes = fun ac -> let a = Hashtbl.find live_aircrafts ac in - match a.fp_group, !map_ref with - Some (g, wpts), Some utm0 -> + match a.fp_group with + Some (g, wpts) -> List.iter (fun (i, w) -> if w#moved then - let {MapCanvas.east=e; MapCanvas.north=n} =w#en in + let wgs84 = w#pos in let vs = ["ac_id", Pprz.String ac; "wp_id", Pprz.Int i; - "utm_east", Pprz.Float (utm0.utm_x+.e); - "utm_north", Pprz.Float (utm0.utm_y+.n); + "lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat); + "long", Pprz.Float ((Rad>>Deg)wgs84.posn_long); "alt", Pprz.Float w#alt ] in Ground_Pprz.message_send "map2d" "MOVE_WAYPOINT" vs) @@ -337,6 +293,13 @@ let dl_settings = fun ac_id xml -> end; window +let center = fun geomap track () -> + match track#last with + None -> () + | Some geo -> + geomap#center geo; + geomap#canvas#update_now () + let active_dl_settings = fun ac_id x -> let ac = Hashtbl.find live_aircrafts ac_id in @@ -344,7 +307,7 @@ let active_dl_settings = fun ac_id x -> if x then w#show () else w#misc#hide ();; -let create_ac = fun (geomap:MapCanvas.widget) (vertical_display:MapCanvas.basic_widget) ac_id config -> +let create_ac = fun (geomap:MapCanvas.widget) ac_id config -> let color = Pprz.string_assoc "default_gui_color" config and name = Pprz.string_assoc "ac_name" config in let ac_menu = geomap#factory#add_submenu name in @@ -353,7 +316,9 @@ let create_ac = fun (geomap:MapCanvas.widget) (vertical_display:MapCanvas.basic_ ignore (fp#connect#toggled (fun () -> show_mission geomap ac_id fp#active)); ignore (ac_menu_fact#add_check_item "Datalink Settings" ~callback:(active_dl_settings ac_id)); - let track = new MapTrack.track ~name ~color:color geomap vertical_display in + let track = new MapTrack.track ~name ~color:color geomap in + + ignore (ac_menu_fact#add_item "Center A/C" ~callback:(center geomap track)); let eb = GBin.event_box ~width:10 ~height:10 () in eb#coerce#misc#modify_bg [`NORMAL, `NAME color]; @@ -365,12 +330,8 @@ let create_ac = fun (geomap:MapCanvas.widget) (vertical_display:MapCanvas.basic_ ignore (ac_menu_fact#add_item "Event 2" ~callback:(fun () -> send_event ac_id 2)); let cam = ac_menu_fact#add_check_item "Cam Display" ~active:false in ignore (cam#connect#toggled (fun () -> track#set_cam_state cam#active)); - let ac_menu_vertical = vertical_display#factory#add_submenu ac_id in - let ac_menu_fact_vertical = new GMenu.factory ac_menu_vertical in let params = ac_menu_fact#add_check_item "flight param. display" ~active:false in ignore (params#connect#toggled (fun () -> track#set_params_state params#active)); - let v_params = ac_menu_fact_vertical#add_check_item "flight param. display" ~active:false in - ignore (v_params#connect#toggled (fun () -> track#set_v_params_state v_params#active)); let event_ac = fun e -> match e with `BUTTON_PRESS _ | `BUTTON_RELEASE _ -> @@ -390,24 +351,24 @@ let create_ac = fun (geomap:MapCanvas.widget) (vertical_display:MapCanvas.basic_ -let ask_config = fun geomap vd ac -> +let ask_config = fun geomap ac -> let get_config = fun _sender values -> - create_ac geomap vd ac values + create_ac geomap ac values in Ground_Pprz.message_req "map2d" "CONFIG" ["ac_id", Pprz.String ac] get_config -let one_new_ac = fun (geomap:MapCanvas.widget)(vertical_display:MapCanvas.basic_widget) ac -> +let one_new_ac = fun (geomap:MapCanvas.widget) ac -> if not (Hashtbl.mem live_aircrafts ac) then begin - ask_config geomap vertical_display ac + ask_config geomap ac end -let live_aircrafts_msg = fun (geomap:MapCanvas.widget) (vertical_display:MapCanvas.basic_widget) acs -> +let live_aircrafts_msg = fun (geomap:MapCanvas.widget) acs -> let acs = Pprz.string_assoc "ac_list" acs in let acs = Str.split list_separator acs in - List.iter (one_new_ac geomap vertical_display) acs + List.iter (one_new_ac geomap) acs let listen_flight_params = fun () -> @@ -416,7 +377,8 @@ let listen_flight_params = fun () -> try let ac = Hashtbl.find live_aircrafts ac_id in let a = fun s -> Pprz.float_assoc s vs in - aircraft_pos_msg ac.track (a "east") (a "north") (a "course") (a "alt") (a "speed") (a "climb") + let wgs84 = { posn_lat = (Deg>>Rad)(a "lat"); posn_long = (Deg>>Rad)(a "long") } in + aircraft_pos_msg ac.track wgs84 (a "course") (a "alt") (a "speed") (a "climb") with Not_found -> () in ignore (Ground_Pprz.message_bind "FLIGHT_PARAM" get_fp); @@ -426,7 +388,8 @@ let listen_flight_params = fun () -> try let ac = Hashtbl.find live_aircrafts ac_id in let a = fun s -> Pprz.float_assoc s vs in - carrot_pos_msg ac.track (a "target_east") (a "target_north") + let wgs84 = { posn_lat = (Deg>>Rad)(a "target_lat"); posn_long = (Deg>>Rad)(a "target_long") } in + carrot_pos_msg ac.track wgs84 with Not_found -> () in ignore (Ground_Pprz.message_bind "NAV_STATUS" get_ns); @@ -436,7 +399,10 @@ let listen_flight_params = fun () -> try let ac = Hashtbl.find live_aircrafts ac_id in let a = fun s -> Pprz.float_assoc s vs in - cam_pos_msg ac.track (a "cam_east") (a "cam_north") (a "target_east") (a "target_north") + let wgs84 = { posn_lat = (Deg>>Rad)(a "cam_lat"); posn_long = (Deg>>Rad)(a "cam_long") } + and target_wgs84 = { posn_lat = (Deg>>Rad)(a "cam_target_lat"); posn_long = (Deg>>Rad)(a "cam_target_long") } in + + cam_pos_msg ac.track wgs84 target_wgs84 with Not_found -> () in ignore (Ground_Pprz.message_bind "CAM_STATUS" get_cam_status); @@ -445,7 +411,8 @@ let listen_flight_params = fun () -> try let ac = Hashtbl.find live_aircrafts ac_id in let a = fun s -> Pprz.float_assoc s vs in - circle_status_msg ac.track (a "circle_east") (a "circle_north") (float_of_string (Pprz.string_assoc "radius" vs)) + let wgs84 = { posn_lat = (Deg>>Rad)(a "circle_lat"); posn_long = (Deg>>Rad)(a "circle_long") } in + circle_status_msg ac.track wgs84 (float_of_string (Pprz.string_assoc "radius" vs)) with Not_found -> () in ignore (Ground_Pprz.message_bind "CIRCLE_STATUS" get_circle_status); @@ -455,7 +422,9 @@ let listen_flight_params = fun () -> try let ac = Hashtbl.find live_aircrafts ac_id in let a = fun s -> Pprz.float_assoc s vs in - segment_status_msg ac.track (a "segment1_east") (a "segment1_north") (a "segment2_east") (a "segment2_north") + let geo1 = { posn_lat = (Deg>>Rad)(a "segment1_lat"); posn_long = (Deg>>Rad)(a "segment2_long") } + and geo2 = { posn_lat = (Deg>>Rad)(a "segment2_lat"); posn_long = (Deg>>Rad)(a "segment2_long") } in + segment_status_msg ac.track geo1 geo2 with Not_found -> () in ignore (Ground_Pprz.message_bind "SEGMENT_STATUS" get_segment_status); @@ -471,117 +440,39 @@ 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 current_gm_tiles = Hashtbl.create 97 - -let gm_tile_diagonal = - let (dx, dy) = Gm.tile_size in - sqrt (float (dx*dx+dy*dy)) - -let middle = fun a b -> (a +. b) /. 2. - - -let gm_no_http = ref false let active_gm_http = fun x -> - gm_no_http := not x - -let display_gm_tile = fun (geomap:MapCanvas.widget) wgs84 -> - let desired_tile = Gm.tile_of_geo wgs84 1 in - - let key = desired_tile.Gm.key in - let scale = - try Hashtbl.find current_gm_tiles key with - Not_found -> - let (tile, jpg_file) = Gm.get_tile ~no_http:!gm_no_http wgs84 1 in - let south_lat = tile.Gm.sw_corner.posn_lat - and west_long = tile.Gm.sw_corner.posn_long in - let north_lat = south_lat +. tile.Gm.height - and east_long = west_long +. tile.Gm.width in - let center = { posn_lat = middle north_lat south_lat; posn_long = middle west_long east_long } - and ne = { posn_lat = north_lat; posn_long = east_long } in - let en_center = en_of_wgs84 geomap center in - - let sw_utm = utm_of WGS84 tile.Gm.sw_corner - and ne_utm = utm_of WGS84 ne in - let diagonal = utm_distance sw_utm ne_utm in - let tile_scale = diagonal /. gm_tile_diagonal in - - let scale = tile_scale /. geomap#get_world_unit () in - - let map = geomap#display_map ~scale en_center ~anchor:(`ANCHOR `CENTER) (GdkPixbuf.from_file jpg_file) in - map#raise 1; - - (* Rotation *) - let diagonal_angle = atan2 (ne_utm.utm_y -. sw_utm.utm_y) (ne_utm.utm_x -. sw_utm.utm_x) in - let a = pi /. 4. -. diagonal_angle in - let cos_a = cos a and sin_a = sin a in - map#affine_relative [| cos_a; sin_a; -. sin_a; cos_a; 0.; 0.|]; - - Hashtbl.add current_gm_tiles key scale; - scale in - truncate (256. *. scale) - - - + Gm.no_http := not x 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 - begin - try ignore (Thread.create (display_gm_tile geomap) wgs84) with - Gm.Not_available -> () - end; - true - - -let fill_gm_tiles = fun (geomap:MapCanvas.widget) -> - try - let sx_w, sy_w = Gdk.Drawable.get_size geomap#canvas#misc#window in + if GdkEvent.Button.button ev = 3 then + let xc = GdkEvent.Button.x ev + and yc = GdkEvent.Button.y ev in + let (xw,yw) = geomap#window_to_world xc yc in - let (ox, oy) = geomap#canvas#get_scroll_offsets in - - let yc = ref oy in - let last_size = ref 0 in - while !yc < sy_w + oy + !last_size do - let xc = ref ox - and min_height = ref max_int in - while !xc < sx_w + ox + !last_size do - let (xw, yw) = geomap#window_to_world (float !xc) (float !yc) in - let en = geomap#en_of_world xw yw in - let wgs84 = geomap#wgs84_of_en en in - let size = truncate (float (display_gm_tile geomap wgs84) *. geomap#current_zoom) in - last_size := size; - xc := !xc + size; - min_height := min !min_height size - done; - yc := !yc + !min_height - done - with - Gm.Not_available -> () - -let fill_gm_tiles = fun geomap -> ignore (Thread.create fill_gm_tiles geomap) + let wgs84 = geomap#of_world (xw,yw) in + let (xw',yw') = geomap#world_of wgs84 in + ignore(Thread.create (fun geo -> + try ignore (MapGoogle.display_tile geomap geo) with + Gm.Not_available -> ()) + wgs84); + false + else + false + + +let fill_gm_tiles = fun geomap -> ignore (Thread.create MapGoogle.fill_window geomap) - let _ = let ivy_bus = ref "127.255.255.255:2010" and geo_ref = ref "" and map_file = ref "" - and mission_file = ref "" in + and mission_file = ref "" + and projection= ref MapCanvas.UTM 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 '')"; + "-mercator", Arg.Unit (fun () -> projection:=MapCanvas.Mercator),"Switch to (Google Maps) Mercator projection"; "-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) @@ -595,27 +486,20 @@ let _ = let window = GWindow.window ~title: "Map2d" ~border_width:1 ~width:400 () in let vbox= GPack.vbox ~packing: window#add () in + let vertical_situation = GWindow.window ~title: "Vertical" ~border_width:1 ~width:400 () in let vertical_vbox= GPack.vbox ~packing: vertical_situation#add () in let quit = fun () -> GMain.Main.quit (); exit 0 in ignore (window#connect#destroy ~callback:quit); + ignore (vertical_situation#connect#destroy ~callback:quit); - let geomap = new MapCanvas.widget ~height:400 () in + let geomap = new MapCanvas.widget ~projection:!projection ~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 - let time_axis = ac_vertical_fact#add_check_item "x_axis : Time" ~active:false in - ignore (time_axis#connect#toggled (fun () -> - let set_one_track = (fun a b -> - (b.track)#set_vertical_time_axis time_axis#active) in - Hashtbl.iter (set_one_track) live_aircrafts)); - let vertical_graduations = GnoCanvas.group vertical_display#canvas#root in - vertical_display#set_vertical_factor 10.0; let active_vertical = fun x -> if x then vertical_situation#show () else vertical_situation#misc#hide () in @@ -628,34 +512,22 @@ let _ = vbox#pack ~expand:true geomap#frame#coerce; - vertical_vbox#pack ~expand:true vertical_display#frame#coerce; (* Loading an initial map *) if !geo_ref <> "" then - set_geo_ref geomap (Latlong.of_string !geo_ref) + set_georef_if_none 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 + load_map geomap xml_map_file end; - let max_level = (float_of_int max_graduations) *. vertical_delta +. !approx_ground_altitude in - vertical_display#set_vertical_max_level max_level; - for i = 0 to max_graduations do - let level = (float_of_int i) *. vertical_delta in - ignore ( vertical_display#segment ~group:vertical_graduations ~fill_color:"blue" {G.east = 0.0 ; G.north = level *. (-. vertical_display#get_vertical_factor) } {G.east = max_east ; G.north = level *. (-. vertical_display#get_vertical_factor) } ) ; - for j = 0 to max_label do - ignore( vertical_display#text ~group:vertical_graduations ~fill_color:"red" ~x_offset:30.0 ~y_offset:(-.0.5) {G.east = (float_of_int j) *. max_east /. (float_of_int max_label) ; G.north = level *. (-. vertical_display#get_vertical_factor) } ((string_of_float ( max_level -. level) )^" m") ) - done; - done; + ignore (Glib.Timeout.add 2000 (fun () -> Ground_Pprz.message_req "map2d" "AIRCRAFTS" [] (fun _sender vs -> live_aircrafts_msg geomap vs); false)); - ignore (Glib.Timeout.add 2000 (fun () -> Ground_Pprz.message_req "map2d" "AIRCRAFTS" [] (fun _sender vs -> live_aircrafts_msg geomap vertical_display vs); false)); - - ignore (Ground_Pprz.message_bind "NEW_AIRCRAFT" (fun _sender vs -> one_new_ac geomap vertical_display (Pprz.string_assoc "ac_id" vs))); + ignore (Ground_Pprz.message_bind "NEW_AIRCRAFT" (fun _sender vs -> one_new_ac geomap (Pprz.string_assoc "ac_id" vs))); listen_flight_params (); window#add_accel_group accel_group; window#show (); -(*** GMain.Main.main () ***) GtkThread.main () diff --git a/sw/ground_segment/tmtc/aircraft.ml b/sw/ground_segment/tmtc/aircraft.ml index 58fceb721d..6b2fb7b5ce 100644 --- a/sw/ground_segment/tmtc/aircraft.ml +++ b/sw/ground_segment/tmtc/aircraft.ml @@ -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; diff --git a/sw/ground_segment/tmtc/aircraft.mli b/sw/ground_segment/tmtc/aircraft.mli index 1e4028db12..b1ca60f5b2 100644 --- a/sw/ground_segment/tmtc/aircraft.mli +++ b/sw/ground_segment/tmtc/aircraft.mli @@ -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; diff --git a/sw/ground_segment/tmtc/maxstream.ml b/sw/ground_segment/tmtc/maxstream.ml index c79857f9c1..e80cf7143b 100644 --- a/sw/ground_segment/tmtc/maxstream.ml +++ b/sw/ground_segment/tmtc/maxstream.ml @@ -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 diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml index 1c11435820..5653173289 100644 --- a/sw/ground_segment/tmtc/server.ml +++ b/sw/ground_segment/tmtc/server.ml @@ -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; diff --git a/sw/ground_segment/tmtc/wavecard_connect.ml b/sw/ground_segment/tmtc/wavecard_connect.ml index bdb06e110d..eee8a5b580 100644 --- a/sw/ground_segment/tmtc/wavecard_connect.ml +++ b/sw/ground_segment/tmtc/wavecard_connect.ml @@ -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 diff --git a/sw/lib/ocaml/Makefile b/sw/lib/ocaml/Makefile index cbc5374cba..d4993c015f 100644 --- a/sw/lib/ocaml/Makefile +++ b/sw/lib/ocaml/Makefile @@ -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) diff --git a/sw/lib/ocaml/gm.ml b/sw/lib/ocaml/gm.ml index f2ba8aaa1a..254c502807 100644 --- a/sw/lib/ocaml/gm.ml +++ b/sw/lib/ocaml/gm.ml @@ -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 diff --git a/sw/lib/ocaml/gm.mli b/sw/lib/ocaml/gm.mli index cf64d13852..8b270353fe 100644 --- a/sw/lib/ocaml/gm.mli +++ b/sw/lib/ocaml/gm.mli @@ -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 + diff --git a/sw/lib/ocaml/latlong.ml b/sw/lib/ocaml/latlong.ml index 9e8b1c826b..b672f71ed4 100644 --- a/sw/lib/ocaml/latlong.ml +++ b/sw/lib/ocaml/latlong.ml @@ -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< 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. + diff --git a/sw/lib/ocaml/latlong.mli b/sw/lib/ocaml/latlong.mli index 8a53b6c57b..7d4565682e 100644 --- a/sw/lib/ocaml/latlong.mli +++ b/sw/lib/ocaml/latlong.mli @@ -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 *) diff --git a/sw/lib/ocaml/mapCanvas.ml b/sw/lib/ocaml/mapCanvas.ml index 53148cf91a..77ca9abd11 100644 --- a/sw/lib/ocaml/mapCanvas.ml +++ b/sw/lib/ocaml/mapCanvas.ml @@ -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 diff --git a/sw/lib/ocaml/mapGoogle.ml b/sw/lib/ocaml/mapGoogle.ml new file mode 100644 index 0000000000..43eede78f9 --- /dev/null +++ b/sw/lib/ocaml/mapGoogle.ml @@ -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 + diff --git a/sw/lib/ocaml/mapGoogle.mli b/sw/lib/ocaml/mapGoogle.mli new file mode 100644 index 0000000000..2b158fe577 --- /dev/null +++ b/sw/lib/ocaml/mapGoogle.mli @@ -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 *) diff --git a/sw/lib/ocaml/mapTrack.ml b/sw/lib/ocaml/mapTrack.ml index 3f011dc403..ead09f1f87 100644 --- a/sw/lib/ocaml/mapTrack.ml +++ b/sw/lib/ocaml/mapTrack.ml @@ -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 diff --git a/sw/lib/ocaml/mapWaypoints.ml b/sw/lib/ocaml/mapWaypoints.ml index 9555147c89..88d00a37c7 100644 --- a/sw/lib/ocaml/mapWaypoints.ml +++ b/sw/lib/ocaml/mapWaypoints.ml @@ -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 = diff --git a/sw/lib/ocaml/mapWaypoints.mli b/sw/lib/ocaml/mapWaypoints.mli index 779c64db25..a65b0ec020 100644 --- a/sw/lib/ocaml/mapWaypoints.mli +++ b/sw/lib/ocaml/mapWaypoints.mli @@ -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 diff --git a/sw/lib/ocaml/ubx.ml b/sw/lib/ocaml/ubx.ml index 09b1b62719..45b7a2fb2f 100644 --- a/sw/lib/ocaml/ubx.ml +++ b/sw/lib/ocaml/ubx.ml @@ -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 -> diff --git a/sw/lib/ocaml/ubx.mli b/sw/lib/ocaml/ubx.mli index 6f2453c46c..21d2035d1e 100644 --- a/sw/lib/ocaml/ubx.mli +++ b/sw/lib/ocaml/ubx.mli @@ -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 diff --git a/sw/lib/ocaml/wavecard.mli b/sw/lib/ocaml/wavecard.mli index 66f5425fb3..7c7a648e99 100644 --- a/sw/lib/ocaml/wavecard.mli +++ b/sw/lib/ocaml/wavecard.mli @@ -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 *) diff --git a/sw/lib/perl/Paparazzi/Aircraft.pm b/sw/lib/perl/Paparazzi/Aircraft.pm index af1c3695c5..e55236dd83 100644 --- a/sw/lib/perl/Paparazzi/Aircraft.pm +++ b/sw/lib/perl/Paparazzi/Aircraft.pm @@ -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], diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml index e36a39ccc1..e4694c60b2 100644 --- a/sw/simulator/sitl.ml +++ b/sw/simulator/sitl.ml @@ -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 ->