diff --git a/src/lib/flight_tasks/CMakeLists.txt b/src/lib/flight_tasks/CMakeLists.txt index d79705f231..d7372362ce 100644 --- a/src/lib/flight_tasks/CMakeLists.txt +++ b/src/lib/flight_tasks/CMakeLists.txt @@ -52,10 +52,8 @@ endif() # add core flight tasks to list list(APPEND flight_tasks_all ManualAltitude - ManualAltitudeSmooth ManualAltitudeSmoothVel ManualPosition - ManualPositionSmooth ManualPositionSmoothVel AutoLineSmoothVel AutoFollowMe diff --git a/src/lib/flight_tasks/tasks/ManualAltitudeSmooth/CMakeLists.txt b/src/lib/flight_tasks/tasks/ManualAltitudeSmooth/CMakeLists.txt deleted file mode 100644 index 30cb1f3595..0000000000 --- a/src/lib/flight_tasks/tasks/ManualAltitudeSmooth/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2018 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. -# -############################################################################ - -px4_add_library(FlightTaskManualAltitudeSmooth - FlightTaskManualAltitudeSmooth.cpp -) - -target_link_libraries(FlightTaskManualAltitudeSmooth PUBLIC FlightTaskManualAltitude FlightTaskUtility) -target_include_directories(FlightTaskManualAltitudeSmooth PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/lib/flight_tasks/tasks/ManualAltitudeSmooth/FlightTaskManualAltitudeSmooth.cpp b/src/lib/flight_tasks/tasks/ManualAltitudeSmooth/FlightTaskManualAltitudeSmooth.cpp deleted file mode 100644 index 4fc431f1aa..0000000000 --- a/src/lib/flight_tasks/tasks/ManualAltitudeSmooth/FlightTaskManualAltitudeSmooth.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/**************************************************************************** - * - * 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 FlightManualAltitude.cpp - */ - -#include "FlightTaskManualAltitudeSmooth.hpp" - -using namespace matrix; - -FlightTaskManualAltitudeSmooth::FlightTaskManualAltitudeSmooth() : - _smoothing(this, _velocity(2), _sticks.getPosition()(2)) -{} - -void FlightTaskManualAltitudeSmooth::_updateSetpoints() -{ - /* Get yaw, thrust */ - FlightTaskManualAltitude::_updateSetpoints(); - - /* Smooth velocity in z*/ - _smoothing.smoothVelFromSticks(_velocity_setpoint(2), _deltatime); - - /* Check for altitude lock*/ - _updateAltitudeLock(); -} diff --git a/src/lib/flight_tasks/tasks/ManualAltitudeSmooth/FlightTaskManualAltitudeSmooth.hpp b/src/lib/flight_tasks/tasks/ManualAltitudeSmooth/FlightTaskManualAltitudeSmooth.hpp deleted file mode 100644 index 2d50d65c1c..0000000000 --- a/src/lib/flight_tasks/tasks/ManualAltitudeSmooth/FlightTaskManualAltitudeSmooth.hpp +++ /dev/null @@ -1,56 +0,0 @@ -/**************************************************************************** - * - * 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 FlightManualAltitude.hpp - * - * Flight task for manual controlled altitude. - */ - -#pragma once - -#include "FlightTaskManualAltitude.hpp" -#include "ManualSmoothingZ.hpp" - -class FlightTaskManualAltitudeSmooth : public FlightTaskManualAltitude -{ -public: - FlightTaskManualAltitudeSmooth(); - virtual ~FlightTaskManualAltitudeSmooth() = default; - -protected: - virtual void _updateSetpoints() override; - -private: - ManualSmoothingZ _smoothing; /**< smoothing for velocity setpoints */ -}; diff --git a/src/lib/flight_tasks/tasks/ManualPositionSmooth/CMakeLists.txt b/src/lib/flight_tasks/tasks/ManualPositionSmooth/CMakeLists.txt deleted file mode 100644 index 1b213cd827..0000000000 --- a/src/lib/flight_tasks/tasks/ManualPositionSmooth/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2018 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. -# -############################################################################ - -px4_add_library(FlightTaskManualPositionSmooth - FlightTaskManualPositionSmooth.cpp -) - -target_link_libraries(FlightTaskManualPositionSmooth PUBLIC FlightTaskManualPosition FlightTaskUtility) -target_include_directories(FlightTaskManualPositionSmooth PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp b/src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp deleted file mode 100644 index 2782e3421d..0000000000 --- a/src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.cpp +++ /dev/null @@ -1,68 +0,0 @@ -/**************************************************************************** - * - * 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 FlightManualPositionSmooth.cpp - */ - -#include "FlightTaskManualPositionSmooth.hpp" - -using namespace matrix; - -FlightTaskManualPositionSmooth::FlightTaskManualPositionSmooth() : - _smoothingXY(this, Vector2f(_velocity)), - _smoothingZ(this, _velocity(2), _sticks.getPosition()(2)) -{} - -void FlightTaskManualPositionSmooth::_updateSetpoints() -{ - /* Get yaw setpont, un-smoothed position setpoints.*/ - FlightTaskManualPosition::_updateSetpoints(); - - /* Smooth velocity setpoint in xy.*/ - Vector2f vel(_velocity); - Vector2f vel_sp_xy(_velocity_setpoint); - _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); - - /* Check for xy position lock.*/ - _updateXYlock(); - - /* Smooth velocity in z.*/ - _smoothingZ.smoothVelFromSticks(_velocity_setpoint(2), _deltatime); - - /* Check for altitude lock*/ - _updateAltitudeLock(); -} diff --git a/src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.hpp b/src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.hpp deleted file mode 100644 index a9c6fdaccb..0000000000 --- a/src/lib/flight_tasks/tasks/ManualPositionSmooth/FlightTaskManualPositionSmooth.hpp +++ /dev/null @@ -1,60 +0,0 @@ -/**************************************************************************** - * - * 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 FlightManualPositionSmooth.hpp - * - * Flight task for smooth manual controlled position. - */ - -#pragma once - -#include "FlightTaskManualPosition.hpp" -#include "ManualSmoothingXY.hpp" -#include "ManualSmoothingZ.hpp" - -class FlightTaskManualPositionSmooth : public FlightTaskManualPosition -{ -public: - FlightTaskManualPositionSmooth(); - - virtual ~FlightTaskManualPositionSmooth() = default; - -protected: - - virtual void _updateSetpoints() override; - -private: - ManualSmoothingXY _smoothingXY; /**< smoothing for velocity setpoints in xy */ - ManualSmoothingZ _smoothingZ; /**< smoothing for velocity in z */ -}; diff --git a/src/lib/flight_tasks/tasks/Orbit/CMakeLists.txt b/src/lib/flight_tasks/tasks/Orbit/CMakeLists.txt index 875444dcee..3ec96bb7a6 100644 --- a/src/lib/flight_tasks/tasks/Orbit/CMakeLists.txt +++ b/src/lib/flight_tasks/tasks/Orbit/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(FlightTaskOrbit FlightTaskOrbit.cpp ) -target_link_libraries(FlightTaskOrbit PUBLIC FlightTaskManualAltitudeSmooth) +target_link_libraries(FlightTaskOrbit PUBLIC FlightTaskManualAltitudeSmoothVel) target_include_directories(FlightTaskOrbit PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp index 3c4c48096f..4035e4b5e6 100644 --- a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp @@ -156,7 +156,7 @@ bool FlightTaskOrbit::checkAcceleration(float r, float v, float a) bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_setpoint) { - bool ret = FlightTaskManualAltitudeSmooth::activate(last_setpoint); + bool ret = FlightTaskManualAltitudeSmoothVel::activate(last_setpoint); _r = _radius_min; _v = 1.f; _center = Vector2f(_position); @@ -177,7 +177,7 @@ bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_set bool FlightTaskOrbit::update() { // update altitude - bool ret = FlightTaskManualAltitudeSmooth::update(); + bool ret = FlightTaskManualAltitudeSmoothVel::update(); // stick input adjusts parameters within a fixed time frame const float r = _r - _sticks.getPositionExpo()(0) * _deltatime * (_radius_max / 8.f); diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp index 5611f39ece..04cb6fcd48 100644 --- a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp @@ -41,12 +41,12 @@ #pragma once -#include "FlightTaskManualAltitudeSmooth.hpp" +#include "FlightTaskManualAltitudeSmoothVel.hpp" #include #include #include -class FlightTaskOrbit : public FlightTaskManualAltitudeSmooth +class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel { public: FlightTaskOrbit(); diff --git a/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt b/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt index 439e2aa837..5842f65778 100644 --- a/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt +++ b/src/lib/flight_tasks/tasks/Utility/CMakeLists.txt @@ -32,8 +32,6 @@ ############################################################################ px4_add_library(FlightTaskUtility - ManualSmoothingZ.cpp - ManualSmoothingXY.cpp StraightLine.cpp VelocitySmoothing.cpp ManualVelocitySmoothingXY.cpp diff --git a/src/lib/flight_tasks/tasks/Utility/ManualSmoothingXY.cpp b/src/lib/flight_tasks/tasks/Utility/ManualSmoothingXY.cpp deleted file mode 100644 index 1a84b9abd8..0000000000 --- a/src/lib/flight_tasks/tasks/Utility/ManualSmoothingXY.cpp +++ /dev/null @@ -1,227 +0,0 @@ -/**************************************************************************** - * - * 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 -#include -#include - -using namespace matrix; - -ManualSmoothingXY::ManualSmoothingXY(ModuleParams *parent, const Vector2f &vel) : - ModuleParams(parent), _vel_sp_prev(vel) - -{ - _acc_state_dependent = _param_mpc_acc_hor.get(); - _jerk_state_dependent = _param_mpc_jerk_max.get(); -} - -void -ManualSmoothingXY::smoothVelocity(Vector2f &vel_sp, const Vector2f &vel, const float &yaw, - const float &yawrate_sp, const float dt) -{ - _updateAcceleration(vel_sp, vel, yaw, yawrate_sp, dt); - _velocitySlewRate(vel_sp, dt); - _vel_sp_prev = vel_sp; -} - -void -ManualSmoothingXY::_updateAcceleration(Vector2f &vel_sp, const Vector2f &vel, const float &yaw, - const float &yawrate_sp, const float dt) -{ - 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. - _setStateAcceleration(vel_sp, vel, intention, dt); -} - -ManualSmoothingXY::Intention -ManualSmoothingXY::_getIntention(const Vector2f &vel_sp, const Vector2f &vel, const float &yaw, - const float &yawrate_sp) -{ - - if (vel_sp.length() > FLT_EPSILON) { - // 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 - Vector2f vel_sp_heading = _getWorldToHeadingFrame(vel_sp, yaw); - Vector2f vel_heading = _getWorldToHeadingFrame(vel, yaw); - - if (vel_sp_heading.length() > FLT_EPSILON) { - vel_sp_heading.normalize(); - } - - if (vel_heading.length() > FLT_EPSILON) { - vel_heading.normalize(); - } - - 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. - 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); - bool deceleration = is_aligned && (vel_sp.length() < _vel_sp_prev.length()) && !yawspeed_demand; - - if (direction_change) { - return Intention::direction_change; - - } else if (deceleration) { - return Intention::deceleration; - - } else { - return Intention::acceleration; - } - } - - return Intention::brake; //default is brake -} - -void -ManualSmoothingXY::_setStateAcceleration(const Vector2f &vel_sp, const Vector2f &vel, - const Intention &intention, const float dt) -{ - switch (intention) { - case Intention::brake: { - - if (_intention == Intention::direction_change) { - // 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) { - // start the brake with lowest acceleration which - // makes stopping smoother - _acc_state_dependent = _param_mpc_dec_hor_slow.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. - _jerk_state_dependent = 1e6f; // default - - if (_param_mpc_jerk_max.get() > _param_mpc_jerk_min.get() && _param_mpc_jerk_min.get() > FLT_EPSILON) { - - _jerk_state_dependent = math::min((_param_mpc_jerk_max.get() - _param_mpc_jerk_min.get()) - / _vel_max * vel.length() + _param_mpc_jerk_min.get(), _param_mpc_jerk_max.get()); - } - - // 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; - } - - /* limit jerk when braking to zero */ - float jerk = (_param_mpc_acc_hor_max.get() - _acc_state_dependent) / dt; - - if (jerk > _jerk_state_dependent) { - _acc_state_dependent = _jerk_state_dependent * dt - + _acc_state_dependent; - - } else { - _acc_state_dependent = _param_mpc_acc_hor_max.get(); - } - - break; - } - - case Intention::direction_change: { - // We allow for fast change by setting previous setpoint to current setpoint. - - - // Because previous setpoint is equal to current setpoint, - // slewrate will have no effect. Nonetheless, just set - // acceleration to maximum. - _acc_state_dependent = _param_mpc_acc_hor_max.get(); - break; - } - - case Intention::acceleration: { - // Limit acceleration linearly based on velocity setpoint. - _acc_state_dependent = (_param_mpc_acc_hor.get() - _param_mpc_dec_hor_slow.get()) - / _vel_max * vel_sp.length() + _param_mpc_dec_hor_slow.get(); - break; - } - - case Intention::deceleration: { - _acc_state_dependent = _param_mpc_dec_hor_slow.get(); - break; - } - } - - // Update intention for next iteration. - _intention = intention; -} - -void -ManualSmoothingXY::_velocitySlewRate(Vector2f &vel_sp, const float dt) -{ - // Adjust velocity setpoint if demand exceeds acceleration. / - Vector2f acc{}; - - if (dt > FLT_EPSILON) { - acc = (vel_sp - _vel_sp_prev) / dt; - } - - if (acc.length() > _acc_state_dependent) { - vel_sp = acc.normalized() * _acc_state_dependent * dt + _vel_sp_prev; - } -} - -Vector2f -ManualSmoothingXY::_getWorldToHeadingFrame(const Vector2f &vec, const float &yaw) -{ - Quatf q_yaw = AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), yaw); - Vector3f vec_heading = q_yaw.conjugate_inversed(Vector3f(vec(0), vec(1), 0.0f)); - return Vector2f(vec_heading); -} - -Vector2f -ManualSmoothingXY::_getHeadingToWorldFrame(const Vector2f &vec, const float &yaw) -{ - Quatf q_yaw = AxisAnglef(Vector3f(0.0f, 0.0f, 1.0f), yaw); - Vector3f vec_world = q_yaw.conjugate(Vector3f(vec(0), vec(1), 0.0f)); - return Vector2f(vec_world); -} diff --git a/src/lib/flight_tasks/tasks/Utility/ManualSmoothingXY.hpp b/src/lib/flight_tasks/tasks/Utility/ManualSmoothingXY.hpp deleted file mode 100644 index 5e31a422a9..0000000000 --- a/src/lib/flight_tasks/tasks/Utility/ManualSmoothingXY.hpp +++ /dev/null @@ -1,175 +0,0 @@ -/**************************************************************************** - * - * 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 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 - -#include -#include - -class ManualSmoothingXY : public ModuleParams -{ -public: - ManualSmoothingXY(ModuleParams *parent, const matrix::Vector2f &vel); - ~ManualSmoothingXY() = default; - - /** - * Maximum velocity is required to detect user intention. - * 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;}; - - /** - * Smoothing of velocity setpoint horizontally based - * on flight direction. - * @param vel_sp: velocity setpoint in xy - * @param dt: time delta in seconds - */ - void smoothVelocity(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw, - const float &yawrate_sp, const float dt); - - /* - * 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 - }; - /** - * Get user intention. - * @see Intention - */ - Intention getIntention() { return _intention; } - - /** - * Overwrite methods: - * Needed if different parameter values than default required. - */ - void overwriteHoverAcceleration(float acc) { _param_mpc_acc_hor_max.set(acc); } - void overwriteMaxAcceleration(float acc) { _param_mpc_acc_hor.set(acc); } - void overwriteDecelerationMin(float dec) { _param_mpc_dec_hor_slow.set(dec); } - void overwriteJerkMax(float jerk) { _param_mpc_jerk_max.set(jerk); } - void overwriteJerkMin(float jerk) { _param_mpc_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); - - /** - * 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 - */ - Intention _getIntention(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw, - const float &yawrate_sp); - - /** - * 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) _param_mpc_acc_hor_max, /**< acceleration in hover */ - (ParamFloat) _param_mpc_acc_hor, /**< acceleration in flight */ - (ParamFloat) _param_mpc_dec_hor_slow, /**< deceleration in flight */ - (ParamFloat) _param_mpc_jerk_min, /**< jerk min during brake */ - (ParamFloat) _param_mpc_jerk_max, /**< jerk max during brake */ - (ParamFloat) _param_mpc_vel_manual /**< maximum velocity in manual controlled mode */ - ) -}; diff --git a/src/lib/flight_tasks/tasks/Utility/ManualSmoothingZ.cpp b/src/lib/flight_tasks/tasks/Utility/ManualSmoothingZ.cpp deleted file mode 100644 index 714ae0e498..0000000000 --- a/src/lib/flight_tasks/tasks/Utility/ManualSmoothingZ.cpp +++ /dev/null @@ -1,159 +0,0 @@ -/**************************************************************************** - * - * 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 spec{fic 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 ManualSmoothingZ.cpp - */ - -#include "ManualSmoothingZ.hpp" -#include -#include - -ManualSmoothingZ::ManualSmoothingZ(ModuleParams *parent, const float &vel, const float &stick) : - ModuleParams(parent), - _vel(vel), _stick(stick), _vel_sp_prev(vel) -{ - _acc_state_dependent = _param_mpc_acc_up_max.get(); - _max_acceleration = _param_mpc_acc_up_max.get(); -} - -void -ManualSmoothingZ::smoothVelFromSticks(float &vel_sp, const float dt) -{ - updateAcceleration(vel_sp, dt); - velocitySlewRate(vel_sp, dt); - _vel_sp_prev = vel_sp; -} - -void -ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt) -{ - // check for max acceleration - setMaxAcceleration(); - - // check if zero input stick - const bool is_current_zero = (fabsf(_stick) <= FLT_EPSILON); - - // default is acceleration - ManualIntentionZ intention = ManualIntentionZ::acceleration; - - // check zero input stick - if (is_current_zero) { - intention = ManualIntentionZ::brake; - } - - - // update intention - if ((_intention != ManualIntentionZ::brake) && (intention == ManualIntentionZ::brake)) { - - // we start with lowest acceleration - _acc_state_dependent = _param_mpc_acc_down_max.get(); - - // reset slew-rate: this ensures that there - // is no delay present when user demands to brake - _vel_sp_prev = _vel; - - } - - switch (intention) { - case ManualIntentionZ::brake: { - - // limit jerk when braking to zero - float jerk = (_param_mpc_acc_up_max.get() - _acc_state_dependent) / dt; - - if (jerk > _param_mpc_jerk_max.get()) { - _acc_state_dependent = _param_mpc_jerk_max.get() * dt + _acc_state_dependent; - - } else { - _acc_state_dependent = _param_mpc_acc_up_max.get(); - } - - break; - } - - case ManualIntentionZ::acceleration: { - - _acc_state_dependent = (getMaxAcceleration() - _param_mpc_acc_down_max.get()) - * fabsf(_stick) + _param_mpc_acc_down_max.get(); - break; - } - } - - _intention = intention; -} - -void -ManualSmoothingZ::setMaxAcceleration() -{ - if (_stick < -FLT_EPSILON) { - // accelerating upward - _max_acceleration = _param_mpc_acc_up_max.get(); - - } else if (_stick > FLT_EPSILON) { - // accelerating downward - _max_acceleration = _param_mpc_acc_down_max.get(); - - } else { - - // want to brake - if (fabsf(_vel_sp_prev) < FLT_EPSILON) { - // at rest - _max_acceleration = _param_mpc_acc_up_max.get(); - - } else if (_vel_sp_prev < 0.0f) { - // braking downward - _max_acceleration = _param_mpc_acc_down_max.get(); - - } else { - // braking upward - _max_acceleration = _param_mpc_acc_up_max.get(); - } - } -} - -void -ManualSmoothingZ::velocitySlewRate(float &vel_sp, const float dt) -{ - // limit vertical acceleration - float acc = 0.f; - - if (dt > FLT_EPSILON) { - acc = (vel_sp - _vel_sp_prev) / dt; - } - - float max_acc = (acc < 0.0f) ? -_acc_state_dependent : _acc_state_dependent; - - if (fabsf(acc) > fabsf(max_acc)) { - vel_sp = max_acc * dt + _vel_sp_prev; - } -} diff --git a/src/lib/flight_tasks/tasks/Utility/ManualSmoothingZ.hpp b/src/lib/flight_tasks/tasks/Utility/ManualSmoothingZ.hpp deleted file mode 100644 index 2a06ba0b30..0000000000 --- a/src/lib/flight_tasks/tasks/Utility/ManualSmoothingZ.hpp +++ /dev/null @@ -1,133 +0,0 @@ -/**************************************************************************** - * - * 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 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: vehicle should stop - * - acceleration: vehicle should move up or down - */ -enum class ManualIntentionZ { - brake, - acceleration, -}; - -class ManualSmoothingZ : public ModuleParams -{ -public: - ManualSmoothingZ(ModuleParams *parent, const float &vel, const float &stick); - ~ManualSmoothingZ() = default; - - /** - * 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. - * @param dt: time delta in seconds - */ - void smoothVelFromSticks(float &vel_sp, const float dt); - - /** - * Get max accleration. - */ - float getMaxAcceleration() { return _max_acceleration; } - - /** - * Get user intention. - * @see ManualIntentionZ - */ - ManualIntentionZ getIntention() { return _intention; } - - /* - * Overwrite methods: - * Needed if different parameter values than default required. - */ - void overwriteAccelerationUp(float acc_max_up) { _param_mpc_acc_up_max.set(acc_max_up); } - void overwriteAccelerationDown(float acc_max_down) {_param_mpc_acc_down_max.set(acc_max_down); } - void overwriteJerkMax(float jerk_max) {_param_mpc_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 - * @see ManualIntentionZ - */ - ManualIntentionZ _intention{ManualIntentionZ::acceleration}; - - const float &_vel; /**< vehicle velocity (dependency injection) */ - const float &_stick; /**< stick input (dependency injection) */ - - 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) _param_mpc_acc_up_max, - (ParamFloat) _param_mpc_acc_down_max, - (ParamFloat) _param_mpc_jerk_max - ) -}; diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 7786d21bc0..683d4142b7 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -593,10 +593,6 @@ void MulticopterPositionControl::start_flight_task() error = _flight_tasks.switchTask(FlightTaskIndex::ManualPosition); break; - case 1: - error = _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmooth); - break; - case 3: error = _flight_tasks.switchTask(FlightTaskIndex::ManualPositionSmoothVel); break; @@ -631,10 +627,6 @@ void MulticopterPositionControl::start_flight_task() error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitude); break; - case 1: - error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitudeSmooth); - break; - case 3: default: error = _flight_tasks.switchTask(FlightTaskIndex::ManualAltitudeSmoothVel); diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index 877c257e78..7313ab69a9 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -68,7 +68,6 @@ set(srcs test_search_min.cpp test_servo.c test_sleep.c - test_smooth_z.cpp test_uart_baudchange.c test_uart_console.c test_uart_loopback.c diff --git a/src/systemcmds/tests/test_smooth_z.cpp b/src/systemcmds/tests/test_smooth_z.cpp deleted file mode 100644 index 63edd8fcd2..0000000000 --- a/src/systemcmds/tests/test_smooth_z.cpp +++ /dev/null @@ -1,277 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2019 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 test_smooth_z.cpp - * Tests flight path smoothing algorithms. - */ - -#include -#include -#include -#include - -class SmoothZTest : public UnitTest -{ -public: - virtual bool run_tests(); - - bool brakeUpward(); - bool brakeDownward(); - bool accelerateUpwardFromBrake(); - bool accelerateDownwardFromBrake(); - -}; - -bool SmoothZTest::run_tests() -{ - ut_run_test(brakeUpward); - ut_run_test(brakeDownward); - ut_run_test(accelerateUpwardFromBrake); - ut_run_test(accelerateDownwardFromBrake); - - return (_tests_failed == 0); -} - -bool SmoothZTest::brakeUpward() -{ - /* Downward flight and want to stop */ - float stick_current = 0.0f; // sticks are at zero position - float vel_sp_current = 0.0f; // desired velocity is at 0m/s - float vel_sp_previous = 5.0f; // the demanded previous setpoint was 5m/s downwards - float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint - float acc_max_up = 5.0f; - float acc_max_down = 2.0f; - - ManualSmoothingZ smooth(nullptr, vel, stick_current); - - /* overwrite parameters since they might change depending on configuration */ - smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss - smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss - smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1 - - float dt = 0.1f; // dt is set to 0.1s - - /* It should start with acceleration */ - ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration); - - for (int i = 0; i < 100; i++) { - - smooth.smoothVelFromSticks(vel_sp_current, dt); - - /* Test if intention is brake */ - ut_assert_true(smooth.getIntention() == ManualIntentionZ::brake); - - /* we should always use upward acceleration */ - ut_assert_true((smooth.getMaxAcceleration() - acc_max_up < FLT_EPSILON)); - - /* New setpoint has to be lower than previous setpoint (NED frame) or equal 0. 0 velocity - * occurs once the vehicle is at perfect rest. */ - ut_assert_true((vel_sp_current < vel_sp_previous) || (fabsf(vel_sp_current) < FLT_EPSILON)); - - - /* We reset the previou setpoint to newest setpoint - * and set the current setpoint to 0 because we still want to brake. - * We also set vel to previous setpoint where we make the assumption that - * the vehicle can perfectly track the setpoints. - */ - vel_sp_previous = vel_sp_current; - vel_sp_current = 0.0f; - vel = vel_sp_previous; - - - } - - return true; -} - -bool SmoothZTest::brakeDownward() -{ - /* Downward flight and want to stop */ - float stick_current = 0.0f; // sticks are at zero position - float vel_sp_current = 0.0f; // desired velocity is 0m/s - float vel_sp_previous = -5.0f; // the demanded previous setpoint was -5m/s downwards - float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint - float acc_max_up = 5.0f; - float acc_max_down = 2.0f; - - ManualSmoothingZ smooth(nullptr, vel, stick_current); - - /* overwrite parameters since they might change depending on configuration */ - smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss - smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss - smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1 - - float dt = 0.1f; // dt is set to 0.1s - - /* It should start with acceleration */ - ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration); - - for (int i = 0; i < 100; i++) { - - smooth.smoothVelFromSticks(vel_sp_current, dt); - - /* Test if intention is brake */ - ut_assert_true(smooth.getIntention() == ManualIntentionZ::brake); - - /* New setpoint has to be larger than previous setpoint (NED frame) or equal 0. 0 velocity - * occurs once the vehicle is at perfect rest. */ - ut_assert_true((vel_sp_current > vel_sp_previous) || (fabsf(vel_sp_current) < FLT_EPSILON)); - - /* we should always use downward acceleration except when vehicle is at rest*/ - if (fabsf(vel_sp_previous) < FLT_EPSILON) { - ut_assert_true(fabsf(smooth.getMaxAcceleration() - acc_max_up) < FLT_EPSILON); - - } else { - ut_assert_true(fabsf(smooth.getMaxAcceleration() - acc_max_down) < FLT_EPSILON); - } - - - /* We reset the previou setpoint to newest setpoint - * and set the current setpoint to 0 because we still want to brake. - * We also set vel to previous setpoint where we make the assumption that - * the vehicle can perfectly track the setpoints. - */ - vel_sp_previous = vel_sp_current; - vel_sp_current = 0.0f; - vel = vel_sp_previous; - - } - - return true; -} - -bool SmoothZTest::accelerateUpwardFromBrake() -{ - /* Downward flight and want to stop */ - float stick_current = -1.0f; // sticks are at full upward position - float vel_sp_target = -5.0f; // desired velocity is at -5m/s - float vel_sp_current = vel_sp_target; - float vel_sp_previous = 0.0f; // the demanded previous setpoint was 0m/s downwards - float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint - float acc_max_up = 5.0f; - float acc_max_down = 2.0f; - - ManualSmoothingZ smooth(nullptr, vel, stick_current); - - /* overwrite parameters since they might change depending on configuration */ - smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss - smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss - smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1 - - float dt = 0.1f; // dt is set to 0.1s - - for (int i = 0; i < 100; i++) { - - smooth.smoothVelFromSticks(vel_sp_current, dt); - - /* Test if intention is acceleration */ - ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration); - - /* we should always use upward acceleration */ - ut_assert_true(fabsf(smooth.getMaxAcceleration() - acc_max_up) < FLT_EPSILON); - - /* New setpoint has to be larger than previous setpoint or equal to target velocity - * vel_sp_current. The negative sign is because of NED frame. - */ - ut_assert_true((-vel_sp_current > -vel_sp_previous) || (fabsf(vel_sp_current - vel_sp_previous) < FLT_EPSILON)); - - - /* We reset the previous setpoint to newest setpoint and reset the current setpoint. - * We also set the current velocity to the previous setpoint with the assumption that - * the vehicle does perfect tracking. - */ - vel_sp_previous = vel_sp_current; - vel_sp_current = vel_sp_target; - vel = vel_sp_previous; - - } - - return true; -} - -bool SmoothZTest::accelerateDownwardFromBrake() -{ - /* Downward flight and want to stop */ - float stick_current = 1.0f; // sticks are at full downward position - float vel_sp_target = 5.0f; // desired velocity is at 5m/s - float vel_sp_current = vel_sp_target; - float vel_sp_previous = 0.0f; // the demanded previous setpoint was 0m/s downwards - float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint - float acc_max_up = 5.0f; - float acc_max_down = 2.0f; - - ManualSmoothingZ smooth(nullptr, vel, stick_current); - - /* overwrite parameters since they might change depending on configuration */ - smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss - smooth.overwriteAccelerationUp(acc_max_up); // upward max acceleration of 5m/ss - smooth.overwriteJerkMax(0.1f); // maximum jerk of 0.1 - - float dt = 0.1f; // dt is set to 0.1s - - for (int i = 0; i < 100; i++) { - - smooth.smoothVelFromSticks(vel_sp_current, dt); - - /* Test if intention is acceleration */ - ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration); - - /* we should always use downward acceleration except when target velocity is reached */ - if (fabsf(vel_sp_current - vel_sp_target) < FLT_EPSILON) { - ut_assert_true(smooth.getMaxAcceleration() - acc_max_up < FLT_EPSILON); - - } else { - ut_assert_true(fabsf(smooth.getMaxAcceleration() - acc_max_down) < FLT_EPSILON); - } - - /* New setpoint has to be larger than previous setpoint or equal to target velocity - * vel_sp_current (NED frame). - */ - ut_assert_true((vel_sp_current > vel_sp_previous) || (fabsf(vel_sp_current - vel_sp_target) < FLT_EPSILON)); - - - /* We reset the previous setpoint to newest setpoint and reset the current setpoint. - * We also set the current velocity to the previous setpoint with the assumption that - * the vehicle does perfect tracking. - */ - vel_sp_previous = vel_sp_current; - vel_sp_current = vel_sp_target; - vel = vel_sp_previous; - - } - - return true; -} - -ut_declare_test_c(test_smooth_z, SmoothZTest) diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 9042c83fe0..5db876a57a 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -114,7 +114,6 @@ const struct { {"search_min", test_search_min, 0}, {"servo", test_servo, OPT_NOJIGTEST | OPT_NOALLTEST}, {"sleep", test_sleep, OPT_NOJIGTEST}, - {"smoothz", test_smooth_z, 0}, {"uart_loopback", test_uart_loopback, OPT_NOJIGTEST | OPT_NOALLTEST}, {"uart_send", test_uart_send, OPT_NOJIGTEST | OPT_NOALLTEST}, {"versioning", test_versioning, 0}, diff --git a/src/systemcmds/tests/tests_main.h b/src/systemcmds/tests/tests_main.h index eeb71502b6..e5f7881f0a 100644 --- a/src/systemcmds/tests/tests_main.h +++ b/src/systemcmds/tests/tests_main.h @@ -80,7 +80,6 @@ extern int test_rc(int argc, char *argv[]); extern int test_search_min(int argc, char *argv[]); extern int test_servo(int argc, char *argv[]); extern int test_sleep(int argc, char *argv[]); -extern int test_smooth_z(int argc, char *argv[]); extern int test_time(int argc, char *argv[]); extern int test_uart_baudchange(int argc, char *argv[]); extern int test_uart_break(int argc, char *argv[]);