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

[nav] glide for rotorcraft navigation

Browse Source
This commit is contained in:
Gautier Hattenberger
2012-06-29 17:38:21 +02:00
parent 4f4bb60f2d
commit 9aad86fce8
2 changed files with 21 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/navigation.c
+10 -5
View File
@@ -53,6 +53,9 @@ 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_leg_progress;
int32_t nav_leg_length;
int32_t nav_roll, nav_pitch;
int32_t nav_heading, nav_course;
float nav_radius;
@@ -99,6 +102,8 @@ void nav_init(void) {
nav_radius = DEFAULT_CIRCLE_RADIUS;
nav_throttle = 0;
nav_climb = 0;
nav_leg_progress = 0;
nav_leg_length = 1;
}
@@ -186,17 +191,16 @@ void nav_route(uint8_t wp_start, uint8_t wp_end) {
// go back to metric precision or values are too large
INT32_VECT2_RSHIFT(wp_diff,wp_diff,INT32_POS_FRAC);
INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC);
int32_t leg_length;
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;
INT32_SQRT(nav_leg_length,leg_length2);
nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_leg_length;
int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0);
nav_leg_progress += progress;
int32_t prog_2 = leg_length;// + progress / 2;
int32_t prog_2 = nav_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);
VECT2_SDIV(progress_pos, progress_pos, nav_leg_length);
INT32_VECT2_LSHIFT(progress_pos,progress_pos,INT32_POS_FRAC);
VECT2_SUM(navigation_target,waypoints[wp_start],progress_pos);
//printf("target %d %d | p %d %d | s %d %d | l %d %d %d\n",
@@ -244,6 +248,7 @@ static inline void nav_set_altitude( void ) {
}
}
/** Reset the geographic reference to the current GPS fix */
unit_t nav_reset_reference( void ) {
ins_ltp_initialised = FALSE;
sw/airborne/firmwares/rotorcraft/navigation.h
+11 -2
View File
@@ -58,6 +58,9 @@ extern int32_t nav_roll, nav_pitch; ///< with #INT32_ANGLE_FRAC
extern int32_t nav_heading, nav_course; ///< with #INT32_ANGLE_FRAC
extern float nav_radius;
extern int32_t nav_leg_progress;
extern int32_t nav_leg_length;
extern uint8_t vertical_mode;
extern uint32_t nav_throttle; ///< direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL
extern int32_t nav_climb, nav_altitude, nav_flight_altitude;
@@ -124,12 +127,18 @@ extern void nav_route(uint8_t wp_start, uint8_t wp_end);
nav_route(_start, _end); \
}
/** Nav glide routine */
#define NavGlide(_last_wp, _wp) { \
int32_t start_alt = waypoints[_last_wp].z; \
int32_t diff_alt = waypoints[_wp].z - start_alt; \
int32_t alt = start_alt + ((diff_alt * nav_leg_progress) / nav_leg_length); \
NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(alt),0); \
}
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) { \