cleaning

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 ========================================= *)

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 *)

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
 

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

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

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 ()
 

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