Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

navigation in circle

Browse Source
This commit is contained in:
Gautier Hattenberger
2009-09-08 09:03:07 +00:00
parent 17b8567ff4
commit e83584dfc7
3 changed files with 80 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/booz2_navigation.c
+50 -1
View File
@@ -52,9 +52,16 @@ int32_t ground_alt;
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;
@@ -64,7 +71,7 @@ float flight_altitude;
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 @@ void booz2_nav_init(void) {
nav_pitch = 0;
nav_heading = 0;
nav_course = 0;
nav_radius = DEFAULT_CIRCLE_RADIUS;
nav_throttle = 0;
nav_climb = 0;
@@ -121,6 +129,46 @@ void booz2_nav_run(void) {
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 @@ unit_t nav_reset_alt( void ) {
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;
}
sw/airborne/booz/booz2_navigation.h
+24 -17
View File
@@ -48,12 +48,15 @@ extern int32_t ground_alt;
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_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos);
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 @@ void nav_home(void);
#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,25 +120,25 @@ void nav_home(void);
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);
@@ -141,11 +147,12 @@ extern void nav_route(uint8_t wp_start, uint8_t wp_end);
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) { \
sw/airborne/booz/booz2_telemetry.h
+6
View File
@@ -675,6 +675,12 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
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) { \