From 250afd49a93d7e38296777172be5188ab16908c7 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 2 May 2018 18:40:28 +0200 Subject: [PATCH] ManualSmoothingZ: update comments --- .../tasks/Utility/ManualSmoothingZ.cpp | 45 +++++-------- .../tasks/Utility/ManualSmoothingZ.hpp | 67 +++++++++++++------ 2 files changed, 65 insertions(+), 47 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp index a0b08e4afb..6c30f6794d 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp @@ -48,11 +48,6 @@ ManualSmoothingZ::ManualSmoothingZ(ModuleParams *parent, const float &vel, const _max_acceleration = _acc_max_up.get(); } -/* in manual altitude control apply acceleration limit based on stick input - * we consider two states - * 1.) brake - * 2.) accelerate */ - void ManualSmoothingZ::smoothVelFromSticks(float &vel_sp, const float dt) { @@ -64,32 +59,29 @@ ManualSmoothingZ::smoothVelFromSticks(float &vel_sp, const float dt) void ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt) { - /* Check for max acceleration */ + // check for max acceleration setMaxAcceleration(); - /* check if zero input stick */ + // check if zero input stick const bool is_current_zero = (fabsf(_stick) <= FLT_EPSILON); - /* default is acceleration */ + // default is acceleration ManualIntentionZ intention = ManualIntentionZ::acceleration; - /* check zero input stick */ + // check zero input stick if (is_current_zero) { intention = ManualIntentionZ::brake; } - /* - * update intention - */ + + // update intention if ((_intention != ManualIntentionZ::brake) && (intention == ManualIntentionZ::brake)) { - /* we start with lowest acceleration */ + // we start with lowest acceleration _acc_state_dependent = _acc_max_down.get(); - /* reset slew-rate: this ensures that there - * is no delay present when user demands to brake - */ - + // reset slew-rate: this ensures that there + // is no delay present when user demands to brake _vel_sp_prev = _vel; } @@ -97,7 +89,7 @@ ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt) switch (intention) { case ManualIntentionZ::brake: { - /* limit jerk when braking to zero */ + // limit jerk when braking to zero float jerk = (_acc_max_up.get() - _acc_state_dependent) / dt; if (jerk > _jerk_max.get()) { @@ -124,30 +116,27 @@ ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt) void ManualSmoothingZ::setMaxAcceleration() { - /* Note: NED frame */ - if (_stick < -FLT_EPSILON) { - /* accelerating upward */ + // accelerating upward _max_acceleration = _acc_max_up.get(); } else if (_stick > FLT_EPSILON) { - /* accelerating downward */ + // accelerating downward _max_acceleration = _acc_max_down.get(); } else { - /* want to brake */ - + // want to brake if (fabsf(_vel_sp_prev) < FLT_EPSILON) { - /* at rest */ + // at rest _max_acceleration = _acc_max_up.get(); } else if (_vel_sp_prev < 0.0f) { - /* braking downward */ + // braking downward _max_acceleration = _acc_max_down.get(); } else { - /* braking upward */ + // braking upward _max_acceleration = _acc_max_up.get(); } } @@ -156,7 +145,7 @@ ManualSmoothingZ::setMaxAcceleration() void ManualSmoothingZ::velocitySlewRate(float &vel_sp, const float dt) { - /* limit vertical acceleration */ + // limit vertical acceleration float acc = (vel_sp - _vel_sp_prev) / dt; float max_acc = (acc < 0.0f) ? -_acc_state_dependent : _acc_state_dependent; diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp index 12858c9eea..b5f3063022 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp @@ -35,13 +35,18 @@ * @file SmoothingZ.hpp * * This Class is used for smoothing the velocity setpoints in Z-direction. + * In manual altitude control apply acceleration limit based on stick input. */ #pragma once #include -/* User intention: brake or acceleration */ +/** + * User-intention. + * - brake: vehicle should stop + * - acceleration: vehicle should move up or down + */ enum class ManualIntentionZ { brake, acceleration, @@ -54,21 +59,27 @@ public: ~ManualSmoothingZ() = default; /** - * Smooths velocity setpoint based - * on flight direction. + * Smooth velocity setpoint based on flight direction. * @param vel_sp[2] array: vel_sp[0] = current velocity set-point; * vel_sp[1] = previous velocity set-point - * vel_sp will contain smoothed current previous set-point. + * vel_sp will contain smoothed current / previous set-point. * @param dt: time delta in seconds */ void smoothVelFromSticks(float &vel_sp, const float dt); - - /* Getter methods */ + /** + * Get max accleration. + */ float getMaxAcceleration() { return _max_acceleration; } + + /** + * Get user intention. + * @see ManualIntentionZ + */ ManualIntentionZ getIntention() { return _intention; } - /* Overwrite methods: + /* + * Overwrite methods: * Needed if different parameter values than default required. */ void overwriteAccelerationUp(float acc_max_up) { _acc_max_up.set(acc_max_up); } @@ -76,25 +87,43 @@ public: void overwriteJerkMax(float jerk_max) {_jerk_max.set(jerk_max); } private: + /** + * Add delay to velocity setpoint change. + * This method is used to smooth the velocity setpoint change. + * @param vel_sp current velocity setpoint + * @param dt delta-time + */ void velocitySlewRate(float &vel_sp, const float dt); + + /** + * Computes the velocity setpoint change limit. + * This method computes the limit with which the velocity setpoint change + * is limited. + * @see velocitySlewRate + * @param vel_sp current velocity setpoint + * @param dt delta-time + */ void updateAcceleration(float &vel_sp, const float dt); + + /** + * Set maximum acceleration. + * The maximum acceleration depends on the desired direction (up vs down). + * @see _max_acceleration + */ void setMaxAcceleration(); - /* User intention: brake or acceleration */ + /** + * User intention + * @see ManualIntentionZ + */ ManualIntentionZ _intention{ManualIntentionZ::acceleration}; - /* Dependency injection: vehicle velocity in z-direction; - * stick input in z-direction - */ - const float &_vel; - const float &_stick; + const float &_vel; /**< vehicle velocity (dependency injection) */ + const float &_stick; /**< stick input (dependency injection) */ - /* Acceleration that depends on vehicle state - * _acc_max_down <= _acc_state_dependent <= _acc_max_up - */ - float _acc_state_dependent; - float _max_acceleration; // can be up or down maximum accleration - float _vel_sp_prev; + float _acc_state_dependent; /**< acceleration that depends on _intention */ + float _max_acceleration; /**< can be up or down maximum acceleration */ + float _vel_sp_prev; /**< previous velocity setpoint */ DEFINE_PARAMETERS( (ParamFloat) _acc_max_up,