fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-07 03:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

fligh-mode-manager: First implementation of a three-stage-landing for multirotos, in case LIDAR is available

Browse Source
This commit is contained in:
Thomas Debrunner
2022-02-04 21:56:20 -05:00
committed by Daniel Agar
parent 893cdf8f38
commit 2a6d9bc1dd
3 changed files with 42 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+11 -5
View File
@@ -227,9 +227,15 @@ void FlightTaskAuto::_prepareLandSetpoints()
_velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint
// Slow down automatic descend close to ground
float land_speed = math::gradual(_dist_to_ground,
_param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
_param_mpc_land_speed.get(), _param_mpc_z_v_auto_dn.get());
float speed = math::gradual(_dist_to_ground,
_param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
_param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get());
bool range_dist_available = PX4_ISFINITE(_dist_to_bottom);
if (range_dist_available && _dist_to_bottom <= _param_mpc_land_alt3.get()) {
speed = _param_mpc_land_crawl_speed.get();
}
if (_type_previous != WaypointType::land) {
// initialize xy-position and yaw to waypoint such that home is reached exactly without user input
@@ -241,7 +247,7 @@ void FlightTaskAuto::_prepareLandSetpoints()
// User input assisted landing
if (_param_mpc_land_rc_help.get() && _sticks.checkAndSetStickInputs()) {
// Stick full up -1 -> stop, stick full down 1 -> double the speed
land_speed *= (1 + _sticks.getPositionExpo()(2));
speed *= (1 + _sticks.getPositionExpo()(2));
_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading,
_sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _is_yaw_good_for_control, _deltatime);
@@ -263,7 +269,7 @@ void FlightTaskAuto::_prepareLandSetpoints()
_position_setpoint = _land_position; // The last element of the land position has to stay NAN
_yaw_setpoint = _land_heading;
_velocity_setpoint(2) = land_speed;
_velocity_setpoint(2) = speed;
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
+5 -2
View File
@@ -171,11 +171,14 @@ protected:
(ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p,
(ParamFloat<px4::params::MPC_XY_ERR_MAX>) _param_mpc_xy_err_max,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
(ParamFloat<px4::params::MPC_LAND_CRWL>) _param_mpc_land_crawl_speed,
(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
(ParamFloat<px4::params::MPC_LAND_ALT1>)
_param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed
_param_mpc_land_alt1, // altitude at which we start ramping down speed
(ParamFloat<px4::params::MPC_LAND_ALT2>)
_param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed
_param_mpc_land_alt2, // altitude at which we descend at land speed
(ParamFloat<px4::params::MPC_LAND_ALT3>)
_param_mpc_land_alt3, // altitude where we switch to crawl speed, if LIDAR available
(ParamFloat<px4::params::MPC_Z_V_AUTO_UP>) _param_mpc_z_v_auto_up,
(ParamFloat<px4::params::MPC_Z_V_AUTO_DN>) _param_mpc_z_v_auto_dn,
(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
src/modules/mc_pos_control/mc_pos_control_params.c
+26 -1
View File
@@ -414,6 +414,16 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f);
*/
PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f);
/**
* Land crawl descend rate. Used below
*
* @unit m/s
* @min 0.1
* @decimal 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_LAND_CRWL, 0.15f);
/**
* Enable user assisted descent speed for autonomous land routine.
*
@@ -714,7 +724,7 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT1, 10.0f);
* Altitude for 2. step of slow landing (landing)
*
* Below this altitude descending velocity gets
* limited to "MPC_LAND_SPEED".
* limited to "MPC_LAND_SPEED"
* Value needs to be lower than "MPC_LAND_ALT1"
*
* @unit m
@@ -725,6 +735,21 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT1, 10.0f);
*/
PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.0f);
/**
* Altitude for 3. step of slow landing
*
* Below this altitude descending velocity gets
* limited to "MPC_CRWL_SPEED", if LIDAR available.
* No effect if LIDAR not available
*
* @unit m
* @min 0
* @max 122
* @decimal 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_LAND_ALT3, 1.0f);
/**
* Position control smooth takeoff ramp time constant
*