From e9656391a2609066644a147effb3ef90024ee4cf Mon Sep 17 00:00:00 2001 From: Fabien-B Date: Tue, 4 May 2021 16:51:34 +0200 Subject: [PATCH] Remove static sectors (#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 --- .../ENAC/crazyflie_multi_ranger_test.xml | 2 +- conf/flight_plans/ENAC/fish_outdoor.xml | 4 +- conf/flight_plans/ENAC/fish_voliere.xml | 2 +- .../OPENUAS/openuas_versatile_unified.xml | 6 +- .../competitions/IMAV2018_carto.xml | 4 +- .../competitions/IMAV2019_carto.xml | 4 +- .../competitions/IMAV2019_drop.xml | 4 +- conf/flight_plans/dynamic_sectors.xml | 2 +- .../flight_plans/rotorcraft_joystick_enac.xml | 2 +- conf/flight_plans/rover_optitrack_PIR.xml | 2 +- conf/flight_plans/tudelft/delft_bebop.xml | 2 +- sw/lib/ocaml/mapFP.ml | 7 +- sw/tools/generators/gen_flight_plan.ml | 89 +++++++------------ 13 files changed, 52 insertions(+), 78 deletions(-) diff --git a/conf/flight_plans/ENAC/crazyflie_multi_ranger_test.xml b/conf/flight_plans/ENAC/crazyflie_multi_ranger_test.xml index 33d85b41be5..3de84b393ff 100644 --- 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 - + diff --git a/conf/flight_plans/ENAC/fish_outdoor.xml b/conf/flight_plans/ENAC/fish_outdoor.xml index a2f3c707460..0d4744b3eae 100644 --- a/conf/flight_plans/ENAC/fish_outdoor.xml +++ b/conf/flight_plans/ENAC/fish_outdoor.xml @@ -23,13 +23,13 @@ - + - + diff --git a/conf/flight_plans/ENAC/fish_voliere.xml b/conf/flight_plans/ENAC/fish_voliere.xml index 44241f3067e..86d1d9a3470 100644 --- a/conf/flight_plans/ENAC/fish_voliere.xml +++ b/conf/flight_plans/ENAC/fish_voliere.xml @@ -28,7 +28,7 @@ - + diff --git a/conf/flight_plans/OPENUAS/openuas_versatile_unified.xml b/conf/flight_plans/OPENUAS/openuas_versatile_unified.xml index de6897d2822..4a266c4bea4 100644 --- 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 - + @@ -101,14 +101,14 @@ Should be unified for Hybrid, FW and rotorcraft - + - + diff --git a/conf/flight_plans/competitions/IMAV2018_carto.xml b/conf/flight_plans/competitions/IMAV2018_carto.xml index 213a68d13f7..54be4bdfe4d 100644 --- 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) { - + @@ -81,7 +81,7 @@ static inline void set_expo(float e) { - + diff --git a/conf/flight_plans/competitions/IMAV2019_carto.xml b/conf/flight_plans/competitions/IMAV2019_carto.xml index deef0c4ef29..a71ce1f283b 100644 --- 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) { - + @@ -80,7 +80,7 @@ static inline void set_expo(float e) { - + diff --git a/conf/flight_plans/competitions/IMAV2019_drop.xml b/conf/flight_plans/competitions/IMAV2019_drop.xml index 42ba79ce900..1b8cf20759c 100644 --- 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) { - + @@ -69,7 +69,7 @@ static inline bool delay_test_gf(bool test, int delay) { - + diff --git a/conf/flight_plans/dynamic_sectors.xml b/conf/flight_plans/dynamic_sectors.xml index 3f7b9669e3e..e7e150d49fc 100644 --- a/conf/flight_plans/dynamic_sectors.xml +++ b/conf/flight_plans/dynamic_sectors.xml @@ -18,7 +18,7 @@ - + diff --git a/conf/flight_plans/rotorcraft_joystick_enac.xml b/conf/flight_plans/rotorcraft_joystick_enac.xml index e4b23e403c0..dfc02ac3f47 100644 --- 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 - + diff --git a/conf/flight_plans/rover_optitrack_PIR.xml b/conf/flight_plans/rover_optitrack_PIR.xml index e5031e0d44e..1888ad15045 100644 --- a/conf/flight_plans/rover_optitrack_PIR.xml +++ b/conf/flight_plans/rover_optitrack_PIR.xml @@ -27,7 +27,7 @@ - + diff --git a/conf/flight_plans/tudelft/delft_bebop.xml b/conf/flight_plans/tudelft/delft_bebop.xml index f1516d95c27..9c1b4181ed9 100644 --- a/conf/flight_plans/tudelft/delft_bebop.xml +++ b/conf/flight_plans/tudelft/delft_bebop.xml @@ -23,7 +23,7 @@ - + diff --git a/sw/lib/ocaml/mapFP.ml b/sw/lib/ocaml/mapFP.ml index 15f35fe742d..305b364b379 100644 --- 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 index 4045f3d36e3..de8078e7e64 100644 --- 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