From 26889bc0e14e4978a1f23dac856e3c8f49e7fda0 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Fri, 5 Jan 2018 16:46:11 +0100 Subject: [PATCH] ManualSmoothing in xy --- src/lib/FlightTasks/CMakeLists.txt | 1 + .../tasks/Utility/ManualSmoothingXY.cpp | 241 ++++++++++++++++++ .../tasks/Utility/ManualSmoothingXY.hpp | 117 +++++++++ 3 files changed, 359 insertions(+) create mode 100644 src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp create mode 100644 src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp diff --git a/src/lib/FlightTasks/CMakeLists.txt b/src/lib/FlightTasks/CMakeLists.txt index 981582b365..34f440b052 100644 --- a/src/lib/FlightTasks/CMakeLists.txt +++ b/src/lib/FlightTasks/CMakeLists.txt @@ -41,6 +41,7 @@ px4_add_module( tasks/FlightTaskManualAltitudeSmooth.cpp tasks/FlightTaskManualPosition.cpp tasks/Utility/ManualSmoothingZ.cpp + tasks/Utility/ManualSmoothingXY.cpp SubscriptionArray.cpp FlightTasks.cpp DEPENDS diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp new file mode 100644 index 0000000000..68fba24027 --- /dev/null +++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp @@ -0,0 +1,241 @@ +/**************************************************************************** + * + * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ManualSmoothingXY.cpp + */ + +#include "ManualSmoothingXY.hpp" +#include "uORB/topics/parameter_update.h" +#include +#include + +ManualSmoothingXY::ManualSmoothingXY(const matrix::Vector2f &vel) : + _vel(vel), _vel_sp_prev(vel) +{ + _acc_hover_h = param_find("MPC_ACC_HOR_MAX"); + _acc_xy_max_h = param_find("MPC_ACC_HOR"); + _dec_xy_min_h = param_find("DEC_HOR_SLOW"); + _jerk_max_h = param_find("MPC_JERK_MAX"); + _jerk_min_h = param_find("MPC_JERK_MIN"); + _vel_manual_h = param_find("MPC_VEL_MANUAL"); + + /* Load the params the very first time */ + _setParams(); +} + +void +ManualSmoothingXY::smoothVelFromSticks(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float dt) +{ + _updateParams(); + + _updateAcceleration(vel_sp, vel, dt); + + _velocitySlewRate(vel_sp, dt); +} + +void +ManualSmoothingXY::_updateParams() +{ + bool updated; + parameter_update_s param_update; + orb_check(_parameter_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(parameter_update), _parameter_sub, ¶m_update); + _setParams(); + } +} + +void +ManualSmoothingXY::_setParams() +{ + param_get(_acc_hover_h, &_acc_hover); + param_get(_acc_xy_max_h, &_acc_xy_max); + param_get(_dec_xy_min_h, &_dec_xy_min); + param_get(_jerk_max_h, &_jerk_max); + param_get(_jerk_min_h, &_jerk_min); + param_get(_vel_manual_h, &_vel_manual); +} + +void +ManualSmoothingXY::_updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, 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); + + /* Adapt acceleration and jerk based on current state and + * intention. Jerk is only used for braking. + */ + _getStateAcceleration(vel_sp, vel, intention, dt); + + /* Smooth velocity setpoint based on acceleration */ + _velocitySlewRate(vel_sp, dt); +} + +ManualSmoothingXY::Intention +ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_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 previous and current setpoint are more than 120 apart, we assume + * that the user demanded a direction change. */ + matrix::Vector2f vel_sp_unit = vel_sp;; + matrix::Vector2f vel_sp_prev_unit = _vel_sp_prev; + + if (vel_sp.length() > FLT_EPSILON) { + vel_sp_unit.normalize(); + } + + if (_vel_sp_prev.length() > FLT_EPSILON) { + vel_sp_prev_unit.normalize(); + } + + const bool is_aligned = (vel_sp_unit * vel_sp_prev_unit) > -0.5f; + + /* 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; + + } else if (!is_aligned) { + return Intention::direction_change; + + } else { + return Intention::deceleration; + } + } + + return Intention::brake; //default is brake +} + +void +ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, + const Intention &intention, const float dt) +{ + + switch (intention) { + case Intention::brake: { + + /* First iteration where user demands brake */ + if (intention != intention) { + /* we start braking with lowest acceleration + * This make stopping smoother. */ + _acc_state_dependent = _dec_xy_min; + + /* 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 = _jerk_max; // default + + if (_jerk_max > _jerk_min) { + + _jerk_state_dependent = (_jerk_max - _jerk_min) + / _vel_manual * vel.length() + _jerk_min; + } + + /* 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. */ + _vel_sp_prev = vel; + } + + /* limit jerk when braking to zero */ + float jerk = (_acc_hover - _acc_state_dependent) / dt; + + if (jerk > _jerk_state_dependent) { + _acc_state_dependent = _jerk_state_dependent * dt + + _acc_state_dependent; + + } else { + _acc_state_dependent = _acc_hover; + } + + break; + } + + case Intention::direction_change: { + + /* 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. + */ + _acc_state_dependent = _acc_xy_max; + + break; + } + + case Intention::acceleration: { + /* Limit acceleration linearly based on velocity setpoint.*/ + _acc_state_dependent = (_acc_xy_max - _dec_xy_min) + / _vel_manual * vel_sp.length() + _dec_xy_min; + break; + } + + case Intention::deceleration: { + _acc_state_dependent = _dec_xy_min; + break; + } + } + + /* Update intention for next iteration */ + _intention = intention; +} + +void +ManualSmoothingXY::_velocitySlewRate(matrix::Vector2f &vel_sp, const float dt) +{ + /* 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; + } +} diff --git a/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp new file mode 100644 index 0000000000..ec98496688 --- /dev/null +++ b/src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.hpp @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file SmoothingXY.hpp + * + * This Class is used for smoothing the velocity setpoints in Z-direction. + */ + +#pragma once + +#include +#include + +class ManualSmoothingXY +{ +public: + ManualSmoothingXY(const matrix::Vector2f &vel); + ~ManualSmoothingXY() {}; + + /* Smoothing of velocity setpoint horizontally based + * on flight direction. + * @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); + + /* User intention: brake or acceleration */ + enum class Intention { + brake, + acceleration, + deceleration, + direction_change + }; + + /* Getter methods */ + //float getMaxAcceleration(); + Intention getIntention() {return _intention;}; + + /* Overwrite methods: + * Needed if different parameter values than default required. + */ + void overwriteHoverAcceleration(float acc) {_acc_hover = acc;}; + void overwriteMaxAcceleration(float acc) {_acc_xy_max = acc;}; + void overwriteDecelerationMin(float dec) {_dec_xy_min = dec;}; + void overwriteJerkMax(float jerk) {_jerk_max = jerk;}; + void overwriteJerkMin(float jerk) {_jerk_min = jerk;}; + +private: + + /* 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{0.0f}; + float _jerk_state_dependent{0.0f}; + + matrix::Vector2f _vel; // current velocity xy + matrix::Vector2f _vel_sp_prev; // previous velocity setpoint + + /* Params */ + param_t _acc_hover_h{PARAM_INVALID}; + param_t _acc_xy_max_h{PARAM_INVALID}; + param_t _dec_xy_min_h{PARAM_INVALID}; + param_t _jerk_max_h{PARAM_INVALID}; + param_t _jerk_min_h{PARAM_INVALID}; + param_t _vel_manual_h{PARAM_INVALID}; + float _acc_hover{50.0f}; // acceleration in hover + float _acc_xy_max{10.0f}; // acceleration in flight + float _dec_xy_min{1.0f}; // deceleration in flight + float _jerk_max{15.0f}; // jerk max during brake + float _jerk_min{1.0f}; // jerk min during brake + float _vel_manual{}; //maximum velocity in manual controlled mode + int _parameter_sub{-1}; + + /* 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 _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); + +};