diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp index 36d8454984..8c9bd2f3fa 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp @@ -74,13 +74,13 @@ void FlightTaskManualAltitude::_updateAltitudeLock() */ /* handle position and altitude hold */ - const bool stick_z_zero = fabsf(_sticks_expo(2)) <= FLT_EPSILON; + const bool apply_brake_z = fabsf(_vel_sp_z) <= FLT_EPSILON; const bool stopped_z = (_vel_z_dz.get() < FLT_EPSILON || fabsf(_velocity(2)) < _vel_z_dz.get()); - if (stick_z_zero && stopped_z && !PX4_ISFINITE(_pos_sp_z)) { + if (apply_brake_z && stopped_z && !PX4_ISFINITE(_pos_sp_z)) { _pos_sp_z = _position(2); - } else if (!stick_z_zero) { + } else if (!apply_brake_z) { _pos_sp_z = NAN; } } diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp index c4e985806a..f0c63288e3 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp @@ -84,13 +84,13 @@ void FlightTaskManualPosition::_updateXYlock() { /* If position lock is not active, position setpoint is set to NAN.*/ const float vel_xy_norm = Vector2f(&_velocity(0)).length(); - const bool stick_zero = matrix::Vector2f(_sticks_expo(0), _sticks_expo(1)).length() < FLT_EPSILON; + const bool apply_brake = _vel_sp_xy.length() < FLT_EPSILON; const bool stopped = (_vel_xy_dz.get() < FLT_EPSILON || vel_xy_norm < _vel_xy_dz.get()); - if (stick_zero && stopped && !PX4_ISFINITE(_pos_sp_xy(0))) { + if (apply_brake && stopped && !PX4_ISFINITE(_pos_sp_xy(0))) { _pos_sp_xy = matrix::Vector2f(&_position(0)); - } else if (!stick_zero) { + } else if (!apply_brake) { _pos_sp_xy = _pos_sp_xy * NAN; } }