path fixed

sw/airborne/nav.c

@@ -106,25 +106,24 @@ static float qdr;
 
 #define CircleXY(x, y, radius) { \
   last_alpha = alpha; \
-  alpha = atan2(estimator_y - y, \
-		      estimator_x - x); \
-	if (! new_circle) { \
-	  float alpha_diff = alpha - last_alpha; \
-		NormRadAngle(alpha_diff); \
-	  sum_alpha += alpha_diff; \
-	} \
-	else new_circle = FALSE; \
-	circle_count = fabs(sum_alpha) / (2*M_PI); \
+  alpha = atan2(estimator_y - y, estimator_x - x); \
+  if (! new_circle) { \
+    float alpha_diff = alpha - last_alpha; \
+    NormRadAngle(alpha_diff); \
+    sum_alpha += alpha_diff; \
+  } else \
+    new_circle = FALSE; \
+  circle_count = fabs(sum_alpha) / (2*M_PI); \
   float alpha_carrot = alpha + CARROT / -radius * estimator_hspeed_mod; \
   horizontal_mode = HORIZONTAL_MODE_CIRCLE; \
   fly_to_xy(x+cos(alpha_carrot)*fabs(radius), \
 						y+sin(alpha_carrot)*fabs(radius)); \
   qdr = DegOfRad(M_PI/2 - alpha_carrot); \
   NormCourse(qdr); \
-	in_circle = TRUE; \
-	circle_x = x; \
-	circle_y = y; \
-	circle_radius = radius; \
+  in_circle = TRUE; \
+  circle_x = x; \
+  circle_y = y; \
+  circle_radius = radius; \
 }
 
 #define MAX_DIST_CARROT 250.
@@ -200,32 +199,27 @@ const uint8_t nb_waypoint = NB_WAYPOINT;
 struct point waypoints[NB_WAYPOINT+1] = WAYPOINTS;
 
 
-/** distance of carrot (in meter) */
-static float carrot;
 
-/** static bool_t approaching(uint8_t wp)
- *  \brief Decide if uav is approaching of current waypoint.
- */
-/** Computes \a dist2_to_wp and compare it to square \a carrot.
+/** \brief Decide if uav is approaching of current waypoint.
+ *  Computes \a dist2_to_wp and compare it to square \a carrot.
  *  Return true if it is smaller. Else computes by scalar products if 
  *  uav has not gone past waypoint.
  *  Return true if it is the case.
  */
-static bool_t approaching(uint8_t wp, float carrot_time) {
+static bool_t approaching(uint8_t wp, float approaching_time) {
   /** distance to waypoint in x */
   float pw_x = waypoints[wp].x - estimator_x;
   /** distance to waypoint in y */
   float pw_y = waypoints[wp].y - estimator_y;
 
   dist2_to_wp = pw_x*pw_x + pw_y *pw_y;
-  carrot = carrot_time * estimator_hspeed_mod;
-  //  carrot = (carrot < 40 ? 40 : carrot);
-  if (dist2_to_wp < carrot*carrot)
+  float min_dist = approaching_time * estimator_hspeed_mod;
+  if (dist2_to_wp < min_dist*min_dist)
     return TRUE;
 
   float scal_prod = (waypoints[wp].x - last_x) * pw_x + (waypoints[wp].y - last_y) * pw_y;
   
-  return (scal_prod < 0);
+  return (scal_prod < 0.);
 }
 
 /** static inline void fly_to_xy(float x, float y)
@@ -260,7 +254,10 @@ static void route_to(uint8_t _last_wp, uint8_t wp) {
   alpha = ((estimator_x - last_wp_x) * leg_x + (estimator_y - last_wp_y) * leg_y) / leg2;
   alpha = Max(alpha, 0.);
   leg = sqrt(leg2);
-  /** carrot is computed in approaching() */
+
+  /** distance of carrot (in meter) */
+  float carrot = CARROT * estimator_hspeed_mod;
+
   alpha += Max(carrot / leg, 0.);
   alpha = Min(1., alpha);
   in_segment = TRUE;

sw/tools/fp_proc.ml

@@ -404,6 +404,13 @@ 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 ->
@@ -413,31 +420,42 @@ let compile_path = fun wpts radius last_last last ps rest ->
 	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
-	(* The center C of the arc is such that P0P1 _|_ P1C *)
-	let x01 = x1 -. x0 and y01 = y1 -. y0
-	and x02 = x2 -. x0 and y02 = y2 -. y0 in
-	let pvect = x01*. y02 -. y01*.x02 in
-	let s = sign pvect in
-	let alpha_1c = atan2 (y1-.y0) (x1-.x0) +. s *. pi /. 2. in
-	let xc = x1 +. radius *. cos alpha_1c
-	and yc = y1 +. radius *. sin alpha_1c 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
-	let d_f2 = sqrt (d_c2*.d_c2 -. radius*.radius) in
-	let alpha_c2f = atan (radius/.d_f2) in
-	let alpha_2f = atan2 (yc-.y2) (xc-.x2) +. s *. alpha_c2f in
-	let xf = x2 +. d_f2 *. cos alpha_2f
-	and yf = y2 +. d_f2 *. sin alpha_2f in
-	Printf.fprintf stderr "%s->%s: xf=%.0f yf=%.0f s=%.0f\n" last wp xf yf s;
-	
-	let alpha_cf = atan2 (y2-.yc) (x2-.xc) +. (pi/.2. -. alpha_c2f) in
-	let theta = abs_float ((pi +. alpha_1c) -. alpha_cf) /. 2. /. pi in
-	let theta = if theta > 1. then theta -. 1. else theta in
-	let c_wp_qdr= norm_2pi (pi /. 2. -. alpha_1c) in
-	let f_wp_qdr= norm_2pi (pi /. 2. -. alpha_2f) 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
+
+	(* Angle between P1 and F *)
+	let alpha_c1 = atan2 (y1-.yc) (x1-.xc) 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 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_wp_qdr);
+				"wp_qdr", string_of_float ((Rad>>Deg)c_last_qdr);
 				"wp_dist", string_of_float radius;
 		                "radius", sradius;
 				"until", until],[])::
@@ -445,6 +463,7 @@ let compile_path = fun wpts radius last_last last ps rest ->
 			    "from_qdr", string_of_float ((Rad>>Deg)f_wp_qdr); 
 			    "from_dist", string_of_float d_f2;
 			    "hmode", "route";
+			    "approaching_time", "2";
 			    "wp", wp], [])::
 	loop (xf,yf) wp ps in
   loop last_last last ps;;