Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-29 02:38:07 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

Some changes to NAV Hybrid (#3335)

Browse Source 
* first push

* Addressed the comments

* address comments

* Correct prefix and variable name

* make radius circle limiting optional

* first push

* Addressed the comments

* address comments

* Correct prefix and variable name

* make radius circle limiting optional

* Added soft acceleration limit

* Remove nav goto where not used. Added logic for minimum no-transition distance. Code clean-up.

* Add possibility to force max goto speed if needed

* Removed nav_goto_max_speed
This commit is contained in:
Tomaso Maria Luigi De Ponti
2024-10-01 13:13:33 +02:00
committed by GitHub
parent 5a45a6158d
commit 77b0daffd4
9 changed files with 70 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/modules/nav/nav_rotorcraft_hybrid.c
+52 -21
View File
@@ -51,13 +51,27 @@ float nav_max_speed = NAV_MAX_SPEED;
#define NAV_HYBRID_GOTO_MAX_SPEED NAV_MAX_SPEED
#endif
float nav_goto_max_speed = NAV_HYBRID_GOTO_MAX_SPEED;
#ifndef NAV_HYBRID_MAX_DECELERATION
#define NAV_HYBRID_MAX_DECELERATION 1.0
#endif
float nav_max_deceleration_sp = NAV_HYBRID_MAX_DECELERATION;
#ifndef NAV_HYBRID_MAX_ACCELERATION
#define NAV_HYBRID_MAX_ACCELERATION 4.0
#endif
#ifndef NAV_HYBRID_SOFT_ACCELERATION
#define NAV_HYBRID_SOFT_ACCELERATION NAV_HYBRID_MAX_ACCELERATION
#endif
float nav_max_deceleration_sp = NAV_HYBRID_MAX_DECELERATION; //Maximum deceleration allowed for the set-point
float nav_max_acceleration_sp = NAV_HYBRID_MAX_ACCELERATION; //Maximum acceleration allowed for the set-point
float nav_hybrid_soft_acceleration = NAV_HYBRID_SOFT_ACCELERATION; //Soft acceleration limit allowed for the set-point (Equal to the maximum acceleration by default)
float nav_hybrid_max_acceleration = NAV_HYBRID_MAX_ACCELERATION; //Maximum general limit in acceleration allowed for the hybrid navigation
#ifndef NAV_HYBRID_MAX_EXPECTED_WIND
#define NAV_HYBRID_MAX_EXPECTED_WIND 5.0f
#endif
float nav_hybrid_max_expected_wind = NAV_HYBRID_MAX_EXPECTED_WIND;
#ifdef NAV_HYBRID_LINE_GAIN
float nav_hybrid_line_gain = NAV_HYBRID_LINE_GAIN;
@@ -83,15 +97,21 @@ float nav_hybrid_pos_gain = 1.0f;
bool force_forward = 0.0f;
#endif
#ifndef NAV_HYBRID_GOTO_MODE
#define NAV_HYBRID_GOTO_MODE NAV_SETPOINT_MODE_SPEED
/* When using external vision, run the nav functions in position mode for better tracking*/
#ifndef NAV_HYBRID_EXT_VISION_SETPOINT_MODE
#define NAV_HYBRID_EXT_VISION_SETPOINT_MODE FALSE
#endif
/* Use max target groundspeed and max acceleration to limit circle radius*/
#ifndef NAV_HYBRID_LIMIT_CIRCLE_RADIUS
#define NAV_HYBRID_LIMIT_CIRCLE_RADIUS FALSE
#endif
/** Implement basic nav function for the hybrid case
*/
static void nav_hybrid_goto(struct EnuCoor_f *wp)
{
nav_max_acceleration_sp = nav_hybrid_soft_acceleration;
nav_rotorcraft_base.goto_wp.to = *wp;
nav_rotorcraft_base.goto_wp.dist2_to_wp = get_dist2_to_point(wp);
VECT2_COPY(nav.target, *wp);
@@ -107,7 +127,7 @@ static void nav_hybrid_goto(struct EnuCoor_f *wp)
// Bound the setpoint velocity vector
float max_h_speed = nav_max_speed;
if (!force_forward) {
// If not in force_forward, compute speed based on decceleration and nav_goto_max_speed
// If not in force_forward, compute speed based on deceleration and nav_max_speed
// Calculate distance to waypoint
float dist_to_wp = float_vect2_norm(&pos_error);
// Calculate max speed when decelerating at MAX capacity a_max
@@ -118,13 +138,19 @@ static void nav_hybrid_goto(struct EnuCoor_f *wp)
float max_speed_decel2 = fabsf(2.f * dist_to_wp * nav_max_deceleration_sp); // dist_to_wp can only be positive, but just in case
float max_speed_decel = sqrtf(max_speed_decel2);
// Bound the setpoint velocity vector
max_h_speed = Min(nav_goto_max_speed, max_speed_decel); // use hover max speed
max_h_speed = Min(nav_max_speed, max_speed_decel);
}
float_vect2_bound_in_2d(&speed_sp, max_h_speed);
VECT2_COPY(nav.speed, speed_sp);
nav.horizontal_mode = NAV_HORIZONTAL_MODE_WAYPOINT;
nav.setpoint_mode = NAV_HYBRID_GOTO_MODE;
// In optitrack tests use position mode for more precise hovering
#if NAV_HYBRID_EXT_VISION_SETPOINT_MODE
nav.setpoint_mode = NAV_SETPOINT_MODE_POS;
#else
nav.setpoint_mode = NAV_SETPOINT_MODE_SPEED;
#endif
}
static void nav_hybrid_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
@@ -225,7 +251,10 @@ static void nav_hybrid_circle(struct EnuCoor_f *wp_center, float radius)
float sign_radius = radius > 0.f ? 1.f : -1.f;
// absolute radius
float abs_radius = fabsf(radius);
#if NAV_HYBRID_LIMIT_CIRCLE_RADIUS
float min_radius = pow(nav_max_speed+nav_hybrid_max_expected_wind,2) / (nav_hybrid_max_acceleration * 0.8);
abs_radius = Max(abs_radius, min_radius);
#endif
if (abs_radius > 0.1f) {
// store last qdr
float last_qdr = nav_rotorcraft_base.circle.qdr;
@@ -248,20 +277,22 @@ static void nav_hybrid_circle(struct EnuCoor_f *wp_center, float radius)
VECT2_COPY(nav.target, *wp_center);
}
// compute desired speed
float radius_diff = fabsf(float_vect2_norm(&pos_diff) - abs_radius);
if (radius_diff > NAV_HYBRID_NAV_CIRCLE_DIST) {
// far from circle, speed proportional to diff
desired_speed = radius_diff * nav_hybrid_pos_gain;
nav_max_acceleration_sp = nav_hybrid_soft_acceleration;
} else {
// close to circle, speed function of radius for a feasible turn
// 0.8 * MAX_BANK gives some margins for the turns
desired_speed = sqrtf(PPRZ_ISA_GRAVITY * abs_radius * tanf(0.8f * nav_hybrid_max_bank));
nav_max_acceleration_sp = nav_hybrid_max_acceleration ;
}
if (force_forward) {
desired_speed = nav_max_speed;
} else {
float radius_diff = fabsf(float_vect2_norm(&pos_diff) - abs_radius);
if (radius_diff > NAV_HYBRID_NAV_CIRCLE_DIST) {
// far from circle, speed proportional to diff
desired_speed = radius_diff * nav_hybrid_pos_gain;
} else {
// close to circle, speed function of radius for a feasible turn
// 0.8 * MAX_BANK gives some margins for the turns
desired_speed = sqrtf(PPRZ_ISA_GRAVITY * abs_radius * tanf(0.8f * nav_hybrid_max_bank));
}
Bound(desired_speed, 0.0f, nav_max_speed);
nav_max_acceleration_sp = nav_hybrid_max_acceleration ;
}
Bound(desired_speed, 0.0f, nav_max_speed);
// compute speed vector from target position
struct FloatVect2 speed_unit;
VECT2_DIFF(speed_unit, nav.target, *stateGetPositionEnu_f());
@@ -269,7 +300,7 @@ static void nav_hybrid_circle(struct EnuCoor_f *wp_center, float radius)
VECT2_SMUL(nav.speed, speed_unit, desired_speed);
nav_rotorcraft_base.circle.center = *wp_center;
nav_rotorcraft_base.circle.radius = radius;
nav_rotorcraft_base.circle.radius = sign_radius * abs_radius;
nav.horizontal_mode = NAV_HORIZONTAL_MODE_CIRCLE;
nav.setpoint_mode = NAV_SETPOINT_MODE_SPEED;
}
sw/airborne/modules/nav/nav_rotorcraft_hybrid.h
+3 -2
View File
@@ -33,16 +33,17 @@
// settings
extern float nav_max_speed; // max speed in route mode
extern float nav_goto_max_speed; // max speed in goto/stay mode
extern float nav_max_deceleration_sp;
extern float nav_max_acceleration_sp; // Maximum limit that can vary depending on the function
extern float nav_hybrid_max_acceleration; // General setting
extern float nav_hybrid_line_gain;
extern float nav_hybrid_pos_gain;
extern float nav_hybrid_max_bank;
extern float nav_hybrid_max_expected_wind;
#ifndef GUIDANCE_INDI_HYBRID
extern bool force_forward;
#endif
extern void nav_rotorcraft_hybrid_init(void);
#endif
sw/airborne/modules/rotwing_drone/rotwing_state.c
-1
View File
@@ -552,7 +552,6 @@ void rotwing_state_set_state_settings(void)
force_forward = rotwing_state_settings.force_forward;
nav_max_speed = rotwing_state_settings.nav_max_speed;
nav_goto_max_speed = rotwing_state_settings.nav_max_speed;
// TO DO: pitch angle now hard coded scheduled by wing angle
sw/airborne/modules/rotwing_drone/rotwing_state_V2.c
-1
View File
@@ -603,7 +603,6 @@ void rotwing_state_set_state_settings(void)
force_forward = rotwing_state_settings.force_forward;
nav_max_speed = rotwing_state_settings.nav_max_speed;
nav_goto_max_speed = rotwing_state_settings.nav_max_speed;
// TO DO: pitch angle now hard coded scheduled by wing angle