dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FLightTaskAuto: limit nudging speed based on distance sensor

This commit is contained in:
Matthias Grob 2024-01-26 15:05:57 +01:00
parent 584d8abe1e
commit 97cb933cff
1 changed files with 6 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View File

@ -281,6 +281,12 @@ void FlightTaskAuto::_prepareLandSetpoints()
sticks_xy.setZero();
}
// If ground distance estimate valid (distance sensor) during nudging then limit horizontal speed
if (PX4_ISFINITE(_dist_to_bottom)) {
// Below 50cm no horizontal speed, above allow per meter altitude 0.5m/s speed
max_speed = math::max(0.f, math::min(max_speed, (_dist_to_bottom - .5f) * .5f));
}
_stick_acceleration_xy.setVelocityConstraint(max_speed);
_stick_acceleration_xy.generateSetpoints(sticks_xy, _yaw, _land_heading, _position,
_velocity_setpoint_feedback.xy(), _deltatime);