Handling of convex, clockwise described, sectors

conf/flight_plans/flight_plan.dtd

@@ -1,6 +1,6 @@
 <!-- Paparazzi flight plan DTD -->
 
-<!ELEMENT flight_plan (header?, waypoints,include*,exceptions?,blocks)>
+<!ELEMENT flight_plan (header?,waypoints,sectors?,include*,exceptions?,blocks)>
 
 <!ELEMENT procedure (param*,waypoints,exceptions?,blocks)>
 
@@ -9,6 +9,11 @@
 <!ELEMENT waypoints (waypoint+)>
 <!ELEMENT waypoint EMPTY>
 
+<!ELEMENT sectors (sector|kml)*>
+<!ELEMENT sector (corner+)>
+<!ELEMENT corner EMPTY>
+<!ELEMENT kml EMPTY>
+
 <!ELEMENT exceptions (exception*)>
 
 <!ELEMENT blocks (block+)>
@@ -63,6 +68,17 @@ lat CDATA #IMPLIED
 lon CDATA #IMPLIED
 alt CDATA #IMPLIED>
 
+<!ATTLIST sectors>
+
+<!ATTLIST sector
+name CDATA #REQUIRED>
+
+<!ATTLIST corner
+name CDATA #REQUIRED>
+
+<!ATTLIST kml
+file CDATA #REQUIRED>
+
 <!ATTLIST block 
 name CDATA #REQUIRED
 strip_button CDATA #IMPLIED

conf/flight_plans/sectors.xml

@@ -4,30 +4,30 @@
 <sectors>
   <sector name="airexpo">
     <polygon>
-     <point pos="WGS84 43.4491 1.2674"/>
-     <point pos="WGS84 43.4520 1.2637"/>
-     <point pos="WGS84 43.4519 1.2592"/>
-     <point pos="WGS84 43.4504 1.2582"/>
-     <point pos="WGS84 43.4472 1.2618"/>
      <point pos="WGS84 43.4475 1.2663"/>
+     <point pos="WGS84 43.4472 1.2618"/>
+     <point pos="WGS84 43.4504 1.2582"/>
+     <point pos="WGS84 43.4519 1.2592"/>
+     <point pos="WGS84 43.4520 1.2637"/>
+     <point pos="WGS84 43.4491 1.2674"/>
     </polygon>
   </sector>
 
   <sector name="survey">
     <polygon>
-      <point pos="WGS84 43.449442	1.264670"/>
+      <point pos="WGS84 43.465789	1.271318"/>
-      <point pos="WGS84 43.450918	1.260512"/>
+      <point pos="WGS84 43.465429	1.277119"/>
-      <point pos="WGS84 43.450290	1.260029"/>
+      <point pos="WGS84 43.461804	1.276055"/>
-      <point pos="WGS84 43.448759	1.264170"/>
+      <point pos="WGS84 43.462949	1.268933"/>
     </polygon>
   </sector>
 
   <sector name="latrape">
     <polygon>
-      <point pos="WGS84 43.239970	1.333672"/>
-      <point pos="WGS84 43.241134	1.298982"/>
-      <point pos="WGS84 43.232172	1.299416"/>
       <point pos="WGS84 43.232776	1.334940"/>
+      <point pos="WGS84 43.232172	1.299416"/>
+      <point pos="WGS84 43.241134	1.298982"/>
+      <point pos="WGS84 43.239970	1.333672"/>
     </polygon>
   </sector>
 </sectors>

conf/flight_plans/versatile.xml

@@ -15,7 +15,22 @@
     <waypoint name="TOD" x="-150" y="0"/>
     <waypoint alt="185" name="TD" x="80" y="20"/>
     <waypoint name="BASELEG" x="400" y="300"/>
+    <waypoint name="_1" x="-119.7" y="401.3"/>
+    <waypoint name="_2" x="348.7" y="351.5"/>
+    <waypoint name="_3" x="299.7" y="-144.6"/>
+    <waypoint name="_4" x="-453.7" y="27.8"/>
   </waypoints>
+  <sectors>
+    <sector name="Muret">
+      <corner name="_1"/>
+      <corner name="_2"/>
+      <corner name="_3"/>
+      <corner name="_4"/>
+    </sector>
+  </sectors>
+  <exceptions>
+    <exception cond="! InsideMuret(estimator_x, estimator_y)" deroute="circle 1"/>
+  </exceptions>
   <blocks>
     <block name="wait GPS">
       <while cond="!GpsFixValid()"/>

sw/lib/ocaml/geometry_2d.ml

@@ -902,13 +902,13 @@ let slice_polygon = fun poly ->
   and last_y = ref (poly.(!bottom).y2D) in
   while !i < n do
     while poly.(!d).y2D = !last_y do
-      let d' = next !d in
+      let d' = prev !d in
       alpha_d := slope !d d';
       d := d';
       incr i
     done;
     while poly.(!g).y2D = !last_y do
-      let g' = prev !g in
+      let g' = next !g in
       alpha_g := slope !g g';
       g := g';
       incr i

sw/lib/ocaml/mapFP.ml

@@ -106,7 +106,8 @@ let new_wp = fun ?(editable = false) (geomap:MapCanvas.widget) xml_tree waypoint
   let wgs84 = Latlong.of_utm WGS84 (utm_add utm_ref (x, y)) in
   let alt = try float_attrib "alt" with _ -> alt in
   let name = XmlEdit.attrib node "name" in
-  let wp = MapWaypoints.waypoint waypoints ~name ~alt wgs84 in
+  let show = editable || name.[0] <> '_' in
+  let wp = MapWaypoints.waypoint ~show waypoints ~name ~alt wgs84 in
   geomap#register_to_fit (wp:>MapCanvas.geographic);
   XmlEdit.connect node (update_wp utm_ref wp);
   XmlEdit.connect node (update_wp_refs (ref name) xml_tree); 
@@ -171,6 +172,29 @@ class flight_plan = fun ?format_attribs ?editable ~show_moved geomap color fp_dt
 	end)
       (XmlEdit.children xml_wpts) in
 
+  (* The sectors *)
+  let _ =
+    if editable = None then
+    let waypoints = ExtXml.child xml "waypoints" in
+    try
+      List.iter (fun x ->
+	if String.lowercase (Xml.tag x) = "sector" then
+	  let wgs84 = fun wp_name ->
+	    let wp_name = Xml.attrib wp_name "name" in
+	    let select = fun wp -> Xml.attrib wp "name" = wp_name in
+	    let wp = ExtXml.child waypoints ~select "waypoint" in
+	    let x = float_attr wp "x"
+	    and y = float_attr wp "y" in
+	    of_utm WGS84 (utm_add utm0 (x, y)) in
+	  let points = List.map wgs84 (Xml.children x) in
+	  let points = Array.of_list points in
+	  let n = Array.length points in
+	  for i = 0 to n - 1 do
+	    ignore (geomap#segment ~width:3 ~fill_color:color points.(i) points.((i+1)mod n))
+	  done)
+	(Xml.children (ExtXml.child xml "sectors"))
+    with Not_found -> () in
+
   (** Expands the blocks *)
   let _ =
     XmlEdit.expand_node xml_tree_view xml_root;

sw/tools/gen_flight_plan.ml

@@ -635,6 +635,16 @@ let parse_sector = fun rel_utm_of_wgs84 x ->
       (ExtXml.attrib x "name", pts)
   | s -> failwith (sprintf "sector: %s not yet" s)
 
+let parse_wpt_sector = fun waypoints xml ->
+  let p2D_of = fun x ->
+    let name = name_of x in
+    let wp = List.find (fun wp -> name_of wp = name) waypoints in
+    let x = float_attrib wp "x"
+    and y = float_attrib wp "y" in
+    {G2D.x2D = x; G2D.y2D = y } in
+  (ExtXml.attrib xml "name", List.map p2D_of (Xml.children xml))
+  
+
 
 let _ =
   let xml_file = ref "fligh_plan.xml"
@@ -736,6 +746,12 @@ let _ =
 	List.iter print_inside_sector sectors
       end;
 
+      let sectors_element = try ExtXml.child xml "sectors" with Not_found -> Xml.Element ("", [], []) in
+      let sectors = List.filter (fun x -> String.lowercase (Xml.tag x) = "sector") (Xml.children sectors_element) in
+      let sectors =  List.map (parse_wpt_sector waypoints) sectors in
+      List.iter print_inside_sector sectors;
+      
+      
       lprintf "#ifdef NAV_C\n";
       lprintf "\nstatic inline void auto_nav(void) {\n";
       right ();