diff --git a/conf/flight_plans/dynamic_sectors.xml b/conf/flight_plans/dynamic_sectors.xml new file mode 100644 index 0000000000..6f5b153ed9 --- /dev/null +++ b/conf/flight_plans/dynamic_sectors.xml @@ -0,0 +1,83 @@ + + + +
+#include "subsystems/datalink/datalink.h" +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd index 68be3712bb..1fc5ab2079 100644 --- a/conf/flight_plans/flight_plan.dtd +++ b/conf/flight_plans/flight_plan.dtd @@ -77,7 +77,8 @@ height CDATA #IMPLIED> +color CDATA #IMPLIED +type CDATA #IMPLIED> diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index f69feb7dea..7828f1980b 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -1332,7 +1332,8 @@ let listen_waypoint_moved = fun () -> try let w = ac.fp_group#get_wp wp_id in w#set_ground_alt ground_alt; - w#set ~altitude ~update:true geo + w#set ~altitude ~update:true geo; + ac.fp_group#update_sectors w#name with Not_found -> () (* Silently ignore unknown waypoints *) in diff --git a/sw/lib/ocaml/mapFP.ml b/sw/lib/ocaml/mapFP.ml index 633bec71ef..30061183ad 100644 --- a/sw/lib/ocaml/mapFP.ml +++ b/sw/lib/ocaml/mapFP.ml @@ -180,9 +180,11 @@ let georef_of_xml = fun xml -> let display_lines = fun ?group color (geomap:MapCanvas.widget) points -> let n = Array.length points in + let l = ref [] in for i = 0 to n - 1 do - ignore (geomap#segment ?group ~width:3 ~fill_color:color points.(i) points.((i+1)mod n)) - done + l := !l @ [(geomap#segment ?group ~width:3 ~fill_color:color points.(i) points.((i+1)mod n))] + done; + !l let space_regexp = Str.regexp " " let comma_regexp = Str.regexp "," @@ -209,7 +211,7 @@ let display_kml = fun ?group color geomap xml -> [Xml.PCData text] -> let points = Str.split space_regexp text in let points = List.map wgs84_of_kml_point points in - display_lines ?group color geomap (Array.of_list points) + ignore(display_lines ?group color geomap (Array.of_list points)) | _ -> failwith "coordinates expected" end @@ -257,14 +259,16 @@ class flight_plan = fun ?format_attribs ?editable ~show_moved geomap color fp_dt w in (* The sectors *) - let _ = + (* Parse sectors and store dynamic ones *) + let sectors = let waypoints = ExtXml.child xml "waypoints" in try - List.iter (fun x -> + List.fold_left (fun l x -> match String.lowercase (Xml.tag x) with "kml" -> let file = ExtXml.attrib x "file" in - display_kml ~group:wpts_group#group color geomap (ExtXml.parse_file (Env.flight_plans_path // file)) + display_kml ~group:wpts_group#group color geomap (ExtXml.parse_file (Env.flight_plans_path // file)); + l | "sector" -> let wgs84 = fun wp_name -> let wp_name = Xml.attrib wp_name "name" in @@ -275,10 +279,15 @@ class flight_plan = fun ?format_attribs ?editable ~show_moved geomap color fp_dt let points = List.map wgs84 (Xml.children x) in let points = Array.of_list points in let color_sector = ExtXml.attrib_or_default x "color" color in - display_lines ~group:wpts_group#group color_sector geomap points + 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 | _ -> failwith "Unknown sectors child") - (Xml.children (ExtXml.child xml "sectors")) - with Not_found -> () in + [] (Xml.children (ExtXml.child xml "sectors")) + with Not_found -> [] in (* The waypoints *) let _ = List.iter @@ -368,6 +377,24 @@ object method connect_activated = fun cb -> XmlEdit.connect_activated xml_tree_view cb + method update_sectors = fun wp_name -> + List.iter (fun (wps_name, segments, color) -> + let wp_in_sector = List.exists (fun name -> name = wp_name) wps_name in + if wp_in_sector then begin + (* Build WP array *) + let points = List.map (fun n -> let (_, w) = Hashtbl.find yaws n in w#pos) wps_name in + let points = Array.of_list points in + let segments = Array.of_list segments in + let n = Array.length points in + (* Update segments *) + for i = 0 to n - 1 do + let (x1, y1) = geomap#world_of points.(i) + and (x2, y2) = geomap#world_of (points.((i+1)mod n)) in + segments.(i)#set [`POINTS [|x1; y1; x2; y2 |]] + done + end + ) sectors + 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) | _ -> ()) diff --git a/sw/lib/ocaml/mapFP.mli b/sw/lib/ocaml/mapFP.mli index 010cbfb309..1d5a52a9cd 100644 --- a/sw/lib/ocaml/mapFP.mli +++ b/sw/lib/ocaml/mapFP.mli @@ -45,6 +45,7 @@ class flight_plan : method insert_path : (MapWaypoints.waypoint * float) list -> unit method highlight_stage : int -> int -> unit method connect_activated : (XmlEdit.node->unit) -> unit + method update_sectors : string -> unit end (** Extracts [lat0] and [Lon0] attributes *) diff --git a/sw/tools/generators/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml index 9394fa0d7f..4a2380a31a 100644 --- a/sw/tools/generators/gen_flight_plan.ml +++ b/sw/tools/generators/gen_flight_plan.ml @@ -698,8 +698,8 @@ let dummy_waypoint = []) - let print_inside_polygon = fun 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 @@ -723,25 +723,51 @@ let print_inside_polygon = fun pts -> in f 0 (Array.length layers - 1);; - - -let print_inside_sector = fun (s, pts) -> - lprintf "static inline bool_t %s(float _x, float _y) { \\\n" (inside_function s); +let print_inside_polygon_global = fun pts -> + lprintf "uint8_t i, j;\n"; + lprintf "bool_t c = FALSE;\n"; + (* build array of wp id *) + let (ids, _) = List.split pts in + lprintf "const uint8_t nb_pts = %d;\n" (List.length pts); + lprintf "const uint8_t wps_id[] = { %s };\n\n" (String.concat ", " ids); + (* start algo *) + lprintf "for (i = 0, j = nb_pts - 1; i < nb_pts; j = i++) {\n"; right (); - print_inside_polygon pts; + lprintf "if (((WaypointY(wps_id[i]) > _y) != (WaypointY(wps_id[j]) > _y)) &&\n"; + lprintf " (_x < (WaypointX(wps_id[j])-WaypointX(wps_id[i])) * (_y-WaypointY(wps_id[i])) / (WaypointY(wps_id[j])-WaypointY(wps_id[i])) + WaypointX(wps_id[i]))) {\n"; + right (); + lprintf "if (c == TRUE) { c = FALSE; } else { c = TRUE; }\n"; + left(); + lprintf "}\n"; + left(); + lprintf "}\n"; + lprintf "return c;\n" + + +type sector_type = StaticSector | DynamicSector + +let print_inside_sector = fun t (s, pts) -> + lprintf "static inline bool_t %s(float _x, float _y) {\n" (inside_function s); + right (); + begin + match t with + | StaticSector -> print_inside_polygon pts + | DynamicSector -> print_inside_polygon_global pts + end; left (); lprintf "}\n" -let parse_wpt_sector = fun waypoints xml -> +let parse_wpt_sector = fun indexes waypoints xml -> let sector_name = ExtXml.attrib xml "name" in let p2D_of = fun x -> let name = name_of x in try let wp = List.find (fun wp -> name_of wp = name) waypoints in + let i = get_index_waypoint name indexes in let x = float_attrib wp "x" and y = float_attrib wp "y" in - {G2D.x2D = x; G2D.y2D = y } + (i, {G2D.x2D = x; G2D.y2D = y }) with Not_found -> failwith (sprintf "Error: corner '%s' of sector '%s' not found" name sector_name) in @@ -870,16 +896,18 @@ let () = Xml2h.define "HOME_MODE_HEIGHT" (sof home_mode_height); Xml2h.define "MAX_DIST_FROM_HOME" (sof mdfh); + lprintf "\n#ifdef NAV_C\n\n"; + let index_of_waypoints = let i = ref (-1) in List.map (fun w -> incr i; (name_of w, !i)) waypoints in 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; + let sectors_type = List.map (fun x -> match ExtXml.attrib_or_default x "type" "static" with "dynamic" -> DynamicSector | _ -> StaticSector) sectors in + let sectors = List.map (parse_wpt_sector index_of_waypoints waypoints) sectors in + List.iter2 print_inside_sector sectors_type sectors; - lprintf "#ifdef NAV_C\n"; lprintf "\nstatic inline void auto_nav(void) {\n"; right (); List.iter print_exception global_exceptions;