[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [4098] navigation in circle
From: |
Gautier Hattenberger |
Subject: |
[paparazzi-commits] [4098] navigation in circle |
Date: |
Tue, 08 Sep 2009 09:03:07 +0000 |
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) {
\
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [4098] navigation in circle,
Gautier Hattenberger <=