diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp index 2b37d63233..81f003837a 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp @@ -44,7 +44,7 @@ FlightTaskManualAltitude::FlightTaskManualAltitude(control::SuperBlock *parent, FlightTaskManualStabilized(parent, name), _vel_max_down(parent, "MPC_Z_VEL_MAX_DN", false), _vel_max_up(parent, "MPC_Z_VEL_MAX_UP", false), - _vel_z_dz(parent, "MPC_HOLD_MAX_Z", false) + _vel_hold_thr_z(parent, "MPC_HOLD_MAX_Z", false) {} @@ -73,7 +73,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock() /* handle position and altitude hold */ 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()); + const bool stopped_z = (_vel_hold_thr_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _vel_hold_thr_z.get()); if (apply_brake_z && stopped_z && !PX4_ISFINITE(_pos_sp_z)) { _pos_sp_z = _position(2); diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp index 6f4e68c4cf..528044ed83 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp @@ -58,7 +58,7 @@ protected: control::BlockParamFloat _vel_max_down; /**< Maximum speed allowed to go up. */ control::BlockParamFloat _vel_max_up; /**< Maximum speed allowed to go down. */ - control::BlockParamFloat _vel_z_dz; /**< velocity threshold/deadzone to switch into vertical position hold */ + control::BlockParamFloat _vel_hold_thr_z; /**< velocity threshold to switch back into vertical position hold */ void _updateAltitudeLock(); /**< Checks for position lock. */ void _updateSetpoints() override; /**< Updates all setpoints. */ diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp index f4bcadca3e..480368930b 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp @@ -45,7 +45,7 @@ FlightTaskManualPosition::FlightTaskManualPosition(control::SuperBlock *parent, FlightTaskManualAltitude(parent, name), _vel_xy_manual_max(parent, "MPC_VEL_MANUAL", false), _acc_xy_max(parent, "MPC_ACC_HOR_MAX", false), - _vel_xy_dz(parent, "MPC_HOLD_MAX_XY", false) + _vel_hold_thr_xy(parent, "MPC_HOLD_MAX_XY", false) {} bool FlightTaskManualPosition::activate() @@ -84,7 +84,7 @@ 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 apply_brake = _vel_sp_xy.length() < FLT_EPSILON; - const bool stopped = (_vel_xy_dz.get() < FLT_EPSILON || vel_xy_norm < _vel_xy_dz.get()); + const bool stopped = (_vel_hold_thr_xy.get() < FLT_EPSILON || vel_xy_norm < _vel_hold_thr_xy.get()); if (apply_brake && stopped && !PX4_ISFINITE(_pos_sp_xy(0))) { _pos_sp_xy = matrix::Vector2f(&_position(0)); diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp index 127927d92e..62856287b3 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp @@ -54,12 +54,12 @@ public: bool update() override; protected: - matrix::Vector2f _vel_sp_xy{}; /**< Scaled velocity setpoint from stick. NAN during position lock. */ - matrix::Vector2f _pos_sp_xy{}; /**< Position setpoint during lock. Otherwise NAN.*/ + matrix::Vector2f _vel_sp_xy{}; /**< Scaled velocity setpoint from stick. NAN during position lock */ + matrix::Vector2f _pos_sp_xy{}; /**< Position setpoint during lock. Otherwise NAN */ - control::BlockParamFloat _vel_xy_manual_max; /**< Maximum speed allowed horizontally, */ - control::BlockParamFloat _acc_xy_max;/**< Maximum acceleration horizontally. Only used to compute lock time. */ - control::BlockParamFloat _vel_xy_dz; /**< velocity threshold/deadzone to switch into horizontal position hold */ + control::BlockParamFloat _vel_xy_manual_max; /**< Maximum speed allowed horizontally */ + control::BlockParamFloat _acc_xy_max;/**< Maximum acceleration horizontally. Only used to compute lock time */ + control::BlockParamFloat _vel_hold_thr_xy; /**< velocity threshold to switch back into horizontal position hold */ void _updateXYlock(); /**< Applies positon lock based on stick and velocity */ void _updateSetpoints() override;