fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskManualAltitude: add slow upwards start

Browse Source 
when still close to ground
This commit is contained in:
Matthias Grob 2019-03-01 22:51:46 +01:00 committed by Lorenz Meier
parent 63a6ab34f7
commit f794ee0c8a
2 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
View File

@ -277,12 +277,15 @@ void FlightTaskManualAltitude::_respectGroundSlowdown()
dist_to_ground = -(_position(2) - _sub_home_position->get().z);
}
// limit downwards speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2
// limit speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2
if (PX4_ISFINITE(dist_to_ground)) {
const float slowdown_limit = math::gradual(dist_to_ground,
MPC_LAND_ALT2.get(), MPC_LAND_ALT1.get(),
MPC_LAND_SPEED.get(), _constraints.speed_down);
_velocity_setpoint(2) = math::min(_velocity_setpoint(2), slowdown_limit);
const float limit_down = math::gradual(dist_to_ground,
MPC_LAND_ALT2.get(), MPC_LAND_ALT1.get(),
MPC_LAND_SPEED.get(), _constraints.speed_down);
const float limit_up = math::gradual(dist_to_ground,
MPC_LAND_ALT2.get(), MPC_LAND_ALT1.get(),
MPC_TKO_SPEED.get(), _constraints.speed_up);
_velocity_setpoint(2) = math::constrain(_velocity_setpoint(2), -limit_up, limit_down);
}
}

7
src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
View File

@ -76,9 +76,10 @@ protected:
(ParamFloat<px4::params::MPC_Z_P>) MPC_Z_P, /**< position controller altitude propotional gain */
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) MPC_MAN_Y_MAX, /**< scaling factor from stick to yaw rate */
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) MPC_MAN_TILT_MAX, /**< maximum tilt allowed for manual flight */
(ParamFloat<px4::params::MPC_LAND_ALT1>) MPC_LAND_ALT1, // altitude at which to start downwards slowdown
(ParamFloat<px4::params::MPC_LAND_ALT2>) MPC_LAND_ALT2, // altitude below wich to land with land speed
(ParamFloat<px4::params::MPC_LAND_SPEED>) MPC_LAND_SPEED
(ParamFloat<px4::params::MPC_LAND_ALT1>) MPC_LAND_ALT1, /**< altitude at which to start downwards slowdown */
(ParamFloat<px4::params::MPC_LAND_ALT2>) MPC_LAND_ALT2, /**< altitude below wich to land with land speed */
(ParamFloat<px4::params::MPC_LAND_SPEED>) MPC_LAND_SPEED, /**< desired downwards speed when approaching the ground */
(ParamFloat<px4::params::MPC_TKO_SPEED>) MPC_TKO_SPEED /**< desired upwards speed when still close to the ground */
)
private:
/**