fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 17:43:33 +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-11 20:09:45 +01:00
committed by Lorenz Meier
parent 63a6ab34f7
commit f794ee0c8a
2 changed files with 12 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+8 -5
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);
}
}