diff --git a/sw/configurator/medit.ml b/sw/configurator/medit.ml
index dbfb118c1a..613461dd56 100644
--- a/sw/configurator/medit.ml
+++ b/sw/configurator/medit.ml
@@ -320,8 +320,9 @@ let float_attrib = fun x a -> float_of_string (Xml.attrib x a)
 
 
 let load_mission_xml = fun root zoomadj xml ->
-  let xml_tree_view = XmlEdit.create (Dtd.parse_file dtd) xml in
+  let xml_tree_view, window = XmlEdit.create (Dtd.parse_file dtd) xml in
   let xml_root = XmlEdit.root xml_tree_view in
+  window#show ();
   let wpts = XmlEdit.child xml_root "waypoints" in
 
   Ref.set (float_attrib xml "lat0") (float_attrib xml "lon0");
diff --git a/sw/ground_segment/cockpit/Makefile b/sw/ground_segment/cockpit/Makefile
index f2072db372..717ffde887 100644
--- a/sw/ground_segment/cockpit/Makefile
+++ b/sw/ground_segment/cockpit/Makefile
@@ -1,5 +1,5 @@
-OCAMLC=ocamlc
-OCAMLOPT=ocamlopt
+OCAMLC=ocamlc -thread
+OCAMLOPT=ocamlopt -thread
 INCLUDES=-I +lablgtk2 -I +camlimages -I ../../lib/ocaml
 LIBS=glibivy-ocaml.cma lablgtk.cma ci_core.cma ci_png.cma ci_gif.cma ci_jpeg.cma ci_tiff.cma ci_bmp.cma ci_ppm.cma ci_ps.cma lib-pprz.cma lablgnomecanvas.cma xlib-pprz.cma
 CMXA=$(LIBS:.cma=.cmxa)
@@ -11,19 +11,19 @@ opt : map2d.opt
 
 
 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 $@
+	$(OCAMLC) -custom $(INCLUDES) $(LIBS) threads.cma gtkThread.cmo gtkInit.cmo $< -o $@
 
 map2d.opt : map2d.cmx
-	$(OCAMLOPT) $(INCLUDES) str.cmxa unix.cmxa xml-light.cmxa $(LIBS:.cma=.cmxa) gtkInit.cmx $< -o $@
+	$(OCAMLOPT) $(INCLUDES) str.cmxa unix.cmxa xml-light.cmxa $(LIBS:.cma=.cmxa) threads.cmxa gtkInit.cmx $< -o $@
 
 
 .SUFFIXES: .ml .mli .cmo .cmi .cmx
 
-.ml.cmo:
+%.cmo: %.ml
 	$(OCAMLC) $(INCLUDES) -labels -w s -c $<
-.mli.cmi:
+%.cmi: %.ml
 	$(OCAMLC) $(INCLUDES) -labels -w s -c $<
-.ml.cmx:
+%.cmx: %.ml
 	$(OCAMLOPT) $(INCLUDES) -labels -w s -c $<
 
 clean:
diff --git a/sw/ground_segment/cockpit/map2d.ml b/sw/ground_segment/cockpit/map2d.ml
index 7d86413558..4b5d65aa75 100644
--- a/sw/ground_segment/cockpit/map2d.ml
+++ b/sw/ground_segment/cockpit/map2d.ml
@@ -31,10 +31,10 @@ 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 soi = string_of_int
+let sof = string_of_float
 let list_separator = Str.regexp ","
 
 (*** parameters used for creating the vertical display window
@@ -51,17 +51,33 @@ let home = Env.paparazzi_home
 let (//) = Filename.concat
 let default_path_srtm = home // "data" // "srtm"
 let default_path_maps = home // "data" // "maps" // ""
+let path_fps = home // "conf" // "flight_plans" // ""
+let fp_dtd = path_fps // "flight_plan.dtd"
 let var_maps_path = home // "var" // "maps"
 let _ = 
   ignore (Sys.command (sprintf "mkdir -p %s" var_maps_path))
+let fp_example = path_fps // "example.xml"
+
+let labelled_entry = fun text value h ->
+  let _ = GMisc.label ~text ~packing:h#add () in
+  GEdit.entry ~text:value ~packing:h#add ()
 
 
-
+(** Dummy flight plan *)
+let dummy_fp = fun latlong ->
+  Xml.Element("flight_plan",
+	      ["lat0", sof ((Rad>>Deg)latlong.posn_lat);
+	       "lon0", sof ((Rad>>Deg)latlong.posn_long);
+	       "alt", "42.";
+	       "MAX_DIST_FROM_HOME", "1000."
+	     ],
+	      [Xml.Element("waypoints", [],[])])
+    
 type aircraft = {
     config : Pprz.values;
     track : MapTrack.track;
     color: color;
-    mutable fp_group : (MapWaypoints.group * (int * MapWaypoints.waypoint) list) option;
+    fp_group : MapFP.flight_plan;
     fp : Xml.xml;
     dl_settings : GWindow.window
   }
@@ -72,7 +88,6 @@ let set_georef_if_none = fun geomap wgs84 ->
   match geomap#georef with
     None -> geomap#set_georef wgs84
   | Some _ -> ()
-	
 
 
 let display_map = fun (geomap:G.widget) xml_map ->
@@ -106,41 +121,22 @@ let display_map = fun (geomap:G.widget) xml_map ->
   | _ -> failwith (sprintf "display_map: two ref points required")
 
 
-let load_map = fun geomap () ->
+let load_map = fun (geomap:G.widget) () ->
   match GToolbox.select_file ~title:"Open Map" ~filename:(default_path_maps^"*.xml") () with
     None -> ()
   | Some f -> display_map geomap f
 
 
-let load_mission = fun color geomap xml ->
+(** FIXME : also in MapFP.ml *)
+let georef_of_fp = fun xml ->
   let lat0 = float_attr xml "lat0"
-  and lon0 = float_attr xml "lon0"
-  and alt0 = float_attr xml "alt" 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
-  
-  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 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 wgs84 in
-	if  ExtXml.attrib wp "name" = "HOME" then
-	  ignore (geomap#circle ~color wgs84 max_dist_from_home);
-	incr i;
-	!i, w
-      ) 
-      (Xml.children waypoints) in
-  fp, wpts
+  and lon0 = float_attr xml "lon0" in
+  {posn_lat = (Deg>>Rad)lat0; posn_long = (Deg>>Rad)lon0 }
 
+	    
+let load_mission = fun color geomap xml ->
+  set_georef_if_none geomap (georef_of_fp xml);
+  new MapFP.flight_plan geomap color fp_dtd xml
 
 let aircraft_pos_msg = fun track wgs84 heading altitude speed climb ->
   let h = 
@@ -166,42 +162,30 @@ let ap_status_msg = fun track flight_time ->
     track#update_ap_status flight_time
     
 
-let display_fp = fun geomap ac ->
-  try
-    let ac = Hashtbl.find live_aircrafts ac in
-    ac.fp_group <- Some (load_mission ac.color geomap ac.fp)
-  with Failure x ->
-    GToolbox.message_box ~title:"Error while loading flight plan" x
-
-
 let show_mission = fun geomap ac on_off ->
+  let a = Hashtbl.find live_aircrafts ac in
   if on_off then
-    display_fp geomap ac
+    a.fp_group#show ()
   else
-    let a = Hashtbl.find live_aircrafts ac in
-    match a.fp_group with
-      None -> ()
-    | Some (g, _wpts) -> 
-	a.fp_group <- None;
-	g#group#destroy ()
+    a.fp_group#hide ()
+
 
 let commit_changes = fun ac ->
   let a = Hashtbl.find live_aircrafts ac in
-  match a.fp_group with
-    Some (_g, wpts) -> 
-      List.iter 
-	(fun (i, w) -> 
-	  if w#moved then 
-	    let wgs84 = w#pos in
-	    let vs = ["ac_id", Pprz.String ac;
-		      "wp_id", Pprz.Int i;
-		      "lat", Pprz.Float ((Rad>>Deg)wgs84.posn_lat);
-		      "long", Pprz.Float ((Rad>>Deg)wgs84.posn_long);
+  List.iter 
+    (fun w ->
+      let (i, w) = a.fp_group#index w in
+      if w#moved then 
+	let wgs84 = w#pos in
+	let vs = ["ac_id", Pprz.String ac;
+		  "wp_id", Pprz.Int i;
+		  "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)
-	wpts
-  | _ -> ()
+		] in
+	Ground_Pprz.message_send "map2d" "MOVE_WAYPOINT" vs)
+    a.fp_group#waypoints
+
 
 let send_event = fun ac e ->
   Ground_Pprz.message_send "map2d" "SEND_EVENT" 
@@ -347,7 +331,9 @@ let create_ac = fun (geomap:MapCanvas.widget) ac_id config ->
 
   let ds = dl_settings ac_id fp_xml in
 
-  Hashtbl.add live_aircrafts ac_id { track = track; color = color; fp_group = None ; config = config ; fp = fp_xml; dl_settings = ds}
+  let fp = load_mission color geomap fp_xml in
+  fp#hide ();
+  Hashtbl.add live_aircrafts ac_id { track = track; color = color; fp_group = fp ; config = config ; fp = fp_xml; dl_settings = ds}
   
 
 
@@ -447,24 +433,6 @@ let gm_auto = ref false
 let active_gm_auto = fun x -> 
   gm_auto := x
 
-let button_press = fun (geomap:MapCanvas.widget) ev ->
-  if GdkEvent.Button.button ev = 3 then begin
-    let xc = GdkEvent.Button.x ev 
-    and yc = GdkEvent.Button.y ev in
-    let (xw,yw) = geomap#window_to_world xc yc in
-    
-    let wgs84 = geomap#of_world (xw,yw) in
-    let display = fun geo ->
-      if Gdk.Convert.test_modifier `SHIFT (GdkEvent.Button.state ev) then
-	MapIGN.display_tile geomap geo
-      else
-	try ignore (MapGoogle.display_tile geomap geo) with
-	  Gm.Not_available -> () in
-
-    ignore(Thread.create display wgs84)
-  end;
-  false
-
 
 let fill_gm_tiles = fun geomap -> 
   ignore (Thread.create MapGoogle.fill_window geomap)
@@ -472,17 +440,6 @@ let fill_gm_tiles = fun geomap ->
 let gm_update = fun geomap ->
   if !gm_auto then fill_gm_tiles geomap
 
-let file_dialog ?(filename="*.xml") ~title ~callback () =
-  let sel = GWindow.file_selection ~title ~filename ~modal:true () in
-  ignore (sel#cancel_button#connect#clicked ~callback:sel#destroy);
-  ignore
-    (sel#ok_button#connect#clicked
-       ~callback:(fun () ->
-	 let name = sel#filename in
-	 sel#destroy ();
-	 callback name));
-  sel#show ()
-
 let map_from_region = fun (geomap:MapCanvas.widget) () ->
   match geomap#region with
     None -> GToolbox.message_box "Error" "Select a region first (drag left button)"
@@ -494,20 +451,204 @@ let map_from_region = fun (geomap:MapCanvas.widget) () ->
       let (x0, y0) = geomap#canvas#get_scroll_offsets in
       let xc1= xc1 - x0 and yc1 = yc1 - y0 in
       GdkPixbuf.get_from_drawable ~dest:p ~width ~height ~src_x:xc1 ~src_y:yc1 geomap#canvas#misc#window;
-      file_dialog ~filename:(default_path_maps//".xml") ~title:"Save region map" ~callback:(fun xml_file ->
-	let jpg = Filename.chop_extension xml_file ^ ".png" in
-	GdkPixbuf.save jpg "png" p;
-	let point = fun (x,y) xyw ->
-	  let wgs84 = geomap#of_world xyw in
-	  Xml.Element ("point", ["x",soi x;"y",soi y;"geo", Latlong.string_of wgs84], []) in
-	let points = [point (0, 0) (xw1,yw1); point (width, height) (xw2,yw2)] in
-	let xml = Xml.Element ("map", 
-			       ["file", Filename.basename jpg;
-				"projection", geomap#projection],
+      match GToolbox.select_file ~filename:(default_path_maps//".xml") ~title:"Save region map" () with
+	None -> ()
+      | Some xml_file  ->
+	  let jpg = Filename.chop_extension xml_file ^ ".png" in
+	  GdkPixbuf.save jpg "png" p;
+	  let point = fun (x,y) xyw ->
+	    let wgs84 = geomap#of_world xyw in
+	    Xml.Element ("point", ["x",soi x;"y",soi y;"geo", Latlong.string_of wgs84], []) in
+	  let points = [point (0, 0) (xw1,yw1); point (width, height) (xw2,yw2)] in
+	  let xml = Xml.Element ("map", 
+				 ["file", Filename.basename jpg;
+				  "projection", geomap#projection],
 			       points) in
-	let f = open_out xml_file in
-	Printf.fprintf f "%s\n" (Xml.to_string_fmt xml);
-	close_out f) ()
+	  let f = open_out xml_file in
+	  Printf.fprintf f "%s\n" (Xml.to_string_fmt xml);
+	  close_out f
+
+
+module Edit = struct
+  (** Editing of ONE single flight plan *)
+  let current_fp = ref None
+
+  (** Wrapper checking there is currently no flight plan loaded *)
+  let if_none = fun f ->
+    match !current_fp with
+      Some _ -> GToolbox.message_box "Error" "Only one editable flight plan at a time"
+    | None -> f ()
+
+
+  let load_xml_fp = fun geomap ?(xml_file=path_fps) xml ->
+    set_georef_if_none geomap (georef_of_fp xml);
+    let fp = new MapFP.flight_plan geomap "red" fp_dtd xml in
+    fp#show_xml ();
+    current_fp := Some (fp,xml_file);
+    fp
+
+
+  let new_fp = fun geomap () ->
+    if_none (fun () ->
+      let dialog = GWindow.window ~border_width:10 ~title:"New flight plan" () in
+      let dvbx = GPack.box `VERTICAL ~packing:dialog#add () in
+      let h = GPack.hbox ~packing:dvbx#pack () in
+      let default_latlong =
+	match geomap#georef with
+	  None -> "WGS84 37.21098 -113.45678"
+	| Some geo -> Latlong.string_of geo in
+      let latlong  = labelled_entry "latlong" default_latlong h in
+      let alt0 = labelled_entry "ground_alt" "380" h in
+      let h = GPack.hbox ~packing:dvbx#pack () in
+      let alt = labelled_entry "alt" "430" h in
+      let qfu = labelled_entry "QFU" "270" h in
+      let mdfh = labelled_entry "Max dist" "500" h in
+      
+      let h = GPack.hbox ~packing:dvbx#pack () in
+      let name  = labelled_entry "Name" "Test flight" h in
+      
+      let h = GPack.hbox ~packing:dvbx#pack () in
+      let createfp = GButton.button ~label:"Create FP" ~packing: h#add () in
+      let cancel = GButton.button ~label:"Close" ~packing: h#add () in 
+      ignore(cancel#connect#clicked ~callback:dialog#destroy);
+      ignore(createfp#connect#clicked ~callback:
+	       begin fun _ ->
+		 let xml = Xml.parse_file fp_example in
+		 let s = ExtXml.subst_attrib in
+		 let wgs84 = Latlong.of_string latlong#text in
+		 let xml = s "lat0" (deg_string_of_rad wgs84.posn_lat) xml in
+		 let xml = s "lon0" (deg_string_of_rad wgs84.posn_long) xml in
+		 let xml = s "ground_alt" alt0#text xml in
+		 let xml = s "qfu" qfu#text xml in
+		 let xml = s "alt" alt#text xml in
+		 let xml = s "max_dist_from_home" mdfh#text xml in
+		 let xml = s "name" name#text xml in
+		 ignore (load_xml_fp geomap xml);
+		 dialog#destroy ()
+	       end);
+      dialog#show ())
+
+	
+(** Loading a flight plan for edition *)
+  let load_fp = fun geomap () ->
+    if_none (fun () ->
+      match GToolbox.select_file ~title:"Open flight plan" ~filename:(path_fps^"*.xml") () with
+	None -> ()
+      | Some xml_file ->
+	  let xml = Xml.parse_file xml_file in
+	  ignore (load_xml_fp geomap ~xml_file xml))
+
+  let create_wp = fun geomap geo ->
+    match !current_fp with
+      None -> GToolbox.message_box "Error" "Load a flight plan first"
+    | Some (fp,_) ->
+	ignore (fp#add_waypoint geo)
+
+  let close_fp = fun geomap () ->
+    match !current_fp with
+      None -> () (* Nothing to close *)
+    | Some (fp, filename) -> 
+	fp#destroy ();
+	current_fp := None
+
+  let save_fp = fun geomap () ->
+    match !current_fp with
+      None -> () (* Nothing to save *)
+    | Some (fp, filename) ->
+	match GToolbox.select_file ~title:"Save Flight Plan" ~filename () with
+	  None -> ()
+	| Some file -> 
+	    let f  = open_out file in
+	    fprintf f "%s\n" (Xml.to_string_fmt fp#xml);
+	    close_out f
+
+  let ref_point_of_waypoint = fun xml ->
+    Xml.Element("point", ["x",Xml.attrib xml "x";
+			  "y",Xml.attrib xml "y";
+			  "geo", Xml.attrib xml "name"],[])
+
+(** Calibration of chosen image *)
+  let calibrate_map = fun (geomap:G.widget) () ->
+    match !current_fp with
+    | Some (fp,_) ->  GToolbox.message_box "Error" "Close current flight plan before calibration"
+    | None ->
+	match GToolbox.select_file ~filename:default_path_maps ~title:"Open Image" () with
+	  None -> ()
+	| Some image -> 
+	    (** Displaying the image in the NW corner *)
+	    let pixbuf = GdkPixbuf.from_file image in
+	    let pix = GnoCanvas.pixbuf ~pixbuf ~props:[`ANCHOR `NW] geomap#canvas#root in
+	    let (x0, y0) = geomap#canvas#get_scroll_offsets in
+	    let (x,y) = geomap#canvas#window_to_world (float x0) (float y0) in
+	    pix#move x y;
+
+	    (** Open a dummy flight plan *)
+	    let dummy_georef =
+	      match geomap#georef with
+		None -> {posn_lat = (Deg>>Rad)43.; posn_long = (Deg>>Rad)1. }
+	      | Some geo -> geo in
+	    let fp_xml = dummy_fp dummy_georef in
+	    let fp = load_xml_fp geomap fp_xml in
+	      
+	    (** Dialog to finish calibration *)
+	    let dialog = GWindow.window ~border_width:10 ~title:"Map calibration" () in
+	    let v = GPack.vbox ~packing:dialog#add () in
+	    let _ = GMisc.label ~text:"Choose 2 (or more) waypoints (CTRL Left Button)\nRename the waypoints with their geographic coordinates\nFor example: 'WGS84 43.123456 1.234567' or 'UTM 530134 3987652 12' or 'LBT2e 123456 543210'\nClick the button below to save the XML result file\n" ~packing:v#add () in
+	    let h = GPack.hbox ~packing:v#pack () in
+	    let cal = GButton.button ~label:"Calibrate" ~packing:h#add () in
+	    let cancel = GButton.button ~label:"Close" ~packing:h#add () in
+	    let destroy = fun () ->
+	      dialog#destroy ();
+	      close_fp geomap ();
+	      pix#destroy () in
+	    ignore(cancel#connect#clicked ~callback:destroy);
+	    ignore(cal#connect#clicked ~callback:(fun _ ->
+	      let points = List.map XmlEdit.xml_of_node fp#waypoints in
+	      let points = List.map ref_point_of_waypoint points in
+	      let xml = Xml.Element ("map", 
+				     ["file", Filename.basename image;
+				      "projection", geomap#projection],
+				     points) in
+	      match GToolbox.select_file ~filename:(default_path_maps//".xml") ~title:"Save calibrated map" () with
+		None -> ()
+	      | Some xml_file ->
+		  let f = open_out xml_file in
+		  Printf.fprintf f "%s\n" (Xml.to_string_fmt xml);
+		  close_out f));
+	    dialog#show ()
+end (** Edit module *)
+    
+
+
+let button_press = fun (geomap:MapCanvas.widget) ev ->
+  let state = GdkEvent.Button.state ev in
+  if GdkEvent.Button.button ev = 3 then begin
+    (** Display a tile from Google Maps or IGN *)
+    let xc = GdkEvent.Button.x ev 
+    and yc = GdkEvent.Button.y ev in
+    let (xw,yw) = geomap#window_to_world xc yc in
+    
+    let wgs84 = geomap#of_world (xw,yw) in
+    let display = fun geo ->
+      if Gdk.Convert.test_modifier `SHIFT (GdkEvent.Button.state ev) then
+	MapIGN.display_tile geomap geo
+      else
+	try ignore (MapGoogle.display_tile geomap geo) with
+	  Gm.Not_available -> () in
+
+    ignore(Thread.create display wgs84);
+    true;
+  end else if GdkEvent.Button.button ev = 1 && Gdk.Convert.test_modifier `CONTROL state then begin
+    let xc = GdkEvent.Button.x ev in
+    let yc = GdkEvent.Button.y ev in
+    let xyw = geomap#canvas#window_to_world xc yc in
+    let geo = geomap#of_world xyw in
+    Edit.create_wp geomap geo;
+    true
+  end else
+    false
+
+
 
 
 let _ =
@@ -558,6 +699,7 @@ let _ =
   let map_menu = geomap#factory#add_submenu "Maps" in
   let map_menu_fact = new GMenu.factory ~accel_group map_menu in
   ignore (map_menu_fact#add_item "Load" ~key:GdkKeysyms._M ~callback:(load_map geomap));
+  ignore (map_menu_fact#add_item "Calibrate" ~key:GdkKeysyms._C ~callback:(Edit.calibrate_map geomap));
   ignore (map_menu_fact#add_item "GM Fill" ~key:GdkKeysyms._G ~callback:(fun _ -> fill_gm_tiles geomap));
   ignore (map_menu_fact#add_check_item "GM Http" ~key:GdkKeysyms._H ~active:true ~callback:active_gm_http);
   ignore (map_menu_fact#add_check_item "GM Auto" ~active:false ~callback:active_gm_auto);
@@ -566,6 +708,15 @@ let _ =
   (** Connect Google Maps display to view change *)
   geomap#connect_view (fun () -> gm_update geomap);
 
+  
+  (** Flight plan editing *)
+  let fp_menu = geomap#factory#add_submenu "Edit" in
+  let fp_menu_fact = new GMenu.factory ~accel_group fp_menu in
+  ignore (fp_menu_fact#add_item "New flight plan" ~key:GdkKeysyms._N ~callback:(Edit.new_fp geomap));
+  ignore (fp_menu_fact#add_item "Open flight plan" ~key:GdkKeysyms._O ~callback:(Edit.load_fp geomap));
+  ignore (fp_menu_fact#add_item "Save flight plan" ~key:GdkKeysyms._S ~callback:(Edit.save_fp geomap));
+  ignore (fp_menu_fact#add_item "Close flight plan" ~callback:(Edit.close_fp geomap));
+
   (** Separate from A/C menus *)
   ignore (geomap#factory#add_separator ());
 
diff --git a/sw/lib/ocaml/Makefile b/sw/lib/ocaml/Makefile
index d1bf61bb8b..f896f9ac2b 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 mapGoogle.ml mapIGN.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 mapIGN.ml ml_gtk_drag.o xmlEdit.ml mapFP.ml
 XCMO = $(XSRC:.ml=.cmo)
 XCMX = $(XSRC:.ml=.cmx)
 
diff --git a/sw/lib/ocaml/mapCanvas.ml b/sw/lib/ocaml/mapCanvas.ml
index 54ae0e5512..6f0c749b45 100644
--- a/sw/lib/ocaml/mapCanvas.ml
+++ b/sw/lib/ocaml/mapCanvas.ml
@@ -55,6 +55,7 @@ class basic_widget = fun ?(height=800) ?width ?(projection = Mercator) ?georef (
   let background = GnoCanvas.group canvas#root in
   let view_cbs = Hashtbl.create 3 in (* Store for view event callback *)
   let region_rectangle = GnoCanvas.rect canvas#root ~props:[`WIDTH_PIXELS 2; `OUTLINE_COLOR "red"] in
+
   object (self)
    
 (** GUI attributes *)
@@ -238,7 +239,7 @@ class basic_widget = fun ?(height=800) ?width ?(projection = Mercator) ?georef (
       let xc = GdkEvent.Button.x ev in
       let yc = GdkEvent.Button.y ev in  
       match GdkEvent.Button.button ev with
-	1 ->
+	1 when Gdk.Convert.test_modifier `SHIFT (GdkEvent.Button.state ev) ->
 	  let (x1,y1) = self#window_to_world xc yc in
 	  grouping <- Some (x1,y1);
 	  region_rectangle#set [`X1 x1; `Y1 y1; `X2 x1; `Y2 y1];
diff --git a/sw/lib/ocaml/mapFP.ml b/sw/lib/ocaml/mapFP.ml
new file mode 100644
index 0000000000..871ca4f270
--- /dev/null
+++ b/sw/lib/ocaml/mapFP.ml
@@ -0,0 +1,136 @@
+(*
+ * $Id$
+ *
+ * Displaying and editing a flight plan on a MapCanvas
+ *  
+ * 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. 
+ *
+ *)
+
+open Latlong
+
+let sof = string_of_float
+let float_attr = fun xml a -> float_of_string (ExtXml.attrib xml a)
+let rec assoc_nocase at = function
+    [] -> raise Not_found
+  | (a, v)::avs ->
+      if String.uppercase at = String.uppercase a then v else assoc_nocase at avs
+
+(** Connect a change in the XML editor to the graphical rep *)
+let update_wp utm_ref wp = function
+    XmlEdit.Deleted -> wp#delete
+  | XmlEdit.New_child _ -> failwith "update_wp"
+  | XmlEdit.Modified attribs ->
+      try
+	let float_attrib = fun a -> float_of_string (assoc_nocase a attribs) in
+	let x = (float_attrib "x") and y = (float_attrib "y") in
+	let wgs84 = Latlong.of_utm WGS84 (utm_add utm_ref (x, y)) in
+	wp#set wgs84;
+	wp#set_name (assoc_nocase "name" attribs)
+      with
+	_ -> ()
+
+let waypoints_node = fun xml_tree ->
+  let xml_root = XmlEdit.root xml_tree in
+  XmlEdit.child xml_root "waypoints"
+
+(** Connect a change from the graphical rep to the xml tree *)
+let update_xml = fun xml_tree utm0 wp ->
+  let xml_wpts = XmlEdit.children (waypoints_node xml_tree) in
+  let node = List.find (fun w -> XmlEdit.attrib w "name" = wp#name) xml_wpts in
+  let utm = utm_of WGS84 (wp#pos) in
+  let (dx, dy) = utm_sub utm utm0 in
+  XmlEdit.set_attribs node ["name",wp#name; "x",sof dx; "y",sof dy; "alt", sof wp#alt]
+
+let new_wp = fun xml_tree waypoints utm_ref node ->
+  let float_attrib = 
+    fun a -> try float_of_string (XmlEdit.attrib node a) with _ -> 0. in
+  let x = (float_attrib "x") and y = (float_attrib "y") in
+  let wgs84 = Latlong.of_utm WGS84 (utm_add utm_ref (x, y)) in
+  let alt = float_attrib "alt" in
+  let name = XmlEdit.attrib node "name" in
+  let wp = MapWaypoints.waypoint waypoints ~name ~alt wgs84 in
+  XmlEdit.connect node (update_wp utm_ref wp);
+  wp#connect (fun () -> update_xml xml_tree utm_ref wp);
+  wp
+
+let gensym =
+  let x = ref 0 in
+  fun p -> incr x; Printf.sprintf "%s%d" p !x
+
+ 
+class flight_plan = fun geomap color fp_dtd xml ->
+  (** Xml Editor *)
+  let xml_tree_view, xml_window = XmlEdit.create (Dtd.parse_file fp_dtd) xml in
+  let xml_root = XmlEdit.root xml_tree_view in
+  let xml_wpts = XmlEdit.child xml_root "waypoints" in
+
+  (** Geographic ref *)
+  let lat0 = float_attr xml "lat0"
+  and lon0 = float_attr xml "lon0"
+  and alt0 = float_attr xml "alt" in
+  let ref_wgs84 = {posn_lat = (Deg>>Rad)lat0; posn_long = (Deg>>Rad)lon0 } in
+  let utm0 = utm_of WGS84 ref_wgs84 in
+
+  (** The graphical waypoints *)
+  let wpts_group = new MapWaypoints.group ~color ~editable:true geomap in
+
+  let yaws = Hashtbl.create 5 in (* Yes Another Waypoints Store *)
+  let create_wp =
+    let i = ref 0 in
+    fun node ->
+      let w = new_wp xml_tree_view wpts_group utm0 node in
+      Hashtbl.add yaws (XmlEdit.attrib node "name") (!i, w);
+      w in
+
+  let max_dist_from_home = float_attr xml "MAX_DIST_FROM_HOME" in
+
+  let _ = List.iter
+      (fun wp ->
+	let w = create_wp wp in
+	let name = XmlEdit.attrib wp "name" in
+	if name = "HOME" then
+	  ignore (geomap#circle ~color w#pos max_dist_from_home)) 
+      (XmlEdit.children xml_wpts) in
+  
+  object
+    val mutable max_dist_from_home = max_dist_from_home
+    method georef = ref_wgs84
+    method show_xml () = xml_window#show ()
+    method destroy () = 
+      wpts_group#group#destroy ();
+      xml_window#destroy ()
+    method show () = wpts_group#group#show ()
+    method hide () = wpts_group#group#hide ()
+    method index wp = Hashtbl.find yaws (XmlEdit.attrib wp "name")
+    method waypoints = XmlEdit.children (waypoints_node xml_tree_view)
+    method xml = XmlEdit.xml_of_view xml_tree_view
+    method add_waypoint (geo:geographic) =
+      let name = gensym "wp" in
+      let utm = utm_of WGS84 geo in
+      let (dx, dy) = utm_sub utm utm0 in
+      let node = XmlEdit.add_child xml_wpts "waypoint" ["x",sof dx;"y",sof dy;"name",name] in
+      create_wp node
+
+    initializer (
+  (** Create a graphic waypoint when it is created from the xml editor *)
+      XmlEdit.connect xml_wpts (function XmlEdit.New_child node -> ignore (create_wp  node) | _ -> ())
+     )
+  end
diff --git a/sw/lib/ocaml/mapFP.mli b/sw/lib/ocaml/mapFP.mli
new file mode 100644
index 0000000000..3ea707926a
--- /dev/null
+++ b/sw/lib/ocaml/mapFP.mli
@@ -0,0 +1,44 @@
+(*
+ * $Id$
+ *
+ * Displaying and editing a flight plan on a MapCanvas
+ *  
+ * 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. 
+ *
+ *)
+
+(** [flight_plan geomap color dtd_tile xml] *)
+class flight_plan :
+  MapCanvas.widget ->
+  string ->
+  string ->
+  Xml.xml ->
+  object
+    val mutable max_dist_from_home : float
+    method add_waypoint : Latlong.geographic -> MapWaypoints.waypoint
+    method destroy : unit -> unit
+    method georef : Latlong.geographic
+    method hide : unit -> unit
+    method index : XmlEdit.node -> int * MapWaypoints.waypoint
+    method show : unit -> unit
+    method show_xml : unit -> unit
+    method waypoints : XmlEdit.node list
+    method xml : Xml.xml
+  end
diff --git a/sw/lib/ocaml/mapWaypoints.ml b/sw/lib/ocaml/mapWaypoints.ml
index 88d00a37c7..ec601895ef 100644
--- a/sw/lib/ocaml/mapWaypoints.ml
+++ b/sw/lib/ocaml/mapWaypoints.ml
@@ -44,6 +44,10 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) wgs84 ->
   and color = group#color
   and editable = group#editable in
   let xw, yw = geomap#world_of wgs84 in
+  let callbacks = Hashtbl.create 5 in
+  let updated () =
+    Hashtbl.iter (fun cb _ -> cb ()) callbacks in
+
   object (self)
     val mutable x0 = 0.
     val mutable y0 = 0.
@@ -52,14 +56,19 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) wgs84 ->
 	~props:[`FILL_COLOR color; `OUTLINE_COLOR "midnightblue" ; `WIDTH_UNITS 1.; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")]
 	
     val label = GnoCanvas.text group#group ~props:[`TEXT name; `X s; `Y 0.; `ANCHOR `SW; `FILL_COLOR "green"]
-    val mutable name = name
+    val mutable name = name (* FIXME: already in label ! *)
     val mutable alt = alt
     val mutable moved = false
+    val mutable deleted = false
     initializer self#move xw yw
+    method connect = fun (cb:unit -> unit) ->
+      Hashtbl.add callbacks cb ()
     method name = name
     method set_name n =
-      if n <> name then
-	name <- n
+      if n <> name then begin
+	name <- n;
+	label#set [`TEXT name]
+      end
     method alt = alt
     method label = label
     method xy = let a = item#i2w_affine in (a.(4), a.(5)) (*** item#i2w 0. 0. causes Seg Fault !***)
@@ -69,7 +78,7 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) wgs84 ->
       let dvbx = GPack.box `VERTICAL ~packing:dialog#add () 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 ename  = GEdit.entry ~text:name ~editable:false ~packing:dvbx#add () in
       let e_pos  = GEdit.entry ~text:s ~packing:dvbx#add () in
       let ea  = GEdit.entry ~text:(string_of_float alt) ~packing:dvbx#add () in
       let cancel = GButton.button ~label:"Cancel" ~packing: dvbx#add () in 
@@ -81,6 +90,7 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) wgs84 ->
 		 alt <- float_of_string ea#text;
 		 label#set [`TEXT name];
 		 self#set (LL.of_string e_pos#text);
+		 updated ();
 		 dialog#destroy ()
 	       end);
       dialog#show ()
@@ -114,6 +124,7 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) wgs84 ->
 	      let dx = geomap#current_zoom *. (x-. x0) 
 	      and dy = geomap#current_zoom *. (y -. y0) in
 	      self#move dx dy ;
+	      updated ();
 	      x0 <- x; y0 <- y
 	    end
 	| `BUTTON_RELEASE ev ->
@@ -126,6 +137,7 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) wgs84 ->
       true
     initializer ignore(if editable then ignore (item#connect#event self#event))
     method moved = moved
+    method deleted = deleted
     method item = item
     method pos = geomap#of_world self#xy
     method set wgs84 = 
@@ -133,6 +145,7 @@ class waypoint = fun (group:group) (name :string) ?(alt=0.) wgs84 ->
       and (xw0, yw0) = self#xy in
       self#move (xw-.xw0) (yw-.yw0)      
     method delete =
+      deleted <- true; (* BOF *)
       item#destroy ();
       label#destroy ()
     method zoom (z:float) =
diff --git a/sw/lib/ocaml/mapWaypoints.mli b/sw/lib/ocaml/mapWaypoints.mli
index a65b0ec020..51a847d4dd 100644
--- a/sw/lib/ocaml/mapWaypoints.mli
+++ b/sw/lib/ocaml/mapWaypoints.mli
@@ -55,6 +55,8 @@ class waypoint :
     method xy : float * float
     method zoom : float -> unit
     method moved : bool
+    method deleted : bool
+    method connect : (unit -> unit) -> unit
   end
 
 
diff --git a/sw/lib/ocaml/xmlEdit.ml b/sw/lib/ocaml/xmlEdit.ml
index 65105cc41b..181390a8df 100644
--- a/sw/lib/ocaml/xmlEdit.ml
+++ b/sw/lib/ocaml/xmlEdit.ml
@@ -37,7 +37,7 @@ open Printf
 type tag = string
 type attributes = (string * string) list
 type t = GTree.tree_store
-type node = t * Gtk.tree_path
+type node = GTree.tree_store * Gtk.tree_path
 
 let cols = new GTree.column_list
 let attribute = cols#add Gobject.Data.string
@@ -249,11 +249,11 @@ let root = fun (model:t) ->
   | Some i -> (model, model#get_path i)
 
   
-let attribs = fun ((model:t), path) ->
+let attribs = fun ((model, path):node) ->
   let row = model#get_iter path in
   model#get ~row ~column:attributes
 
-let set_attribs = fun ((model:t), path) attribs ->
+let set_attribs = fun ((model, path):node) attribs ->
   let row = model#get_iter path in
   model#set ~row ~column:attributes attribs
 
@@ -266,11 +266,11 @@ let attrib = fun node at ->
 	if String.uppercase a = at then v else loop avs in
   loop ats
 
-let tag = fun ((model:t), path) ->
+let tag = fun ((model, path):node) ->
   let row = model#get_iter path in
   model#get ~row ~column:tag_col
 
-let children = fun ((model:t), path) ->
+let children = fun ((model, path):node) ->
   let row = model#get_iter path in
   if model#iter_has_child row then
     let i = model#iter_children (Some row) in
@@ -282,7 +282,7 @@ let children = fun ((model:t), path) ->
   else
     []
 
-let rec xml_of_node = fun node -> 
+let rec xml_of_node = fun (node:node) -> 
   let attrs = attribs node
   and tag = tag node
   and children = List.map xml_of_node (children node) in
@@ -291,7 +291,7 @@ let rec xml_of_node = fun node ->
 let xml_of_view = fun (tree:t) ->
   xml_of_node (root tree)
   
-let child = fun ((model:t), path) (t:string) ->
+let child = fun ((model, path):node) (t:string) ->
   let row = model#get_iter path in
   if model#iter_has_child row then
     let i = model#iter_children (Some row) in
@@ -306,7 +306,7 @@ let child = fun ((model:t), path) (t:string) ->
     failwith (sprintf "XmlEdit.child: %s" t)
     
 
-let delete = fun ((model:t), path) ->
+let delete = fun (model, path) ->
   let row = model#get_iter path in
   if model#iter_is_valid row then
     ignore (model#remove row)
@@ -443,5 +443,4 @@ let create = fun dtd xml ->
   let _ = tree_view#drag#connect#motion ~callback:motion in
   let _ = tree_view#drag#connect#drop ~callback:drop in
 
-  window#show ();
-  tree_model
+  tree_model, window
diff --git a/sw/lib/ocaml/xmlEdit.mli b/sw/lib/ocaml/xmlEdit.mli
index c6acfcea82..88940327f2 100644
--- a/sw/lib/ocaml/xmlEdit.mli
+++ b/sw/lib/ocaml/xmlEdit.mli
@@ -39,10 +39,11 @@ type attributes = (string * string) list
 
 type event = Deleted | Modified of attributes | New_child of node
 
-val create : Dtd.dtd -> Xml.xml -> t
+val create : Dtd.dtd -> Xml.xml -> (t * GWindow.window)
 (** [create dtd xml] Opens a display of [xml] with contextual right button
 actions constrained by [dtd]. Returns the corresponding model. *)
 
+val xml_of_node : node -> Xml.xml
 val xml_of_view : t -> Xml.xml
 (** [xml_of_view v] Returns the XML displayed data structure *)
 
@@ -62,4 +63,3 @@ val add_child : node -> tag -> attributes -> node
 
 val connect : node -> (event -> unit) -> unit
 (** To be kept informed about modifications *)
-