Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-24 22:05:58 +08:00
Code Issues Actions 28 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
conf/airframes/tudelft/rotwing_v3c_oneloop.xml
+2
View File
@@ -120,8 +120,10 @@
<define name="NAV_HYBRID_MAX_AIRSPEED" value="19.0f"/>
<define name="NAV_HYBRID_SPEED_MARGIN" value="0.0f"/>
<define name="NAV_HYBRID_MAX_ACCELERATION" value="4.0f"/>
<define name="NAV_HYBRID_SOFT_ACCELERATION" value="2.0f"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="1.0f"/>
<define name="GUIDANCE_H_USE_REF" value="FALSE"/>
<define name="NAV_HYBRID_LIMIT_CIRCLE_RADIUS" value="TRUE"/>
</module>
<module name="oneloop" type="andi">
conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml
+3 -1
View File
@@ -120,9 +120,11 @@
<define name="NAV_HYBRID_MAX_AIRSPEED" value="19.0f"/>
<define name="NAV_HYBRID_SPEED_MARGIN" value="0.0f"/>
<define name="NAV_HYBRID_MAX_ACCELERATION" value="4.0f"/>
<define name="NAV_HYBRID_SOFT_ACCELERATION" value="2.0f"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="1.0f"/>
<define name="GUIDANCE_H_USE_REF" value="FALSE"/>
<define name="NAV_OPTITRACK" value="TRUE"/>
<define name="NAV_HYBRID_EXT_VISION_SETPOINT_MODE" value="TRUE"/>
<define name="NAV_HYBRID_LIMIT_CIRCLE_RADIUS" value="TRUE"/>
</module>
<module name="oneloop" type="andi">
conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml
+3 -1
View File
@@ -112,9 +112,11 @@
<define name="NAV_HYBRID_MAX_AIRSPEED" value="19.0f"/>
<define name="NAV_HYBRID_SPEED_MARGIN" value="0.0f"/>
<define name="NAV_HYBRID_MAX_ACCELERATION" value="4.0f"/>
<define name="NAV_HYBRID_SOFT_ACCELERATION" value="2.0f"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="1.0f"/>
<define name="GUIDANCE_H_USE_REF" value="FALSE"/>
<define name="NAV_OPTITRACK" value="TRUE"/>
<define name="NAV_HYBRID_EXT_VISION_SETPOINT_MODE" value="TRUE"/>
<define name="NAV_HYBRID_LIMIT_CIRCLE_RADIUS" value="TRUE"/>
</module>
<module name="oneloop" type="andi">
conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml
+2
View File
@@ -81,8 +81,10 @@
<define name="NAV_HYBRID_MAX_SPEED_V" value="3.0f"/>
<define name="NAV_HYBRID_SPEED_MARGIN" value="0.0f"/>
<define name="NAV_HYBRID_MAX_ACCELERATION" value="4.0f"/>
<define name="NAV_HYBRID_SOFT_ACCELERATION" value="2.0f"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="1.0f"/>
<define name="GUIDANCE_H_USE_REF" value="FALSE"/>
<define name="NAV_HYBRID_LIMIT_CIRCLE_RADIUS" value="TRUE"/>
</module>
<module name="oneloop" type="andi">
conf/modules/nav_hybrid.xml
+5 -1
View File
@@ -7,6 +7,9 @@
</description>
<section name="NAV_HYBRID" prefix="NAV_HYBRID_">
<define name="MAX_DECELERATION" value="1.0" description="Maximum deceleration in [m/s2] when arriving to hover at a WP"/>
<define name="MAX_EXPECTED_WIND" value="5" description="Maximum expected constant wind. Used to calculate minimum circle radius to not exceed acceleration bound."/>
<define name="LIMIT_CIRCLE_RADIUS" value="FALSE" description="Limit the circle radius on max target groundspeed and max acceleration."/>
<define name="EXT_VISION_SETPOINT_MODE" value="FALSE" description="Use position setpoints when using external vision systems."/>
</section>
<section name="GUIDANCE_INDI_HYBRID" prefix="GUIDANCE_INDI_">
<define name="MAX_AIRSPEED" value="15." description="maximum airspeed (required)"/>
@@ -19,8 +22,9 @@
<settings>
<dl_settings>
<dl_settings NAME="nav_hybrid">
<dl_setting var="nav_hybrid_max_acceleration" min="1.0" step="0.1" max="9.0"/>
<dl_setting var="nav_max_speed" min="1.0" step="1.0" max="50.0"/>
<dl_setting var="nav_goto_max_speed" min="0.1" step="0.1" max="50.0"/>
<dl_setting var="nav_hybrid_max_expected_wind" min="0.0" step="0.1" max="20.0" shortname="max_expected_wind"/>
<dl_setting var="nav_max_deceleration_sp" min="0.5" step="0.1" max="10.0" shortname="max_deceleration" param="NAV_HYBRID_MAX_DECELERATION"/>
<dl_setting var="nav_hybrid_line_gain" min="0.1" step="0.1" max="3" shortname="nav_hybrid_line_gain" param="NAV_HYBRID_LINE_GAIN"/>
<dl_setting var="nav_hybrid_pos_gain" min="0.1" step="0.1" max="10" shortname="nav_hybrid_pos_gain" param="NAV_HYBRID_POS_GAIN"/>
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