From 2ef2911c3664eb9157e1bfd697d14de50fe4d123 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 13 Nov 2024 02:30:52 +0100 Subject: [PATCH] Remove FlightTaskManualPositionSmoothVel The default implementation for multicopter Position mode is FlightTaskManualAcceleration. The last missing piece was support for CollisionPrevention in this implementation. --- .../init.d-posix/airframes/4006_gz_px4vision | 1 - .../flight_mode_manager/CMakeLists.txt | 1 - .../flight_mode_manager/FlightModeManager.cpp | 4 - .../ManualPositionSmoothVel/CMakeLists.txt | 39 ---- .../FlightTaskManualPositionSmoothVel.cpp | 185 ------------------ .../FlightTaskManualPositionSmoothVel.hpp | 92 --------- .../MulticopterPositionControl.hpp | 1 - .../multicopter_position_mode_params.c | 7 +- 8 files changed, 1 insertion(+), 329 deletions(-) delete mode 100644 src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt delete mode 100644 src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp delete mode 100644 src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision index c590938178..65d290e0cc 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision @@ -75,7 +75,6 @@ param set-default MPC_Z_VEL_P_ACC 5 param set-default MPC_Z_VEL_I_ACC 3 param set-default MPC_LAND_ALT1 3 param set-default MPC_LAND_ALT2 1 -param set-default MPC_POS_MODE 3 param set-default CP_GO_NO_DATA 1 # Navigator Parameters diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt index 81ff6a7a33..84ccba6ac0 100644 --- a/src/modules/flight_mode_manager/CMakeLists.txt +++ b/src/modules/flight_mode_manager/CMakeLists.txt @@ -47,7 +47,6 @@ list(APPEND flight_tasks_all ManualAltitude ManualAltitudeSmoothVel ManualPosition - ManualPositionSmoothVel Transition ) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 3d70726073..c58a7fa157 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -213,10 +213,6 @@ void FlightModeManager::start_flight_task() error = switchTask(FlightTaskIndex::ManualPosition); break; - case 3: - error = switchTask(FlightTaskIndex::ManualPositionSmoothVel); - break; - case 4: default: if (_param_mpc_pos_mode.get() != 4) { diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt deleted file mode 100644 index 8d4489fa3e..0000000000 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/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(FlightTaskManualPositionSmoothVel - FlightTaskManualPositionSmoothVel.cpp -) - -target_link_libraries(FlightTaskManualPositionSmoothVel PUBLIC FlightTaskManualPosition FlightTaskUtility) -target_include_directories(FlightTaskManualPositionSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp deleted file mode 100644 index a89283e146..0000000000 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ /dev/null @@ -1,185 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-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. - * - ****************************************************************************/ - -#include "FlightTaskManualPositionSmoothVel.hpp" - -#include -#include - -using namespace matrix; - -bool FlightTaskManualPositionSmoothVel::activate(const trajectory_setpoint_s &last_setpoint) -{ - bool ret = FlightTaskManualPosition::activate(last_setpoint); - - // Check if the previous FlightTask provided setpoints - Vector3f accel_prev{last_setpoint.acceleration}; - Vector3f vel_prev{last_setpoint.velocity}; - Vector3f pos_prev{last_setpoint.position}; - - for (int i = 0; i < 3; i++) { - // If the position setpoint is unknown, set to the current postion - if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); } - - // If the velocity setpoint is unknown, set to the current velocity - if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); } - - // No acceleration estimate available, set to zero if the setpoint is NAN - if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; } - } - - _smoothing_xy.reset(Vector2f{accel_prev}, Vector2f{vel_prev}, Vector2f{pos_prev}); - _smoothing_z.reset(accel_prev(2), vel_prev(2), pos_prev(2)); - - return ret; -} - -void FlightTaskManualPositionSmoothVel::reActivate() -{ - FlightTaskManualPosition::reActivate(); - // The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly - // using the generated jerk, reset the z derivatives to zero - _smoothing_xy.reset(Vector2f(), _velocity.xy(), _position.xy()); - _smoothing_z.reset(0.f, 0.f, _position(2)); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) -{ - _smoothing_xy.setCurrentPosition(_position.xy()); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) -{ - _smoothing_xy.setCurrentVelocity(_velocity.xy()); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionZ(float delta_z) -{ - _smoothing_z.setCurrentPosition(_position(2)); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityZ(float delta_vz) -{ - _smoothing_z.setCurrentVelocity(_velocity(2)); -} - -void FlightTaskManualPositionSmoothVel::_updateSetpoints() -{ - // Set max accel/vel/jerk - // Has to be done before _updateTrajectories() - _updateTrajConstraints(); - _updateTrajVelFeedback(); - _updateTrajCurrentPositionEstimate(); - - // Get yaw setpoint, un-smoothed position setpoints - FlightTaskManualPosition::_updateSetpoints(); - - _updateTrajectories(_velocity_setpoint); - - // Fill the jerk, acceleration, velocity and position setpoint vectors - _setOutputState(); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajConstraints() -{ - _updateTrajConstraintsXY(); - _updateTrajConstraintsZ(); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsXY() -{ - _smoothing_xy.setMaxJerk(_param_mpc_jerk_max.get()); - _smoothing_xy.setMaxAccel(_param_mpc_acc_hor_max.get()); - _smoothing_xy.setMaxVel(_param_mpc_vel_manual.get()); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsZ() -{ - _smoothing_z.setMaxJerk(_param_mpc_jerk_max.get()); - - _smoothing_z.setMaxAccelUp(_param_mpc_acc_up_max.get()); - _smoothing_z.setMaxVelUp(_constraints.speed_up); - - _smoothing_z.setMaxAccelDown(_param_mpc_acc_down_max.get()); - _smoothing_z.setMaxVelDown(_constraints.speed_down); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajVelFeedback() -{ - _smoothing_xy.setVelSpFeedback(_velocity_setpoint_feedback.xy()); - _smoothing_z.setVelSpFeedback(_velocity_setpoint_feedback(2)); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajCurrentPositionEstimate() -{ - _smoothing_xy.setCurrentPositionEstimate(_position.xy()); - _smoothing_z.setCurrentPositionEstimate(_position(2)); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajectories(Vector3f vel_target) -{ - _smoothing_xy.update(_deltatime, vel_target.xy()); - _smoothing_z.update(_deltatime, vel_target(2)); -} - -void FlightTaskManualPositionSmoothVel::_setOutputState() -{ - _setOutputStateXY(); - _setOutputStateZ(); -} - -void FlightTaskManualPositionSmoothVel::_setOutputStateXY() -{ - _jerk_setpoint.xy() = _smoothing_xy.getCurrentJerk(); - _acceleration_setpoint.xy() = _smoothing_xy.getCurrentAcceleration(); - _velocity_setpoint.xy() = _smoothing_xy.getCurrentVelocity(); - _position_setpoint.xy() = _smoothing_xy.getCurrentPosition(); -} - -void FlightTaskManualPositionSmoothVel::_setOutputStateZ() -{ - _jerk_setpoint(2) = _smoothing_z.getCurrentJerk(); - _acceleration_setpoint(2) = _smoothing_z.getCurrentAcceleration(); - _velocity_setpoint(2) = _smoothing_z.getCurrentVelocity(); - - if (!_terrain_hold) { - if (_terrain_hold_previous) { - // Reset position setpoint to current position when switching from terrain hold to non-terrain hold - _smoothing_z.setCurrentPosition(_position(2)); - } - - _position_setpoint(2) = _smoothing_z.getCurrentPosition(); - } - - _terrain_hold_previous = _terrain_hold; -} diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp deleted file mode 100644 index c8fbdf42c9..0000000000 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp +++ /dev/null @@ -1,92 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-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 FlightTaskManualPositionSmoothVel.hpp - * - * Flight task for smooth manual controlled position. - */ - -#pragma once - -#include "FlightTaskManualPosition.hpp" -#include -#include - -using matrix::Vector2f; -using matrix::Vector3f; - -class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition -{ -public: - FlightTaskManualPositionSmoothVel() = default; - virtual ~FlightTaskManualPositionSmoothVel() = default; - - bool activate(const trajectory_setpoint_s &last_setpoint) override; - void reActivate() override; - -protected: - - virtual void _updateSetpoints() override; - - /** Reset position or velocity setpoints in case of EKF reset event */ - void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override; - void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override; - void _ekfResetHandlerPositionZ(float delta_z) override; - void _ekfResetHandlerVelocityZ(float delta_vz) override; - - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualPosition, - (ParamFloat) _param_mpc_jerk_max, - (ParamFloat) _param_mpc_acc_up_max, - (ParamFloat) _param_mpc_acc_down_max - ) - -private: - void _updateTrajConstraints(); - void _updateTrajConstraintsXY(); - void _updateTrajConstraintsZ(); - - void _updateTrajVelFeedback(); - void _updateTrajCurrentPositionEstimate(); - - void _updateTrajectories(Vector3f vel_target); - - void _setOutputState(); - void _setOutputStateXY(); - void _setOutputStateZ(); - - ManualVelocitySmoothingXY _smoothing_xy; ///< Smoothing in x and y directions - ManualVelocitySmoothingZ _smoothing_z; ///< Smoothing in z direction - - bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */ -}; diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index f7cd9bbbe0..6d58b85cd7 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -167,7 +167,6 @@ private: (ParamFloat) _param_mpc_vel_man_side, (ParamFloat) _param_mpc_xy_cruise, (ParamFloat) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */ - (ParamInt) _param_mpc_pos_mode, (ParamInt) _param_mpc_alt_mode, (ParamFloat) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */ (ParamFloat) _param_mpc_thr_min, diff --git a/src/modules/mc_pos_control/multicopter_position_mode_params.c b/src/modules/mc_pos_control/multicopter_position_mode_params.c index a65ef4f571..9fce344319 100644 --- a/src/modules/mc_pos_control/multicopter_position_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_position_mode_params.c @@ -39,14 +39,10 @@ * Sticks directly map to velocity setpoints without smoothing. * Also applies to vertical direction and Altitude mode. * Useful for velocity control tuning. - * - "Smoothed velocity": - * Sticks map to velocity but with maximum acceleration and jerk limits based on - * jerk optimized trajectory generator (different algorithm than 1). * - "Acceleration based": * Sticks map to acceleration and there's a virtual brake drag * * @value 0 Direct velocity - * @value 3 Smoothed velocity * @value 4 Acceleration based * @group Multicopter Position Control */ @@ -104,7 +100,6 @@ PARAM_DEFINE_FLOAT(MPC_VEL_MAN_BACK, -1.f); * * MPC_POS_MODE * 1 just deceleration - * 3 acceleration and deceleration * 4 not used, use MPC_ACC_HOR instead * * @unit m/s^2 @@ -125,7 +120,7 @@ PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.f); * * Setting this to the maximum value essentially disables the limit. * - * Only used with smooth MPC_POS_MODE Smoothed velocity and Acceleration based. + * Only used with MPC_POS_MODE Acceleration based. * * @unit m/s^3 * @min 0.5