From 1317844a94a5e1b79a8c326168c5c92d7265518d Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Thu, 3 May 2018 07:33:22 +0200 Subject: [PATCH] ManualSmoothingXY fix comments and style --- .../tasks/Utility/ManualSmoothingXY.cpp | 80 +++++------- .../tasks/Utility/ManualSmoothingXY.hpp | 121 ++++++++++++------ 2 files changed, 117 insertions(+), 84 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp index 7a85aeff39..66f003993c 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp @@ -61,19 +61,13 @@ void 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: - * 1. user wants to stop - * 2. user wants to quickly change direction - * 3. user wants to accelerate - * 4. user wants to decelerate - */ + + 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. - */ - _getStateAcceleration(vel_sp, vel, intention, dt); + // Adapt acceleration and jerk based on current state and + // intention. Jerk is only used for braking. + _setStateAcceleration(vel_sp, vel, intention, dt); } ManualSmoothingXY::Intention @@ -82,13 +76,13 @@ ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::V { if (vel_sp.length() > FLT_EPSILON) { - /* Distinguish between acceleration, deceleration and direction change */ + // Distinguish between acceleration, deceleration and direction change - /* 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 detection is done in body frame. */ - /* Rotate velocity setpoint into body frame */ + // 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 detection is done in body frame. + // Rotate velocity setpoint into body frame matrix::Vector2f vel_sp_heading = _getWorldToHeadingFrame(vel_sp, yaw); matrix::Vector2f vel_heading = _getWorldToHeadingFrame(vel, yaw); @@ -102,9 +96,9 @@ ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::V const bool is_aligned = (vel_sp_heading * vel_heading) > -0.707f; - /* In almost all cases we want to use acceleration. - * 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. */ + // In almost all cases we want to use acceleration. + // 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_max) && !yawspeed_demand && (vel.length() > 2.0f); @@ -125,29 +119,26 @@ ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::V } void -ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, +ManualSmoothingXY::_setStateAcceleration(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const Intention &intention, const float dt) { - switch (intention) { case Intention::brake: { if (_intention == Intention::direction_change) { - /* Vehicle switched from direciton change to brake. - * Don't do any slwerate but rather just stop. - */ + // Vehicle switched from direction change to brake. + // Don't do any slwerate and just stop. _jerk_state_dependent = 1e6f; _vel_sp_prev = vel; } else if (intention != _intention) { - /* we start braking with lowest acceleration - * This make stopping smoother. */ + // start the brake with lowest acceleration which + // makes stopping smoother _acc_state_dependent = _dec_xy_min.get(); - /* Adjust jerk based on current velocity, This ensures - * that the vehicle will stop much quicker at large speed but - * very slow at low speed. - */ + // Adjust jerk based on current velocity. This ensures + // that the vehicle will stop much quicker at large speed but + // very slow at low speed. _jerk_state_dependent = 1e6f; // default if (_jerk_max.get() > _jerk_min.get()) { @@ -156,9 +147,9 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m / _vel_max * vel.length() + _jerk_min.get(), _jerk_max.get()); } - /* Since user wants to brake smoothly but NOT continuing to fly - * in the opposite direction, we have to reset the slewrate - * by setting previous velocity setpoint to current velocity. */ + // User wants to brake smoothly but does NOT want the vehicle to + // continue to fly in the opposite direction. slewrate has to be reset + // by setting previous velocity setpoint to current velocity. */ _vel_sp_prev = vel; } @@ -177,23 +168,19 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m } case Intention::direction_change: { + // We allow for fast change by setting previous setpoint to current setpoint. - /* We allow for fast change by setting previous setpoint to current - * setpoint. - */ - _vel_sp_prev = vel_sp; - /* Because previous setpoint is equal to current setpoint, - * slewrate will have no effect. Nonetheless, just set - * acceleration to maximum. - */ + // Because previous setpoint is equal to current setpoint, + // slewrate will have no effect. Nonetheless, just set + // acceleration to maximum. _acc_state_dependent = _acc_xy_max.get(); break; } case Intention::acceleration: { - /* Limit acceleration linearly based on velocity setpoint.*/ + // Limit acceleration linearly based on velocity setpoint. _acc_state_dependent = (_acc_xy_max.get() - _dec_xy_min.get()) / _vel_max * vel_sp.length() + _dec_xy_min.get(); break; @@ -205,18 +192,17 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m } } - /* Update intention for next iteration */ + // Update intention for next iteration. _intention = intention; } void ManualSmoothingXY::_velocitySlewRate(matrix::Vector2f &vel_sp, const float dt) { - /* Adjust velocity setpoint if demand exceeds acceleration. */ + // Adjust velocity setpoint if demand exceeds acceleration. / 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; } } @@ -224,7 +210,6 @@ ManualSmoothingXY::_velocitySlewRate(matrix::Vector2f &vel_sp, const float dt) matrix::Vector2f ManualSmoothingXY::_getWorldToHeadingFrame(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)); @@ -233,7 +218,6 @@ ManualSmoothingXY::_getWorldToHeadingFrame(const matrix::Vector2f &vec, const fl matrix::Vector2f ManualSmoothingXY::_getHeadingToWorldFrame(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_world = q_yaw.conjugate(matrix::Vector3f(vec(0), vec(1), 0.0f)); return matrix::Vector2f(&vec_world(0)); diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp index 024b4dde6a..c2b5bfc299 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp @@ -34,7 +34,15 @@ /** * @file SmoothingXY.hpp * - * This Class is used for smoothing the velocity setpoints in Z-direction. + * This Class is used for smoothing the velocity setpoint in XY-direction. + * The velocity setpoint is smoothed by applying a velocity change limit, which + * we call acceleration. Depending on the user intention, the acceleration limit + * will differ. + * In manual mode we consider four states with different acceleration handling: + * 1. user wants to stop + * 2. user wants to quickly change direction + * 3. user wants to accelerate + * 4. user wants to decelerate */ #pragma once @@ -50,9 +58,9 @@ public: /** * 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. + * Maximum velocity depends on flight-task. + * In order to deduce user intention from velocity, the maximum + * allowed velocity has to be updated. * @param vel_max corresponds to vehicle constraint */ void updateMaxVelocity(const float &vel_max) {_vel_max = vel_max;}; @@ -66,19 +74,28 @@ public: 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 */ + /* + * User intention. + * - brake when user demands a brake + * - acceleration when vehicle keeps speed or accelerates in the same direction + * - deceleration when vehicle slows down in the same direction + * - direction_change whne vehcile demands an abrupt direction change + */ enum class Intention { brake, acceleration, deceleration, direction_change }; - - /* Getter methods */ + /** + * Get user intention. + * @see Intention + */ Intention getIntention() { return _intention; } - /* Overwrite methods: - * Needed if different parameter values than default required. + /** + * Overwrite methods: + * Needed if different parameter values than default required. */ void overwriteHoverAcceleration(float acc) { _acc_hover.set(acc); } void overwriteMaxAcceleration(float acc) { _acc_xy_max.set(acc); } @@ -87,40 +104,72 @@ public: void overwriteJerkMin(float jerk) { _jerk_min.set(jerk); } private: + /** + * Sets velocity change limits (=acceleration). + * Depending on the user intention, the acceleration differs. + * @param vel_sp is desired velocity setpoint before slewrate. + * @param vel is current velocity in horizontal direction + * @param yaw is vehicle yaw + * @param yawrate_sp is desired yawspeed + * @param dt is delta-time + */ 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 _getWorldToHeadingFrame(const matrix::Vector2f &vec, const float &yaw); - matrix::Vector2f _getHeadingToWorldFrame(const matrix::Vector2f &vec, const float &yaw); - - /* User intention: brake or acceleration */ - Intention _intention{Intention::acceleration}; - - /* Acceleration that depends on vehicle state - * _acc_max_down <= _acc_state_dependent <= _acc_max_up - */ - float _acc_state_dependent; - float _jerk_state_dependent; /** - * Maximum velocity. - * It is used to deduce user intention. + * Gets user intention. + * The intention is deduced from desired velocity setpoint. + * @param vel_sp is desired velocity setpoint before slewrate. + * @param vel is vehicle velocity in xy-direction. + * @param yaw is vehicle yaw + * @param yawrate_sp is the desired yaw-speed */ - float _vel_max{10.0f}; + Intention _getIntention(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw, + const float &yawrate_sp); - /* Previous setpoints */ - matrix::Vector2f _vel_sp_prev; // previous velocity setpoint + /** + * Set acceleration depending on Intention. + * @param vel_sp is desired velocity septoint in xy-direction + * @param vel is vehicle velociy in xy-direction + * @param intention is the user intention during flight + * @param dt is delta-time + */ + void _setStateAcceleration(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const Intention &intention, + const float dt); + + /** + * Limits the velocity setpoint change. + * @param vel_sp that gets limited based on acceleration and previous velocity setpoint + * @param dt is delta-time + */ + void _velocitySlewRate(matrix::Vector2f &vel_sp, const float dt); + + /** + * Rotate vector from local frame into heading frame. + * @param vec is an arbitrary vector in local frame + * @param yaw is the vehicle heading + */ + matrix::Vector2f _getWorldToHeadingFrame(const matrix::Vector2f &vec, const float &yaw); + + /** + * Rotate vector from heading frame to local frame. + * @param vec is an arbitrary vector in heading frame + * @param yaw is the vehicle yaw + */ + matrix::Vector2f _getHeadingToWorldFrame(const matrix::Vector2f &vec, const float &yaw); + + Intention _intention{Intention::acceleration}; /**< user intention */ + float _acc_state_dependent; /**< velocity change limit that depends on Intention */ + float _jerk_state_dependent; /**< acceleration change limit during brake */ + float _vel_max{10.0f}; /**< maximum horizontal speed allowed */ + matrix::Vector2f _vel_sp_prev; /**< previous velocity setpoint */ DEFINE_PARAMETERS( - (ParamFloat) _acc_hover, ///< acceleration in hover - (ParamFloat) _acc_xy_max, ///< acceleration in flight - (ParamFloat) _dec_xy_min, ///< deceleration in flight - (ParamFloat) _jerk_min, ///< jerk min during brake - (ParamFloat) _jerk_max, ///< jerk max during brake - (ParamFloat) _vel_manual ///< maximum velocity in manual controlled mode + (ParamFloat) _acc_hover, /**< acceleration in hover */ + (ParamFloat) _acc_xy_max, /**< acceleration in flight */ + (ParamFloat) _dec_xy_min, /**< deceleration in flight */ + (ParamFloat) _jerk_min, /**< jerk min during brake */ + (ParamFloat) _jerk_max, /**< jerk max during brake */ + (ParamFloat) _vel_manual /**< maximum velocity in manual controlled mode */ ) };