From 720271332d7171b595e690dc5b5407006f85768c Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Sun, 24 Dec 2017 16:02:50 +0100 Subject: [PATCH] FlightTaskManual/Alt/Position: update comments --- .../FlightTasks/tasks/FlightTaskManual.cpp | 15 ++++---- .../FlightTasks/tasks/FlightTaskManual.hpp | 12 +++--- .../tasks/FlightTaskManualAltitude.cpp | 38 ++++++++++--------- .../tasks/FlightTaskManualAltitude.hpp | 37 +++++++++--------- .../tasks/FlightTaskManualPosition.cpp | 37 +++++++++--------- .../tasks/FlightTaskManualPosition.hpp | 18 ++++----- 6 files changed, 79 insertions(+), 78 deletions(-) diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp index a626ea2b42..d5c1e8d282 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.cpp @@ -30,14 +30,11 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - /** - * @file FlightTaskManual.hpp + * @file FlightTaskManual.cpp * - * Flight task for the normal, legacy, manual position controlled flight - * where stick inputs map basically to the velocity setpoint + * Linear and exponential map from stick inputs to range -1 and 1. * - * @author Matthias Grob */ #include "FlightTaskManual.hpp" @@ -126,13 +123,16 @@ void FlightTaskManual::_updateYaw() bool FlightTaskManual::_evaluateSticks() { + /* Sticks are rescaled linearly and exponentially from [0,1] to [-1,1] */ if ((_time_stamp_current - _sub_manual_control_setpoint->get().timestamp) < _timeout) { - /* get data and scale correctly */ + + /* Linear scale */ _sticks(0) = _sub_manual_control_setpoint->get().x; /* NED x, "pitch" [-1,1] */ _sticks(1) = _sub_manual_control_setpoint->get().y; /* NED y, "roll" [-1,1] */ _sticks(2) = -(_sub_manual_control_setpoint->get().z - 0.5f) * 2.f; /* NED z, "thrust" resacaled from [0,1] to [-1,1] */ _sticks(3) = _sub_manual_control_setpoint->get().r; /* "yaw" [-1,1] */ + /* Exponential scale */ _sticks_expo(0) = math::expo_deadzone(_sticks(0), _xy_vel_man_expo.get(), _hold_dz.get()); _sticks_expo(1) = math::expo_deadzone(_sticks(1), _xy_vel_man_expo.get(), _hold_dz.get()); _sticks_expo(2) = math::expo_deadzone(_sticks(2), _z_vel_man_expo.get(), _hold_dz.get()); @@ -140,7 +140,8 @@ bool FlightTaskManual::_evaluateSticks() return true; } else { - _sticks.zero(); /* default is all zero */ + /* Timeout: set all sticks to zero */ + _sticks.zero(); _sticks_expo.zero(); return false; } diff --git a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp index c002e44ee7..0a53668676 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManual.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManual.hpp @@ -34,10 +34,8 @@ /** * @file FlightTaskManual.hpp * - * Flight task for the normal, manual position controlled flight - * where stick inputs map basically to the velocity setpoint + * Linear and exponential map from stick inputs to range -1 and 1. * - * @author Matthias Grob */ #pragma once @@ -65,17 +63,17 @@ public: protected: - bool _sticks_data_required = true; /**< let sibling task define if it depends on stick data */ + bool _sticks_data_required = true; /**< let inherited task-class define if it depends on stick data */ matrix::Vector _sticks; /**< unmodified manual stick inputs */ matrix::Vector3f _sticks_expo; /**< modified manual sticks using expo function*/ - control::BlockParamFloat _hold_dz; /**< deadzone around the center for the sticks when flying in position mode */ + control::BlockParamFloat _hold_dz; /**< 0-deadzone around the center for the sticks */ private: uORB::Subscription *_sub_manual_control_setpoint{nullptr}; - control::BlockParamFloat _xy_vel_man_expo; /**< ratio of exponential curve for stick input in xy direction pos mode */ - control::BlockParamFloat _z_vel_man_expo; /**< ratio of exponential curve for stick input in xy direction pos mode */ + control::BlockParamFloat _xy_vel_man_expo; /**< ratio of exponential curve for stick input in xy direction */ + control::BlockParamFloat _z_vel_man_expo; /**< ratio of exponential curve for stick input in z direction */ bool _evaluateSticks(); /**< checks and sets stick inputs */ diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp index 26e3b34963..13e6d38522 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp @@ -32,10 +32,9 @@ ****************************************************************************/ /** - * @file FlightTaskManual.hpp - * - * Flight-task for manual controlled altitude. + * @file FlightManualAltitude.cpp * + * Flight task for manual controlled altitude. */ #include "FlightTaskManualAltitude.hpp" @@ -67,23 +66,25 @@ bool FlightTaskManualAltitude::activate() void FlightTaskManualAltitude::scaleSticks() { - - /* Map stick to velocity */ + /* Map stick to velocity. */ 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()) { - /* Want to hold yaw */ + /* Hold yaw */ _yaw_rate_sp = NAN; _yaw_sp = _yaw_sp_predicted; } else { - /* Want to change yaw based on yaw-rate */ + /* Change yaw through yaw-rate inputs.*/ _yaw_sp = NAN; _yaw_sp_predicted = _wrap_pi(_yaw_sp_predicted + _yaw_rate_sp * _deltatime); } @@ -91,28 +92,33 @@ void FlightTaskManualAltitude::updateHeadingSetpoints() 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) { - - /* Want to hold altitude */ + /* Hold position */ if (_lock_time <= _lock_time_max) { - /* Don't lock: time has not been reached */ + /* 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 { - /* Want to change altitude through velocity */ + /* 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. We take half of max acceleration + * 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. */ @@ -137,10 +143,8 @@ void FlightTaskManualAltitude::updateSetpoints() bool FlightTaskManualAltitude::update() { - scaleSticks(); - updateSetpoints(); - - + scaleSticks(); // scales sticks to yawspeed and velocity + updateSetpoints(); // applies yaw and position lock if required _setPositionSetpoint(Vector3f(NAN, NAN, _pos_sp_z)); _setVelocitySetpoint(Vector3f(NAN, NAN, _vel_sp_z)); _setYawSetpoint(_yaw_sp); diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp index a0a6a8ed7c..9be5128a8b 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp @@ -32,10 +32,9 @@ ****************************************************************************/ /** - * @file FlightTaskOrbit.hpp + * @file FlightManualAltitude.hpp * * Flight task for manual controlled altitude. - * */ #pragma once @@ -54,26 +53,26 @@ public: bool update() override; protected: - float _yaw_rate_sp{}; /** scaled yaw rate directly from stick. NAN if yaw is locked */ - float _yaw_sp{}; /** yaw setpoint once locked. otherwise NAN */ - float _yaw_sp_predicted{}; /** _yaw_sp during lock; predicted yaw through _yaw_rate_sp demand */ - float _vel_sp_z{}; /**< scaled velocity directly from stick. During altitude lock is equal to NAN */ - float _pos_sp_z{}; /**< altitude setpoint in z during lock. Otherwise NAN. */ - float _pos_sp_z_lock{}; /**< altitude which used when lock is engaged */ + 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_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. */ - virtual void updateSetpoints(); /**< updates all setpoints */ - virtual void scaleSticks(); /**< scales sticks to velocity */ + virtual void updateSetpoints(); /**< Updates all setpoints. */ + virtual void scaleSticks(); /**< Scales sticks to velocity. */ private: - void updateHeadingSetpoints(); /**< sets yaw or yaw speed */ - void updateZsetpoints(); /**< sets position or velocity setpoint */ + 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 after stick are at zero position */ + 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.*/ }; diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp index dc88fed421..a6cfb7064f 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp @@ -32,9 +32,9 @@ ****************************************************************************/ /** - * @file FlightTaskManual.hpp + * @file FlightTaskManualPosition.cpp * - * Flight task for manual controlled position. + * Flight task for manual position controlled mode. * */ @@ -62,50 +62,54 @@ bool FlightTaskManualPosition::activate() void FlightTaskManualPosition::scaleSticks() { - /* scale all sticks including altitude and yaw */ + /* Get velocity in z and yawspeed. */ FlightTaskManualAltitude::scaleSticks(); + /* Constrain length of stick inputs to 1. */ matrix::Vector2f stick_xy(_sticks_expo(0), _sticks_expo(1)); - - /* constrain length of stick inputs to 1 */ float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f); stick_xy = stick_xy.normalized() * mag; - /* scale to velocity */ + /* Scale to velocity.*/ _vel_sp_xy = stick_xy * _vel_xy_manual_max.get(); - /* rotate setpoint into local frame */ + /* Rotate setpoint into local frame. */ matrix::Vector3f vel_sp{_vel_sp_xy(0), _vel_sp_xy(1), 0.0f}; vel_sp = (matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, _yaw_sp_predicted)) * vel_sp); _vel_sp_xy = matrix::Vector2f(vel_sp(0), vel_sp(1)); - } void FlightTaskManualPosition::updateXYsetpoints() { + /* If position lock is active, velocity setpoint is set to NAN. Otherwise + * position setpoint is set to NAN. + */ + matrix::Vector2f stick_xy(_sticks_expo(0), _sticks_expo(1)); if (stick_xy.length() < FLT_EPSILON) { - /* Want to hold position */ + /* Hold position */ + if (_lock_time <= _lock_time_max) { - /* Don't lock: time has not been reached */ + /* Don't lock: time has not been reached. */ _vel_sp_xy.zero(); _pos_sp_xy = matrix::Vector2f(NAN, NAN); _pos_sp_xy_lock = matrix::Vector2f(&(_position(0))); _lock_time += _deltatime; } else { - /* want to hold position */ + /*Lock position. */ _vel_sp_xy = matrix::Vector2f(NAN, NAN); _pos_sp_xy = matrix::Vector2f(&(_pos_sp_xy_lock(0))); } } else { - /* Want to change position */ + /* Move based on velocity. Position setpoint + * not used and therefore set to NAN. */ _pos_sp_xy = matrix::Vector2f(NAN, NAN); - /* Time to brake depends on + /* Always update time to brake. It depends on * maximum acceleration. */ if (PX4_ISFINITE(_acc_xy_max.get())) { @@ -124,20 +128,15 @@ void FlightTaskManualPosition::updateXYsetpoints() void FlightTaskManualPosition::updateSetpoints() { - /* scale all sticks */ - scaleSticks(); - - /* update all setpoints */ + /* Apply position lock if required. */ FlightTaskManualAltitude::updateSetpoints(); updateXYsetpoints(); - } bool FlightTaskManualPosition::update() { scaleSticks(); updateSetpoints(); - _setPositionSetpoint(Vector3f(_pos_sp_xy(0), _pos_sp_xy(1), _pos_sp_z)); _setVelocitySetpoint(Vector3f(_vel_sp_xy(0), _vel_sp_xy(1), _vel_sp_z)); _setYawSetpoint(_yaw_sp); diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp index 9b605fa7b7..5fb11bbb7b 100644 --- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp @@ -32,9 +32,9 @@ ****************************************************************************/ /** - * @file FlightTaskOrbit.hpp + * @file FlightTaskManualPosition.hpp * - * Flight task for manual controlled position. + * Flight task for manual position controlled mode. * */ @@ -54,18 +54,18 @@ public: bool update() override; protected: - matrix::Vector2f _vel_sp_xy{}; /* Scaled velocity setpoint from stick. NAN during position lock. */ - matrix::Vector2f _pos_sp_xy{}; /* Position setpoint during lock. Otherwise NAN.*/ - matrix::Vector2f _pos_sp_xy_lock{}; /* Position setpoint during lock */ + matrix::Vector2f _vel_sp_xy{}; /**< Scaled velocity setpoint from stick. NAN during position lock. */ + matrix::Vector2f _pos_sp_xy{}; /**< Position setpoint during lock. Otherwise NAN.*/ + matrix::Vector2f _pos_sp_xy_lock{}; /**< Position setpoint during lock */ - control::BlockParamFloat _vel_xy_manual_max; /**< maximum speed allowed horizontally */ - control::BlockParamFloat _acc_xy_max; + control::BlockParamFloat _vel_xy_manual_max; /**< Maximum speed allowed horizontally, */ + control::BlockParamFloat _acc_xy_max;/**< Maximum acceleration horizontally. Only used to compute lock time. */ void updateSetpoints() override; void scaleSticks() override; private: void updateXYsetpoints(); - float _lock_time_max{0.0f}; /**< defines time when position lock occurs */ - float _lock_time{0.0f}; /**< time after stick are at zero position */ + float _lock_time_max{0.0f}; /**< Defines time when position lock occurs. */ + float _lock_time{0.0f}; /**< Time passed when stick are at zero position. */ };