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: if flow is on, ensure minimum altitude

Browse Source
This commit is contained in:
Dennis Mannhart 2018-04-18 17:24:11 +02:00 committed by Lorenz Meier
parent 93ebb60b84
commit ccd0be82ba
2 changed files with 8 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
View File

@ -63,6 +63,12 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
if (apply_brake_z && stopped_z && !PX4_ISFINITE(_position_setpoint(2))) {
_position_setpoint(2) = _position(2);
if ((PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < SENS_FLOW_MINRNG.get())) {
// if vehicle wants to keep altitude but is below minimum flow distance,
// increase altitude to minimum flow distance
_position_setpoint(2) = _position(2) - (SENS_FLOW_MINRNG.get() - _dist_to_bottom);
}
} else if (!apply_brake_z) {
_position_setpoint(2) = NAN;
}

3
src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
View File

@ -54,7 +54,8 @@ protected:
void _scaleSticks() override; /**< scales sticks to velocity in z */
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualStabilized,
(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) MPC_HOLD_MAX_Z
(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) MPC_HOLD_MAX_Z,
(ParamFloat<px4::params::SENS_FLOW_MINRNG>) SENS_FLOW_MINRNG
)
};