Remove static sectors (paparazzi#2701)

* [sectors] Remove static sectors. * [sectors] Update conf. * [sectors] Keep "type" in DTD and issue error. * [generator] move inside sector functions out of NAV_C section Co-authored-by: Gautier Hattenberger <gautier.hattenberger@enac.fr>
FlyTheThings · May 4, 2021 · e965639 · e965639
1 parent 4a3ae9f
commit e965639
Show file tree

Hide file tree

Showing 13 changed files with 52 additions and 78 deletions.
diff --git a/conf/flight_plans/ENAC/crazyflie_multi_ranger_test.xml b/conf/flight_plans/ENAC/crazyflie_multi_ranger_test.xml
@@ -42,7 +42,7 @@ static inline void obstacle_detection_cb(uint8_t sender_id __attribute__((unused
       <corner name="_N3"/>
       <corner name="_N4"/>
     </sector>
-    <sector name="Survey" color="green" type="dynamic">
+    <sector name="Survey" color="green">
       <corner name="S1"/>
       <corner name="S2"/>
       <corner name="S3"/>

diff --git a/conf/flight_plans/ENAC/fish_outdoor.xml b/conf/flight_plans/ENAC/fish_outdoor.xml
@@ -23,13 +23,13 @@
     <waypoint name="_N4" x="-29.2" y="89.5"/>
   </waypoints>
   <sectors>
-    <sector color="red" name="Net" type="dynamic">
+    <sector color="red" name="Net">
       <corner name="_N1"/>
       <corner name="_N2"/>
       <corner name="_N3"/>
       <corner name="_N4"/>
     </sector>
-    <sector color="green" name="Safety" type="dynamic">
+    <sector color="green" name="Safety">
       <corner name="S1"/>
       <corner name="S2"/>
       <corner name="S3"/>

diff --git a/conf/flight_plans/ENAC/fish_voliere.xml b/conf/flight_plans/ENAC/fish_voliere.xml
@@ -28,7 +28,7 @@
       <corner name="_N3"/>
       <corner name="_N4"/>
     </sector>
-    <sector name="Safety" color="green" type="dynamic">
+    <sector name="Safety" color="green">
       <corner name="S1"/>
       <corner name="S2"/>
       <corner name="S3"/>

diff --git a/conf/flight_plans/OPENUAS/openuas_versatile_unified.xml b/conf/flight_plans/OPENUAS/openuas_versatile_unified.xml
@@ -91,7 +91,7 @@ Should be unified for Hybrid, FW and rotorcraft
   <sectors>
     <!-- ***** -->
     <!-- since hidden corners cannot be accidentaly changed -->
-    <sector color="yellow" name="MissionBoundary" type="dynamic">
+    <sector color="yellow" name="MissionBoundary">
       <corner name="_MB1"/>
       <corner name="_MB2"/>
       <corner name="_MB3"/>
@@ -101,14 +101,14 @@ Should be unified for Hybrid, FW and rotorcraft
       <corner name="_MB7"/>
       <corner name="_MB8"/>
     </sector>
-    <sector color="purple" name="SearchArea" type="dynamic">
+    <sector color="purple" name="SearchArea">
       <corner name="SA1"/>
       <corner name="SA2"/>
       <corner name="SA3"/>
       <corner name="SA4"/>
       <corner name="SA5"/>
     </sector>
-    <sector color="lightblue" name="Square" type="dynamic">
+    <sector color="lightblue" name="Square">
       <corner name="Q1"/>
       <corner name="Q2"/>
       <corner name="Q3"/>

diff --git a/conf/flight_plans/competitions/IMAV2018_carto.xml b/conf/flight_plans/competitions/IMAV2018_carto.xml
@@ -72,7 +72,7 @@ static inline void set_expo(float e) {
     <waypoint name="ZBDIR" x="-229.4" y="1.7"/>
   </waypoints>
   <sectors>
-    <sector color="red" name="FlyZone" type="dynamic">
+    <sector color="red" name="FlyZone">
       <corner name="_NFZ1"/>
       <corner name="_NFZ2"/>
       <corner name="_NFZ3"/>
@@ -81,7 +81,7 @@ static inline void set_expo(float e) {
       <corner name="_NFZ5"/>
       <corner name="_NFZ6"/>
     </sector>
-    <sector color="green" name="MAP" type="dynamic">
+    <sector color="green" name="MAP">
       <corner name="_MAP1"/>
       <corner name="_MAP2"/>
       <corner name="_MAP3"/>

diff --git a/conf/flight_plans/competitions/IMAV2019_carto.xml b/conf/flight_plans/competitions/IMAV2019_carto.xml
@@ -71,7 +71,7 @@ static inline void set_expo(float e) {
     <waypoint name="ZBDIR" x="-83.3" y="-105.3"/>
   </waypoints>
   <sectors>
-    <sector color="red" name="FlyZone" type="dynamic">
+    <sector color="red" name="FlyZone">
       <corner name="_NFZ1"/>
       <corner name="_NFZ2"/>
       <corner name="_NFZ3"/>
@@ -80,7 +80,7 @@ static inline void set_expo(float e) {
       <corner name="_NFZ5"/>
       <corner name="_NFZ6"/>
     </sector>
-    <sector color="green" name="MAP" type="dynamic">
+    <sector color="green" name="MAP">
       <corner name="_MAP1"/>
       <corner name="_MAP2"/>
       <corner name="_MAP3"/>

diff --git a/conf/flight_plans/competitions/IMAV2019_drop.xml b/conf/flight_plans/competitions/IMAV2019_drop.xml
@@ -60,7 +60,7 @@ static inline bool delay_test_gf(bool test, int delay) {
     <waypoint name="_HERE" x="86.0" y="-117.2"/>
   </waypoints>
   <sectors>
-    <sector color="red" name="FlyZone" type="dynamic">
+    <sector color="red" name="FlyZone">
       <corner name="_NFZ1"/>
       <corner name="_NFZ2"/>
       <corner name="_NFZ3"/>
@@ -69,7 +69,7 @@ static inline bool delay_test_gf(bool test, int delay) {
       <corner name="_NFZ5"/>
       <corner name="_NFZ6"/>
     </sector>
-    <sector color="orange" name="Search" type="dynamic">
+    <sector color="orange" name="Search">
       <corner name="S1"/>
       <corner name="S2"/>
       <corner name="S3"/>

diff --git a/conf/flight_plans/dynamic_sectors.xml b/conf/flight_plans/dynamic_sectors.xml
@@ -18,7 +18,7 @@
     <waypoint name="CLIMB" x="-122.5" y="35.4"/>
   </waypoints>
   <sectors>
-    <sector name="Square" type="dynamic">
+    <sector name="Square">
       <corner name="C1"/>
       <corner name="C2"/>
       <corner name="C3"/>

diff --git a/conf/flight_plans/rotorcraft_joystick_enac.xml b/conf/flight_plans/rotorcraft_joystick_enac.xml
@@ -36,7 +36,7 @@ static inline void joystick_handler(uint8_t sender_id __attribute__((unused)), i
       <corner name="_N3"/>
       <corner name="_N4"/>
     </sector>
-    <sector name="Survey" color="green" type="dynamic">
+    <sector name="Survey" color="green">
       <corner name="S1"/>
       <corner name="S2"/>
       <corner name="S3"/>

diff --git a/conf/flight_plans/rover_optitrack_PIR.xml b/conf/flight_plans/rover_optitrack_PIR.xml
@@ -27,7 +27,7 @@
       <corner name="_N3"/>
       <corner name="_N4"/>
     </sector>
-    <sector name="Survey" color="green" type="dynamic">
+    <sector name="Survey" color="green">
       <corner name="_S1"/>
       <corner name="_S2"/>
       <corner name="_S3"/>

diff --git a/conf/flight_plans/tudelft/delft_bebop.xml b/conf/flight_plans/tudelft/delft_bebop.xml
@@ -23,7 +23,7 @@
     <waypoint name="_G4" x="2.5" y="-46.9"/>
   </waypoints>
   <sectors>
-    <sector color="red" name="Flight_Area" type="dynamic">
+    <sector color="red" name="Flight_Area">
       <corner name="_G1"/>
       <corner name="_G2"/>
       <corner name="_G3"/>

diff --git a/sw/lib/ocaml/mapFP.ml b/sw/lib/ocaml/mapFP.ml
@@ -261,7 +261,7 @@ class flight_plan = fun ?format_attribs ?editable ~show_moved geomap color fp_dt
       w in
 
   (* The sectors *)
-  (* Parse sectors and store dynamic ones *)
+  (* Parse and store sectors *)
   let sectors =
     let waypoints = ExtXml.child xml "waypoints" in
     try
@@ -283,10 +283,7 @@ class flight_plan = fun ?format_attribs ?editable ~show_moved geomap color fp_dt
             let color_sector = ExtXml.attrib_or_default x "color" color in
             let segments = display_lines ~group:wpts_group#group color_sector geomap points in
             let wp_names = List.map (fun wp -> Xml.attrib wp "name") (Xml.children x) in
-            if ExtXml.attrib_or_default x "type" "" = "dynamic" then
-              [(wp_names, segments, color_sector)] @ l
-            else
-              l
+            [(wp_names, segments, color_sector)] @ l
           | _ -> failwith "Unknown sectors child")
       [] (Xml.children (ExtXml.child xml "sectors"))
     with Not_found -> [] in

diff --git a/sw/tools/generators/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml
@@ -790,38 +790,12 @@ let dummy_waypoint =
                [])
 
 
-let print_inside_polygon = fun out pts ->
-  let (_, pts) = List.split pts in
-  let layers = Geometry_2d.slice_polygon (Array.of_list pts) in
-  let rec f = fun i j ->
-    if i = j then
-      let {G2D.top=yl; left_side=(xg, ag); right_side=(xd, ad)} = layers.(i) in
-      if xg > xd then begin
-        lprintf out "return FALSE;\n"
-      end else begin
-        if ad <> 0. || ag <> 0. then
-          lprintf out "float dy = _y - %.1f;\n" yl;
-        let dy_times = fun f -> if f = 0. then "" else sprintf "+dy*%f" f in
-        lprintf out "return (%.1f%s<= _x && _x <= %.1f%s);\n" xg (dy_times ag) xd (dy_times ad)
-      end
-    else
-      let ij2 = (i+j) / 2 in
-      let yl = layers.(ij2).G2D.top in
-      lprintf out "if (_y <= %.1f) {\n" yl;
-      right (); f i ij2; left ();
-      lprintf out "} else {\n";
-      right (); f (ij2+1) j; left ();
-      lprintf out "}\n"
-  in
-  f 0 (Array.length layers - 1);;
-
-let print_inside_polygon_global = fun out pts ->
+let print_inside_polygon_global = fun out pts name ->
   lprintf out "uint8_t i, j;\n";
   lprintf out "bool c = false;\n";
   (* build array of wp id *)
-  let (ids, _) = List.split pts in
-  lprintf out "const uint8_t nb_pts = %d;\n" (List.length pts);
-  lprintf out "const uint8_t wps_id[] = { %s };\n\n" (String.concat ", " ids);
+  lprintf out "const uint8_t nb_pts = %s_NB;\n" name;
+  lprintf out "const uint8_t wps_id[] = %s;\n\n" name;
   (* start algo *)
   lprintf out "for (i = 0, j = nb_pts - 1; i < nb_pts; j = i++) {\n";
   right ();
@@ -836,18 +810,16 @@ let print_inside_polygon_global = fun out pts ->
   lprintf out "return c;\n"
 
 
-type sector_type = StaticSector | DynamicSector
-
-let print_inside_sector = fun out t (s, pts) ->
+let print_inside_sector = fun out (s, pts) ->
+  let (ids, _) = List.split pts in
+  let name = "SECTOR_"^(Compat.uppercase_ascii s) in
+  Xml2h.define_out out (name^"_NB") (string_of_int (List.length pts));
+  Xml2h.define_out out name ("{ "^(String.concat ", " ids)^" }");
   lprintf out "static inline bool %s(float _x, float _y) {\n" (inside_function s);
   right ();
-  begin
-    match t with
-    | StaticSector -> print_inside_polygon out pts
-    | DynamicSector -> print_inside_polygon_global out pts
-  end;
+  print_inside_polygon_global out pts name;
   left ();
-  lprintf out "}\n"
+  lprintf out "}\n\n"
 
 
 let parse_wpt_sector = fun indexes waypoints xml ->
@@ -1110,13 +1082,7 @@ let print_flight_plan_h = fun xml ref0 xml_file out_file ->
       _ -> ()
   end;
 
-  (* start "C" part *)
-  lprintf out "\n#ifdef NAV_C\n\n";
-
-  (* print variables and ABI initialization *)
-  List.iter (fun v -> print_var_impl out abi_msgs v) variables;
   lprintf out "\n";
-  print_auto_init_bindings out abi_msgs variables;
 
   (* index of waypoints *)
   let index_of_waypoints =
@@ -1126,12 +1092,32 @@ let print_flight_plan_h = fun xml ref0 xml_file out_file ->
   (* print sectors *)
   let sectors_element = try ExtXml.child xml "sectors" with Not_found -> Xml.Element ("", [], []) in
   let sectors = List.filter (fun x -> Compat.lowercase_ascii (Xml.tag x) = "sector") (Xml.children sectors_element) in
-  let sectors_type = List.map (fun x -> match ExtXml.attrib_or_default x "type" "static" with "dynamic" -> DynamicSector | _ -> StaticSector) sectors in
+  List.iter (fun x -> match ExtXml.attrib_opt x "type" with
+      Some _ -> failwith "Error: attribute \"type\" on flight plan tag \"sector\" is deprecated and must be removed. All sectors are now dynamics.\n"
+    | _ -> ()
+    ) sectors;
   let sectors = List.map (parse_wpt_sector index_of_waypoints waypoints) sectors in
-  List.iter2 (print_inside_sector out) sectors_type sectors;
+  List.iter (print_inside_sector out) sectors;
+
+  (* geofencing sector *)
+  begin
+    try
+      let geofence_sector = Xml.attrib xml "geofence_sector" in
+      lprintf out "\n#define InGeofenceSector(_x, _y) %s(_x, _y)\n" (inside_function geofence_sector)
+    with
+        _ -> ()
+  end;
+
+  (* start "C" part *)
+  lprintf out "\n#ifdef NAV_C\n\n";
+
+  (* print variables and ABI initialization *)
+  List.iter (fun v -> print_var_impl out abi_msgs v) variables;
+  lprintf out "\n";
+  print_auto_init_bindings out abi_msgs variables;
 
   (* print main flight plan state machine *)
-  lprintf out "\nstatic inline void auto_nav(void) {\n";
+  lprintf out "static inline void auto_nav(void) {\n";
   right ();
   List.iter (print_exception out) global_exceptions;
   lprintf out "switch (nav_block) {\n";
@@ -1144,15 +1130,6 @@ let print_flight_plan_h = fun xml ref0 xml_file out_file ->
   lprintf out "}\n";
   lprintf out "#endif // NAV_C\n";
 
-  (* geofencing sector FIXME why here ? *)
-  begin
-    try
-      let geofence_sector = Xml.attrib xml "geofence_sector" in
-      lprintf out "#define InGeofenceSector(_x, _y) %s(_x, _y)\n" (inside_function geofence_sector)
-    with
-        _ -> ()
-  end;
-
   Xml2h.finish_out out h_name;
   close_out out