fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 06:43:35 +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-07-20 09:26:18 +02:00
committed by Lorenz Meier
parent 93ebb60b84
commit ccd0be82ba
2 changed files with 8 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
+6
View File
@@ -63,6 +63,12 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
if (apply_brake_z && stopped_z && !PX4_ISFINITE(_position_setpoint(2))) { if (apply_brake_z && stopped_z && !PX4_ISFINITE(_position_setpoint(2))) {
_position_setpoint(2) = _position(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) { } else if (!apply_brake_z) {
_position_setpoint(2) = NAN; _position_setpoint(2) = NAN;
} }
src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp
+2 -1
View File
@@ -54,7 +54,8 @@ protected:
void _scaleSticks() override; /**< scales sticks to velocity in z */ void _scaleSticks() override; /**< scales sticks to velocity in z */
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualStabilized, 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
) )
}; };