diff --git a/sw/lib/ocaml/geometry_2d.ml b/sw/lib/ocaml/geometry_2d.ml index 8e20a2b3c3..d481c730a4 100644 --- a/sw/lib/ocaml/geometry_2d.ml +++ b/sw/lib/ocaml/geometry_2d.ml @@ -841,6 +841,29 @@ let norm_heading_rad a = let oposite_heading_rad rad = norm_heading_rad (rad +. m_pi) + + +let sign = fun f -> f /. abs_float f + +let arc_segment = fun p0 p1 p2 radius -> + (* C: center of the arc *) + let p0p1 = vect_make p0 p1 + and p0p2 = vect_make p0 p2 in + let u = vect_normalize p0p1 in + let v = vect_rotate_90 u in + let s = sign (cross_product p0p1 p0p2) in + let c = vect_add p1 (vect_mul_scal v (s*.radius)) in + + (* F first point of the segment *) + let d_c2 = distance c p2 in + assert (radius < d_c2); + let alpha_2cf = -. s *. acos (radius /. d_c2) + and alpha_c2 = (cart2polar (vect_make c p2)).theta2D in + let alpha_cf = alpha_c2 +. alpha_2cf in + let f = vect_add c (polar2cart {theta2D=alpha_cf;r2D=radius}) in + + (c, f, s) + (* =============================== FIN ========================================= *) diff --git a/sw/lib/ocaml/geometry_2d.mli b/sw/lib/ocaml/geometry_2d.mli index cbce16edb4..feaff25ad1 100644 --- a/sw/lib/ocaml/geometry_2d.mli +++ b/sw/lib/ocaml/geometry_2d.mli @@ -301,3 +301,11 @@ val heading_of_to_angle_rad : float -> float val norm_angle_rad : float -> float val norm_heading_rad : float -> float val oposite_heading_rad : float -> float + +(** {6 Misc} *) + +val arc_segment : pt_2D -> pt_2D -> pt_2D -> float -> pt_2D*pt_2D*float +(** [arc_segment p0 p1 p2 r] Returns [(c,f,s)]. Point [c] is the center of the +arc of radius [r], tangent to segment [po]-[p1] in [p1]. Point [f] is such +that [f]-[p2] is tangent to the arc. [s] is 1. if [c] is on the left, [-1] +else *) diff --git a/sw/tools/fp_proc.ml b/sw/tools/fp_proc.ml index bb029f0486..7510c71af9 100644 --- a/sw/tools/fp_proc.ml +++ b/sw/tools/fp_proc.ml @@ -24,9 +24,9 @@ * *) -open Fp_syntax +module G2D = Geometry_2d -let distance = fun (x1,y1) (x2,y2) -> sqrt ((x1-.x2)**2.+.(y1-.y2)**2.) +open Fp_syntax let nop_stage = Xml.Element ("while", ["cond","FALSE"],[]) @@ -73,7 +73,7 @@ let rotate_expression = fun a expression -> Call(op, [rot e1; rot e2]) | CallOperator(op, [e]) -> CallOperator(op, [rot e]) | CallOperator(op, [e1; e2]) -> CallOperator(op, [rot e1; rot e2]) - | CallOperator(op, _) -> failwith "fp_proc: Operator should be unary or binary" + | CallOperator(_op, _) -> failwith "fp_proc: Operator should be unary or binary" | _ -> e in rot expression @@ -207,15 +207,6 @@ let transform_block = fun prefix reroutes affine env xml -> List.map (transform_stage prefix reroutes affine env) (Xml.children xml)) -let check_params = fun params env -> - List.iter - (fun p -> - if not (List.mem_assoc p env) then begin - Printf.fprintf stderr "Parameter '%s' is missing\n" p; - exit 1 - end) - params - let parse_include = fun dir include_xml -> let f = Filename.concat dir (ExtXml.attrib include_xml "procedure") in let proc_name = ExtXml.attrib include_xml "name" in @@ -399,61 +390,38 @@ let process_relative_waypoints = fun xml -> replace_children xml ["waypoints", new_waypoints; "blocks", blocks] -let sign = fun f -> f /. abs_float f (** Path preprocessing: a list of waypoints is translated into an alternance of route and circle stages *) -let unit_vect = fun (x1,y1) (x2,y2) -> - let d = distance (x1,y1) (x2,y2) in - ((x2-.x1)/.d, (y2-.y1)/.d) -let prod_vect = fun (x1, y1) (x2,y2) -> - x1*.y2 -. x2*.y1 -let ortho = fun (x, y) -> (-.y, x) - - let compile_path = fun wpts radius last_last last ps rest -> - let rec loop = fun (x0, y0) last ps -> + let rec loop = fun p0 last ps -> match ps with [] -> rest | p::ps -> let wp = Xml.attrib p "wp" in let (x1, y1) = coords_of_wp_name last wpts and (x2, y2) = coords_of_wp_name wp wpts in - - (* C: center of the arc *) - let u = unit_vect (x0,y0) (x1, y1) in - let xv, yv = ortho u in - let s = sign (prod_vect (x1 -. x0, y1 -. y0) (x2 -. x0, y2 -. y0)) in - let xc = x1 +. s *. xv *. radius - and yc = y1 +. s *. yv *. radius in - - (* F first point of the segment *) - let d_c2 = distance (xc, yc) (x2, y2) in - assert (radius < d_c2); - let alpha_2cf = -. s *. acos (radius /. d_c2) - and alpha_c2 = atan2 (y2-.yc) (x2-.xc) in - let alpha_cf = alpha_c2 +. alpha_2cf in - let xf = xc +. radius *. cos alpha_cf - and yf = yc +. radius *. sin alpha_cf in - + let p1 = {G2D.x2D=x1; y2D=y1} + and p2 = {G2D.x2D=x2; y2D=y2} in + let (c, f, s) = G2D.arc_segment p0 p1 p2 radius in + (* Angle between P1 and F *) - let alpha_c1 = atan2 (y1-.yc) (x1-.xc) in + let alpha_cf = (G2D.cart2polar (G2D.vect_make c f)).G2D.theta2D in + let alpha_c1 = (G2D.cart2polar (G2D.vect_make c p1)).G2D.theta2D in let alpha_fc1 = norm_2pi (-. s *. (alpha_c1 -. alpha_cf)) in let theta = abs_float (alpha_fc1) /. 2. /. pi in (* C relative to P1, F relative to P2 *) - let alpha_1c = alpha_c1 -. pi - and alpha_2f = atan2 (yf-.y2) (xf-.x2) - and d_f2 = distance (xf,yf) (x2,y2) in + let alpha_1c = (G2D.cart2polar (G2D.vect_make p1 c)).G2D.theta2D + and alpha_2f = (G2D.cart2polar (G2D.vect_make p2 f)).G2D.theta2D + and d_f2 = G2D.distance f p2 in let c_last_qdr= norm_2pi (pi /. 2. -. alpha_1c) and f_wp_qdr= norm_2pi (pi /. 2. -. alpha_2f) in let until = Printf.sprintf "(circle_count > %f)" theta in let sradius = string_of_float (-. s *. radius) in - Printf.fprintf stderr "%s->%s: xf=%.0f yf=%.0f s=%.0f ac1=%.1f acf=%.1f t=%.1f\n" last wp xf yf s alpha_c1 alpha_cf theta; - Xml.Element ("circle", ["wp", last; "wp_qdr", string_of_float ((Rad>>Deg)c_last_qdr); "wp_dist", string_of_float radius; @@ -465,7 +433,7 @@ let compile_path = fun wpts radius last_last last ps rest -> "hmode", "route"; "approaching_time", "2"; "wp", wp], []):: - loop (xf,yf) wp ps in + loop f wp ps in loop last_last last ps;; @@ -484,8 +452,9 @@ let stage_process_path = fun wpts stage rest -> "hmode","route"; "wp", wp2], []):: (* Here starts the actual translation *) - let x1y1 = coords_of_wp_name wp1 wpts in - compile_path wpts radius x1y1 wp2 ps rest + let x1, y1 = coords_of_wp_name wp1 wpts in + let p1 = {Geometry_2d.x2D=x1; y2D=y1} in + compile_path wpts radius p1 wp2 ps rest else stage::rest diff --git a/sw/tools/gen_airframe.ml b/sw/tools/gen_airframe.ml index 5c5f4b9a84..305b0ee1c3 100644 --- a/sw/tools/gen_airframe.ml +++ b/sw/tools/gen_airframe.ml @@ -36,7 +36,6 @@ type control = { failsafe_value : int; foo : int } let fos = float_of_string let sof = fun x -> if mod_float x 1. = 0. then Printf.sprintf "%.0f" x else string_of_float x -let soi = string_of_int let define_macro name n x = let a = fun s -> ExtXml.attrib x s in diff --git a/sw/tools/gen_control.ml b/sw/tools/gen_control.ml index 7292cb2a3d..0e2733bc8b 100644 --- a/sw/tools/gen_control.ml +++ b/sw/tools/gen_control.ml @@ -125,7 +125,7 @@ let print_loop_declaration = fun mode lp -> end; nl() -let print_loop_code = fun mode lp -> +let print_loop_code = fun _mode lp -> printf " {\n"; printf " %s err = %s - %s;\n" lp.data_type lp.measure lp.setpoint; if is_d lp.loop_type then diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index 323e35d387..b240d14277 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -56,12 +56,6 @@ let parsed_attrib = fun xml a -> let pi = atan 1. *. 4. -let radE8_of_deg = fun d -> - d /. 180. *. pi *. 1e8 - -let rad_of_deg = fun d -> - d /. 180. *. pi - let deg_of_rad = fun r -> r /. pi *. 180. @@ -80,7 +74,6 @@ let lprintf = fun f -> printf f let float_attrib = fun xml a -> float_of_string (Xml.attrib xml a) -let int_attrib = fun xml a -> int_of_string (Xml.attrib xml a) let name_of = fun wp -> ExtXml.attrib wp "name" @@ -121,7 +114,6 @@ let print_exception = fun x -> let c = parsed_attrib x "cond" in lprintf "if %s { GotoBlock(%s) }\n" c i -let return_from_excpt l = Xml.Element ("return_from_excpt", ["name",l], []) let goto l = Xml.Element ("goto", ["name",l], []) let exit_block = Xml.Element ("exit_block", [], []) @@ -362,7 +354,7 @@ let rec print_stage = fun index_of_waypoints x -> stage (); let wp = get_index_waypoint (ExtXml.attrib x "wp") index_of_waypoints in let r = parsed_attrib x "radius" in - let vmode = output_vmode x wp "" in + let _vmode = output_vmode x wp "" in lprintf "Circle(%s, %s);\n" wp r; begin try @@ -387,7 +379,7 @@ let rec print_stage = fun index_of_waypoints x -> lprintf "NextStage();\n"; end; lprintf "return;\n" - | s -> failwith "Unreachable" + | _s -> failwith "Unreachable" end; left () diff --git a/sw/tools/gen_messages.ml b/sw/tools/gen_messages.ml index c1b0d78010..46808f1426 100644 --- a/sw/tools/gen_messages.ml +++ b/sw/tools/gen_messages.ml @@ -45,8 +45,6 @@ module Syntax = struct type messages = message list - let lineno = ref 1 (* For syntax error messages *) - let assoc_types t = try List.assoc t Pprz.types @@ -56,7 +54,6 @@ module Syntax = struct let rec sizeof = function Basic t -> (assoc_types t).Pprz.size | Array (t, i) -> i * sizeof (Basic t) - let glibof = fun t -> (assoc_types t).Pprz.glib_type let formatof = fun t -> (assoc_types t).Pprz.format let print_format t = function @@ -75,8 +72,6 @@ module Syntax = struct List.iter print_field fields; printf "}\n" - let print_messages = List.iter print_message - open Xml let xml_error s = failwith ("Bad XML tag: "^s^ " expected") @@ -279,7 +274,7 @@ let gen_periodic = fun avr_h messages -> let rec gen_or_code = fun tab modulos xors -> List.iter (fun xor -> - let required_modulos = List.map (fun (N (s,p,ms,t)) -> p) xor in + let required_modulos = List.map (fun (N (_s,p,_ms,_t)) -> p) xor in let rec add_new_modulos = fun modulos l -> match l with [] -> modulos @@ -316,7 +311,6 @@ let _ = end; let filename = Sys.argv.(1) in let class_name = Sys.argv.(2) in - let base = Filename.basename (Filename.chop_extension filename) ^ class_name in let messages = Syntax.read filename class_name in