diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp index 13e6d38522..f5d9992243 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp @@ -45,110 +45,64 @@ using namespace matrix; FlightTaskManualAltitude::FlightTaskManualAltitude(control::SuperBlock *parent, const char *name) : - FlightTaskManual(parent, name), + FlightTaskManualStabilized(parent, name), _vel_max_down(parent, "MPC_Z_VEL_MAX_DN", false), _vel_max_up(parent, "MPC_Z_VEL_MAX_UP", false), - _yaw_rate_scaling(parent, "MPC_MAN_Y_MAX", false), - _acc_max_up(parent, "MPC_ACC_UP_MAX", false), - _acc_max_down(parent, "MPC_ACC_DOWN_MAX", false) + _vel_z_dz(parent, "MPC_HOLD_MAX_Z", false) + {} bool FlightTaskManualAltitude::activate() { - _yaw_sp_predicted = _yaw_sp = _yaw; - _yaw_rate_sp = _vel_sp_z = NAN; - _pos_sp_z = _pos_sp_z_lock = _position(2); - _lock_time = 0.0f; - _lock_time_max = 1.0f; // 1s time to brake as default - - return FlightTaskManual::activate(); + _pos_sp_z = _position(2); + return FlightTaskManualStabilized::activate(); } -void FlightTaskManualAltitude::scaleSticks() +void FlightTaskManualAltitude::_scaleSticks() { - /* Map stick to velocity. */ + /* Reuse same scaling as for stabilized */ + FlightTaskManualStabilized::_scaleSticks(); + + /* Scale stick exponentially */ const float vel_max_z = (_sticks(2) > 0.0f) ? _vel_max_down.get() : _vel_max_up.get(); _vel_sp_z = vel_max_z * _sticks_expo(2); - _yaw_rate_sp = _sticks(3) * math::radians(_yaw_rate_scaling.get()); } -void FlightTaskManualAltitude::updateHeadingSetpoints() -{ - /* Yaw-lock depends on stick input. If locked, - * yawspeed_sp is set to NAN. Otherwise yaw_sp is set - * to NAN.*/ - - if (fabsf(_sticks(3)) < _hold_dz.get()) { - /* Hold yaw */ - _yaw_rate_sp = NAN; - _yaw_sp = _yaw_sp_predicted; - - } else { - /* Change yaw through yaw-rate inputs.*/ - _yaw_sp = NAN; - _yaw_sp_predicted = _wrap_pi(_yaw_sp_predicted + _yaw_rate_sp * _deltatime); - } -} - -void FlightTaskManualAltitude::updateZsetpoints() +void FlightTaskManualAltitude::_updateZsetpoints() { /* Depending on stick inputs, position is locked or * velocity setpoint is used. If locked, velocity setpoint * is set to NAN. Otherwise position setpoints is set to NAN. */ - if (fabsf(_sticks_expo(2)) < FLT_EPSILON) { - /* Hold position */ - - if (_lock_time <= _lock_time_max) { - /* Don't lock: time has not been reached yet. */ - _vel_sp_z = 0.0f; - _pos_sp_z = NAN; - _pos_sp_z_lock = _position(2); - _lock_time += _deltatime; - - } else { - /* Lock position in z. */ - _vel_sp_z = NAN; - _pos_sp_z = _pos_sp_z_lock; - } - - } else { - /* Change z-direction based on velocity input. Hence, set - * position setpoin in z to NAN since not used. */ - _pos_sp_z = NAN; - - /* Maximum brake acceleration depends - * on direction (up or down). We take half of max acceleration - * because it is better to lock late than early to prevent - * up and down movement. - */ - float brake_acc = (_velocity(2) > 0.0f) ? (0.5f * _acc_max_up.get()) : (0.5f * _acc_max_down.get());; - if (PX4_ISFINITE(brake_acc)) { - _lock_time_max = fabsf(_velocity(2)) / brake_acc; + /* handle position and altitude hold */ + const bool stick_z_zero = fabsf(_sticks_expo(2)) <= FLT_EPSILON; + const bool stopped_z = (_vel_z_dz.get() < FLT_EPSILON || fabsf(_velocity(2)) < _vel_z_dz.get()); - } else { - _lock_time_max = 1.0f; // 1 second time to brake if no acceleration is set - } + if (stick_z_zero && stopped_z && !PX4_ISFINITE(_pos_sp_z)) { + _pos_sp_z = _position(2); - _lock_time = 0.0f; + } else if (!stick_z_zero) { + _pos_sp_z = NAN; } } -void FlightTaskManualAltitude::updateSetpoints() +void FlightTaskManualAltitude::_updateSetpoints() { - updateHeadingSetpoints(); - updateZsetpoints(); + FlightTaskManualStabilized::_updateSetpoints(); // get yaw and thrust setpoints + _updateZsetpoints(); // get z setpoints } bool FlightTaskManualAltitude::update() { - scaleSticks(); // scales sticks to yawspeed and velocity - updateSetpoints(); // applies yaw and position lock if required + _scaleSticks(); + _updateSetpoints(); + _setPositionSetpoint(Vector3f(NAN, NAN, _pos_sp_z)); _setVelocitySetpoint(Vector3f(NAN, NAN, _vel_sp_z)); _setYawSetpoint(_yaw_sp); _setYawspeedSetpoint(_yaw_rate_sp); + //_setThrustSetpoint(...) TODO return true; } diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp index 9be5128a8b..d78479edeb 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp @@ -39,9 +39,9 @@ #pragma once -#include "FlightTaskManual.hpp" +#include "FlightTaskManualStabilized.hpp" -class FlightTaskManualAltitude : public FlightTaskManual +class FlightTaskManualAltitude : public FlightTaskManualStabilized { public: FlightTaskManualAltitude(control::SuperBlock *parent, const char *name); @@ -53,26 +53,16 @@ public: bool update() override; protected: - float _yaw_rate_sp{}; /**< Scaled yaw rate from stick. NAN if yaw is locked. */ - float _yaw_sp{}; /**< Yaw setpoint once locked. Otherwise NAN. */ - float _yaw_sp_predicted{}; /** Equal to_yaw_sp during lock. Predicted yaw_sp during non-lock.*/ float _vel_sp_z{}; /**< Scaled velocity from stick. During altitude lock it is equal to NAN. */ float _pos_sp_z{}; /**< Setpoint in z during lock. Otherwise NAN. */ - float _pos_sp_z_lock{}; /**< Setpoint in z when lock is engaged. */ control::BlockParamFloat _vel_max_down; /**< Maximum speed allowed to go up. */ control::BlockParamFloat _vel_max_up; /**< Maximum speed allowed to go down. */ - control::BlockParamFloat _yaw_rate_scaling; /**< Scaling factor from stick to yaw rate. */ - control::BlockParamFloat _acc_max_up; /**< Maximum acceleration upward. */ - control::BlockParamFloat _acc_max_down; /**< Maximum acceleration downward. */ + control::BlockParamFloat _vel_z_dz; /**< velocity threshold/deadzone to switch into vertical position hold */ - virtual void updateSetpoints(); /**< Updates all setpoints. */ - virtual void scaleSticks(); /**< Scales sticks to velocity. */ + void _updateSetpoints() override; /**< Updates all setpoints. */ + void _scaleSticks() override; /**< Scales sticks to velocity in z. */ private: - void updateHeadingSetpoints(); /**< Sets yaw or yaw speed. */ - void updateZsetpoints(); /**< Sets position or velocity setpoints. */ - - float _lock_time_max{0.0f}; /**< Defines time when altitude lock occurs. */ - float _lock_time{0.0f}; /**< Time passed when stick is at center position and before lock occurs.*/ + void _updateZsetpoints(); /**< Sets position or velocity setpoints. */ };