fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 03:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskAuto: disable MPC_LAND_RADIUS by default with value -1 (#25275)

Browse Source 
* FlightTaskAuto: disable MPC_LAND_RADIUS by default

* format & comment

* Remove position valididy check again

* clean up according to review
This commit is contained in:
Balduin
2025-10-23 17:36:46 +02:00
committed by GitHub
parent 540e80eb77
commit 1c821cf83d
2 changed files with 26 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+18 -11
View File
@@ -266,21 +266,28 @@ void FlightTaskAuto::_prepareLandSetpoints()
Vector2f sticks_ne = sticks_xy;
Sticks::rotateIntoHeadingFrameXY(sticks_ne, _yaw, _land_heading);
const float distance_to_circle = math::trajectory::getMaxDistanceToCircle(_position.xy(), _initial_land_position.xy(),
_param_mpc_land_radius.get(), sticks_ne);
float max_speed;
float max_speed = INFINITY;
if (PX4_ISFINITE(distance_to_circle)) {
max_speed = math::trajectory::computeMaxSpeedFromDistance(_stick_acceleration_xy.getMaxJerk(),
_stick_acceleration_xy.getMaxAcceleration(), distance_to_circle, 0.f);
if (_param_mpc_land_radius.get() > FLT_EPSILON) {
if (max_speed < 0.5f) {
// = NaN if we are outside of the allowed circle and nudging does not point back towards it
const float distance_to_circle = math::trajectory::getMaxDistanceToCircle(_position.xy(), _initial_land_position.xy(),
_param_mpc_land_radius.get(), sticks_ne);
if (PX4_ISFINITE(distance_to_circle)) {
// We are inside of the allowed circle. Limit speed so we can always brake in time to not leave the circle.
max_speed = math::trajectory::computeMaxSpeedFromDistance(_stick_acceleration_xy.getMaxJerk(),
_stick_acceleration_xy.getMaxAcceleration(), distance_to_circle, 0.f);
if (max_speed < 0.5f) {
sticks_xy.setZero();
}
} else {
max_speed = 0.f;
sticks_xy.setZero();
}
} else {
max_speed = 0.f;
sticks_xy.setZero();
}
_stick_acceleration_xy.setVelocityConstraint(max_speed);
src/modules/mc_pos_control/multicopter_nudging_params.c
+8 -4
View File
@@ -53,13 +53,17 @@ PARAM_DEFINE_INT32(MPC_LAND_RC_HELP, 0);
/**
* User assisted landing radius
*
* When nudging is enabled (see MPC_LAND_RC_HELP), this controls
* the maximum allowed horizontal displacement from the original landing point.
* When nudging is enabled (see MPC_LAND_RC_HELP), this defines the maximum
* allowed horizontal displacement from the original landing point.
* - If inside of the radius, only allow nudging inputs that do not move the vehicle outside of it.
* - If outside of the radius, only allow nudging inputs that move the vehicle back towards it.
*
* Set it to -1 for infinite radius.
*
* @unit m
* @min 0
* @min -1
* @decimal 0
* @increment 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_LAND_RADIUS, 1000.f);
PARAM_DEFINE_FLOAT(MPC_LAND_RADIUS, -1.0f);