[paparazzi-commits] [4146] circle mode

[Gautier Hattenberger]

Revision: 4146
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4146
Author:   gautier
Date:     2009-09-11 11:54:46 +0000 (Fri, 11 Sep 2009)
Log Message:
-----------
circle mode

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c        2009-09-11 
11:53:53 UTC (rev 4145)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c        2009-09-11 
11:54:46 UTC (rev 4146)
@@ -143,7 +143,7 @@
     // compute qdr
     INT32_ATAN2(nav_circle_qdr, pos_diff.y, pos_diff.x);
     // increment circle radians
-    int32_t angle_diff = (nav_circle_qdr - last_qdr) >> (INT32_TRIG_FRAC - 
INT32_ANGLE_FRAC);
+    int32_t angle_diff = nav_circle_qdr - last_qdr;
     INT32_ANGLE_NORMALIZE(angle_diff);
     nav_circle_radians += angle_diff;
 
@@ -152,8 +152,8 @@
     // absolute radius
     int32_t abs_radius = abs(radius);
     // carrot_angle
-    int32_t carrot_angle = (CARROT_DIST / abs_radius) << (INT32_TRIG_FRAC - 
INT32_POS_FRAC);
-    Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_2);
+    int32_t carrot_angle = (CARROT_DIST / abs_radius) << (INT32_ANGLE_FRAC - 
INT32_POS_FRAC);
+    Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4);
     carrot_angle = nav_circle_qdr - sign_radius * carrot_angle;
     int32_t s_carrot, c_carrot;
     PPRZ_ITRIG_SIN(s_carrot, carrot_angle);    
@@ -181,8 +181,10 @@
   int32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y),1);
   INT32_SQRT(leg_length,leg_length2);
   int32_t nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) 
/ leg_length;
-  nav_leg_progress += Max((CARROT_DIST >> INT32_POS_FRAC), 0);
-  Bound(nav_leg_progress,0,leg_length);
+  int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0);
+  nav_leg_progress += progress;
+  int32_t prog_2 = leg_length + progress / 2;
+  Bound(nav_leg_progress, 0, prog_2);
   struct Int32Vect2 progress_pos;
   VECT2_SMUL(progress_pos, wp_diff, nav_leg_progress);
   VECT2_SDIV(progress_pos, progress_pos, leg_length);