[paparazzi-commits] [4098] navigation in circle

[Gautier Hattenberger]

Revision: 4098
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4098
Author:   gautier
Date:     2009-09-08 09:03:07 +0000 (Tue, 08 Sep 2009)
Log Message:
-----------
navigation in circle

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

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c        2009-09-08 
09:02:31 UTC (rev 4097)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c        2009-09-08 
09:03:07 UTC (rev 4098)
@@ -52,10 +52,17 @@
 
 uint8_t horizontal_mode;
 uint8_t nav_segment_start, nav_segment_end;
+uint8_t nav_circle_centre;
+int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians;
 
 int32_t nav_roll, nav_pitch;
 int32_t nav_heading, nav_course;
+float nav_radius;
 
+#ifndef DEFAULT_CIRCLE_RADIUS
+#define DEFAULT_CIRCLE_RADIUS 0.
+#endif
+
 uint8_t vertical_mode;
 uint32_t nav_throttle;
 int32_t nav_climb, nav_altitude, nav_flight_altitude;
@@ -64,7 +71,7 @@
 static inline void nav_set_altitude( void );
 
 #define CLOSE_TO_WAYPOINT (15 << 8)
-#define ARRIVED_AT_WAYPOINT (10 << 8)
+#define ARRIVED_AT_WAYPOINT (3 << 8)
 #define CARROT_DIST (12 << 8)
 
 void booz2_nav_init(void) {
@@ -91,6 +98,7 @@
   nav_pitch = 0;
   nav_heading = 0;
   nav_course = 0;
+  nav_radius = DEFAULT_CIRCLE_RADIUS;
   nav_throttle = 0;
   nav_climb = 0;
 
@@ -121,6 +129,46 @@
   nav_set_altitude();
 }
 
+void nav_circle(uint8_t wp_center, int32_t radius) {
+  if (radius == 0) {
+    VECT2_COPY(booz2_navigation_target, waypoints[wp_center]);
+  }
+  else {
+    struct Int32Vect2 pos_diff;
+    VECT2_DIFF(pos_diff, booz_ins_enu_pos,waypoints[wp_center]);
+    // go back to half metric precision or values are too large
+    INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC-1);
+    // store last qdr
+    int32_t last_qdr = nav_circle_qdr;
+    // 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_ANGLE_NORMALIZE(angle_diff);
+    nav_circle_radians += angle_diff;
+
+    // direction of rotation
+    int8_t sign_radius = radius > 0 ? 1 : -1;
+    // 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_4);
+    carrot_angle = nav_circle_qdr - sign_radius * carrot_angle;
+    int32_t s_carrot, c_carrot;
+    PPRZ_ITRIG_SIN(s_carrot, carrot_angle);    
+    PPRZ_ITRIG_COS(c_carrot, carrot_angle);    
+    // compute setpoint
+    VECT2_ASSIGN(pos_diff, abs_radius * c_carrot, abs_radius * s_carrot);
+    INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_TRIG_FRAC);
+    VECT2_SUM(booz2_navigation_target, waypoints[wp_center], pos_diff);
+  }
+  nav_circle_centre = wp_center;
+  nav_circle_radius = radius;
+  horizontal_mode = HORIZONTAL_MODE_CIRCLE;
+}
+
+
 //#include "stdio.h"
 void nav_route(uint8_t wp_start, uint8_t wp_end) {
   struct Int32Vect2 wp_diff,pos_diff;
@@ -199,6 +247,7 @@
 void nav_init_stage( void ) {
   INT32_VECT3_COPY(nav_last_point, booz_ins_enu_pos);
   stage_time = 0;
+  nav_circle_radians = 0;
   horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
 }
 

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h        2009-09-08 
09:02:31 UTC (rev 4097)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h        2009-09-08 
09:03:07 UTC (rev 4098)
@@ -48,12 +48,15 @@
 
 extern uint8_t horizontal_mode;
 extern uint8_t nav_segment_start, nav_segment_end;
+extern uint8_t nav_circle_centre;
+extern int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians;
 #define HORIZONTAL_MODE_WAYPOINT  0
 #define HORIZONTAL_MODE_ROUTE     1
 #define HORIZONTAL_MODE_CIRCLE    2
 #define HORIZONTAL_MODE_ATTITUDE  3
 extern int32_t nav_roll, nav_pitch;
 extern int32_t nav_heading, nav_course;
+extern float nav_radius;
 
 extern uint8_t vertical_mode;
 extern uint32_t nav_throttle;
@@ -74,8 +77,8 @@
 
 void nav_home(void);
 
-#define NavKillThrottle() { kill_throttle = 1; booz2_autopilot_motors_on = 0; }
-#define NavResurrect() { kill_throttle = 0; booz2_autopilot_motors_on = 1; }
+#define NavKillThrottle() ({ kill_throttle = 1; booz2_autopilot_motors_on = 0; 
FALSE; })
+#define NavResurrect() ({ kill_throttle = 0; booz2_autopilot_motors_on = 1; 
FALSE; })
 
 #define InitStage() nav_init_stage();
 
@@ -106,7 +109,10 @@
 
 #define NavSetWaypointHere(_wp) { FALSE; }
 
-#define NavCircleWaypoint(wp, radius) {}
+#define WaypointX(_wp)    POS_FLOAT_OF_BFP(waypoints[_wp].x)
+#define WaypointY(_wp)    POS_FLOAT_OF_BFP(waypoints[_wp].y)
+#define WaypointAlt(_wp)  POS_FLOAT_OF_BFP(waypoints[_wp].z)
+#define Height(_h) (_h)
 
 /** Normalize a degree angle between 0 and 359 */
 #define NormCourse(x) { \
@@ -114,26 +120,26 @@
   while (x >= 360) x -= 360; \
 }
 
-#define NavCircleCount() {}
-#define NavCircleQdr() {}
-
-/** True if x (in degrees) is close to the current QDR (less than 10 degrees)*/
-#define NavQdrCloseTo(x) {}
-#define NavCourseCloseTo(x) {}
-
-#define WaypointX(_wp)    POS_FLOAT_OF_BFP(waypoints[_wp].x)
-#define WaypointY(_wp)    POS_FLOAT_OF_BFP(waypoints[_wp].y)
-#define WaypointAlt(_wp)  POS_FLOAT_OF_BFP(waypoints[_wp].z)
-#define Height(_h) (_h)
-
 /*********** Navigation to  waypoint *************************************/
 #define NavGotoWaypoint(_wp) { \
   horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \
   INT32_VECT3_COPY( booz2_navigation_target, waypoints[_wp]); \
 }
 
-#define NavGlide(_last_wp, _wp) {}
+/*********** Navigation on a circle **************************************/
+extern void nav_circle(uint8_t wp_center, int32_t radius);
+#define NavCircleWaypoint(_center, _radius) { \
+  horizontal_mode = HORIZONTAL_MODE_CIRCLE; \
+  nav_circle(_center, POS_BFP_OF_REAL(_radius)); \
+}
 
+#define NavCircleCount() (abs(nav_circle_radians) / INT32_ANGLE_2_PI)
+#define NavCircleQdr() ({ int32_t qdr = INT32_DEG_OF_RAD(INT32_ANGLE_2_PI_2 - 
nav_circle_qdr) >> INT32_ANGLE_FRAC; NormCourse(qdr); qdr; })
+
+/** True if x (in degrees) is close to the current QDR (less than 10 degrees)*/
+#define NavQdrCloseTo(x) {}
+#define NavCourseCloseTo(x) {}
+
 /*********** Navigation along a line *************************************/
 extern void nav_route(uint8_t wp_start, uint8_t wp_end);
 #define NavSegment(_start, _end) { \
@@ -141,11 +147,12 @@
   nav_route(_start, _end); \
 }
 
-
 bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx);
 #define NavApproaching(wp, time) nav_approaching_from(wp, 0)
 #define NavApproachingFrom(wp, from, time) nav_approaching_from(wp, from)
 
+#define NavGlide(_last_wp, _wp) {}
+
 /** Set the climb control to auto-throttle with the specified pitch
     pre-command */
 #define NavVerticalAutoThrottleMode(_pitch) { \

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2009-09-08 09:02:31 UTC 
(rev 4097)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2009-09-08 09:03:07 UTC 
(rev 4098)
@@ -675,6 +675,12 @@
       float ey = POS_FLOAT_OF_BFP(waypoints[nav_segment_end].y);       \
       DOWNLINK_SEND_SEGMENT(_chan, &sx, &sy, &ex, &ey);                        
\
     }                                                                  \
+    else if (horizontal_mode == HORIZONTAL_MODE_CIRCLE) {                      
\
+      float cx = POS_FLOAT_OF_BFP(waypoints[nav_circle_centre].x);     \
+      float cy = POS_FLOAT_OF_BFP(waypoints[nav_circle_centre].y);     \
+      float r = POS_FLOAT_OF_BFP(nav_circle_radius); \
+      DOWNLINK_SEND_CIRCLE(_chan, &cx, &cy, &r);                       \
+    }                                                                  \
   }
 
 #define PERIODIC_SEND_WP_MOVED(_chan) {                                        
\