From 4f17fb1303823495ddfe6e504e70e827c43c31d1 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 10 Jan 2018 08:39:05 +0100 Subject: [PATCH] ManualSmoothingXY: detect alignement based on body frame; do direction change only if not yawspeed is demanded --- .../tasks/Utility/ManualSmoothingXY.cpp | 78 ++++++++++++------- .../tasks/Utility/ManualSmoothingXY.hpp | 17 ++-- 2 files changed, 64 insertions(+), 31 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp index efcc4233bf..00417e42a9 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp @@ -41,7 +41,7 @@ #include ManualSmoothingXY::ManualSmoothingXY(const matrix::Vector2f &vel) : - _vel(vel), _vel_sp_prev(vel) + _vel_sp_prev(vel) { _acc_hover_h = param_find("MPC_ACC_HOR_MAX"); _acc_xy_max_h = param_find("MPC_ACC_HOR"); @@ -55,11 +55,12 @@ ManualSmoothingXY::ManualSmoothingXY(const matrix::Vector2f &vel) : } void -ManualSmoothingXY::smoothVelFromSticks(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float dt) +ManualSmoothingXY::smoothVelocity(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw, + const float &yawrate_sp, const float dt) { _updateParams(); - _updateAcceleration(vel_sp, vel, dt); + _updateAcceleration(vel_sp, vel, yaw, yawrate_sp, dt); _velocitySlewRate(vel_sp, dt); } @@ -89,7 +90,8 @@ ManualSmoothingXY::_setParams() } void -ManualSmoothingXY::_updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float dt) +ManualSmoothingXY::_updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw, + const float &yawrate_sp, const float dt) { /* * In manual mode we consider four states with different acceleration handling: @@ -98,7 +100,7 @@ ManualSmoothingXY::_updateAcceleration(matrix::Vector2f &vel_sp, const matrix::V * 3. user wants to accelerate * 4. user wants to decelerate */ - Intention intention = _getIntention(vel_sp); + Intention intention = _getIntention(vel_sp, vel, yaw, yawrate_sp); /* Adapt acceleration and jerk based on current state and * intention. Jerk is only used for braking. @@ -107,44 +109,51 @@ ManualSmoothingXY::_updateAcceleration(matrix::Vector2f &vel_sp, const matrix::V /* Smooth velocity setpoint based on acceleration */ _velocitySlewRate(vel_sp, dt); + + _vel_sp_prev = vel_sp; } ManualSmoothingXY::Intention -ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp) +ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw, + const float &yawrate_sp) { + if (vel_sp.length() > FLT_EPSILON) { /* Distinguish between acceleration, deceleration and direction change */ - /* Check if stick direction and current velocity are within 120. - * If current setpoint and velocity are more than 120 apart, we assume + /* Check if stick direction and current velocity are within 135. + * If current setpoint and velocity are more than 135 apart, we assume * that the user demanded a direction change. - * The detecton has to happen in body frame.*/ - matrix::Vector2f vel_sp_unit = vel_sp;; - matrix::Vector2f vel_sp_prev_unit = _vel_sp_prev; + * The detection is done in body frame. */ + /* Rotate velocity setpoint into body frame */ + matrix::Vector2f vel_sp_heading = _getInHeadingFrame(vel_sp, yaw); + matrix::Vector2f vel_heading = _getInHeadingFrame(vel, yaw); - if (vel_sp.length() > FLT_EPSILON) { - vel_sp_unit.normalize(); + if (vel_sp_heading.length() > FLT_EPSILON) { + vel_sp_heading.normalize(); } - if (_vel_sp_prev.length() > FLT_EPSILON) { - vel_sp_prev_unit.normalize(); + if (vel_heading.length() > FLT_EPSILON) { + vel_heading.normalize(); } - const bool is_aligned = (vel_sp_unit * vel_sp_prev_unit) > -0.5f; + const bool is_aligned = (vel_sp_heading * vel_heading) > -0.707f; - /* Check if user wants to accelerate */ - bool do_acceleration = _vel_sp_prev.length() < FLT_EPSILON; // Because current is not zero but previous sp was zero - do_acceleration = do_acceleration || (is_aligned - && (vel_sp.length() >= _vel_sp_prev.length() - 0.01f)); //User demands larger or same speed - - if (do_acceleration) { - return Intention::acceleration; + /* In almost all cases we want to use acceleration. + * Only use direction change if not aligned, no yawspeed demand and demand larger than 0.7 of max speed. + * Only use deceleration if stick input is lower than previous setpoint, aligned and no yawspeed demand. */ + bool yawspeed_demand = fabsf(yawrate_sp) > 0.05f && PX4_ISFINITE(yawrate_sp); + bool direction_change = !is_aligned && (vel_sp.length() > 0.7f * _vel_manual) && !yawspeed_demand; + bool deceleration = is_aligned && (vel_sp.length() < _vel_sp_prev.length()) && !yawspeed_demand; - } else if (!is_aligned) { + if (direction_change) { return Intention::direction_change; - } else { + } else if (deceleration) { return Intention::deceleration; + + } else { + return Intention::acceleration; } } @@ -237,8 +246,25 @@ ManualSmoothingXY::_velocitySlewRate(matrix::Vector2f &vel_sp, const float dt) matrix::Vector2f acc = (vel_sp - _vel_sp_prev) / dt; if (acc.length() > _acc_state_dependent) { + vel_sp = acc.normalized() * _acc_state_dependent * dt + _vel_sp_prev; } +} - _vel_sp_prev = vel_sp; +matrix::Vector2f +ManualSmoothingXY::_getInHeadingFrame(const matrix::Vector2f &vec, const float &yaw) +{ + + matrix::Quatf q_yaw = matrix::AxisAnglef(matrix::Vector3f(0.0f, 0.0f, 1.0f), yaw); + matrix::Vector3f vec_heading = q_yaw.conjugate_inversed(matrix::Vector3f(vec(0), vec(1), 0.0f)); + return matrix::Vector2f(&vec_heading(0)); +} + +matrix::Vector2f +ManualSmoothingXY::_getInWorldFrame(const matrix::Vector2f &vec, const float &yaw) +{ + + matrix::Quatf q_yaw = matrix::AxisAnglef(matrix::Vector3f(0.0f, 0.0f, 1.0f), yaw); + matrix::Vector3f vec_heading = q_yaw.conjugate(matrix::Vector3f(vec(0), vec(1), 0.0f)); + return matrix::Vector2f(&vec_heading(0)); } diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp index ec98496688..59a07b2f19 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp @@ -53,7 +53,8 @@ public: * @param vel_sp: velocity setpoint in xy * @param dt: time delta in seconds */ - void smoothVelFromSticks(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float dt); + void smoothVelocity(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw, + const float &yawrate_sp, const float dt); /* User intention: brake or acceleration */ enum class Intention { @@ -87,8 +88,10 @@ private: float _acc_state_dependent{0.0f}; float _jerk_state_dependent{0.0f}; - matrix::Vector2f _vel; // current velocity xy - matrix::Vector2f _vel_sp_prev; // previous velocity setpoint + /* Previous setpoints */ + float _yaw_prev{}; + matrix::Vector2f _vel_sp_prev{}; // previous velocity setpoint + /* Params */ param_t _acc_hover_h{PARAM_INVALID}; @@ -108,10 +111,14 @@ private: /* Helper methods */ void _setParams(); void _updateParams(); - void _updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float dt); - Intention _getIntention(const matrix::Vector2f &vel_sp); + void _updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw, + const float &yawrate_sp, const float dt); + Intention _getIntention(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw, + const float &yawrate_sp); void _getStateAcceleration(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const Intention &intention, const float dt); void _velocitySlewRate(matrix::Vector2f &vel_sp, const float dt); + matrix::Vector2f _getInHeadingFrame(const matrix::Vector2f &vec, const float &yaw) ; + matrix::Vector2f _getInWorldFrame(const matrix::Vector2f &vec, const float &yaw); };