Skip to content
Snippets Groups Projects
Commit d22af467 authored by Matthias Grob's avatar Matthias Grob Committed by Beat Küng
Browse files

FlightTaskManualAltitude/Position: renamed and recommented velocity hold threshold

parent 6fb9ca3b
No related branches found
No related tags found
No related merge requests found
......@@ -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);
......
......@@ -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. */
......
......@@ -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));
......
......@@ -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;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment