From 5ef2a61be56dbc5b0b69db437b9c8633b36ded5a Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Thu, 26 Apr 2018 14:44:37 +0200 Subject: [PATCH] ManualSmoothingXY: ensure that maximum speed is correctly propagated to the smoothing class --- .../tasks/FlightTaskManualPositionSmooth.cpp | 1 + .../tasks/Utility/ManualSmoothingXY.cpp | 6 +++--- .../tasks/Utility/ManualSmoothingXY.hpp | 16 +++++++++++++++- 3 files changed, 19 insertions(+), 4 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp index edb033d000..1a8d20f296 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp @@ -52,6 +52,7 @@ void FlightTaskManualPositionSmooth::_updateSetpoints() /* Smooth velocity setpoint in xy.*/ matrix::Vector2f vel(&_velocity(0)); Vector2f vel_sp_xy = Vector2f(&_velocity_setpoint(0)); + _smoothingXY.updateMaxVelocity(_constraints.speed_xy); _smoothingXY.smoothVelocity(vel_sp_xy, vel, _yaw, _yawspeed_setpoint, _deltatime); _velocity_setpoint(0) = vel_sp_xy(0); _velocity_setpoint(1) = vel_sp_xy(1); diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp index 34bd925425..4a4d728637 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp @@ -106,7 +106,7 @@ ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::V * Only use direction change if not aligned, no yawspeed demand, demand larger than 0.7 of max speed and velocity larger than 2m/s. * 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.get()) && !yawspeed_demand + bool direction_change = !is_aligned && (vel_sp.length() > 0.7f * _vel_max) && !yawspeed_demand && (vel.length() > 2.0f); bool deceleration = is_aligned && (vel_sp.length() < _vel_sp_prev.length()) && !yawspeed_demand; @@ -153,7 +153,7 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m if (_jerk_max.get() > _jerk_min.get()) { _jerk_state_dependent = math::min((_jerk_max.get() - _jerk_min.get()) - / _vel_manual.get() * vel.length() + _jerk_min.get(), _jerk_max.get()); + / _vel_max * vel.length() + _jerk_min.get(), _jerk_max.get()); } /* Since user wants to brake smoothly but NOT continuing to fly @@ -195,7 +195,7 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m case Intention::acceleration: { /* Limit acceleration linearly based on velocity setpoint.*/ _acc_state_dependent = (_acc_xy_max.get() - _dec_xy_min.get()) - / _vel_manual.get() * vel_sp.length() + _dec_xy_min.get(); + / _vel_max * vel_sp.length() + _dec_xy_min.get(); break; } diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp index 5307e3da23..a768ae806e 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp @@ -48,6 +48,15 @@ public: ManualSmoothingXY(ModuleParams *parent, const matrix::Vector2f &vel); ~ManualSmoothingXY() = default; + /** + * Maximum velocity is required to detect user intention. + * Maximum velocity changes depending on task. Consequently, + * in order to deduce user intention from velocity, the maximum + * allowed velocity has to updated. + * @param vel_max corresponds to vehicle constraint + */ + void updateMaxVelocity(const float &vel_max) {_vel_max = vel_max;}; + /** * Smoothing of velocity setpoint horizontally based * on flight direction. @@ -97,6 +106,12 @@ private: float _acc_state_dependent{0.0f}; float _jerk_state_dependent{0.0f}; + /** + * Maximum velocity. + * It is used to deduce user intention. + */ + float _vel_max{0.0f}; + /* Previous setpoints */ matrix::Vector2f _vel_sp_prev{}; // previous velocity setpoint @@ -108,5 +123,4 @@ private: (ParamFloat) _jerk_max, ///< jerk max during brake (ParamFloat) _vel_manual ///< maximum velocity in manual controlled mode ) - };