fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Remove FlightTaskManualPositionSmoothVel

Browse Source 
The default implementation for multicopter Position mode is FlightTaskManualAcceleration.
The last missing piece was support for CollisionPrevention in this implementation.
This commit is contained in:
Matthias Grob 2024-11-13 02:30:52 +01:00 committed by Claudio Chies
parent b74dd57e7c
commit 2ef2911c36
8 changed files with 1 additions and 329 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision
View File

@ -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

1
src/modules/flight_mode_manager/CMakeLists.txt
View File

@ -47,7 +47,6 @@ list(APPEND flight_tasks_all
ManualAltitude
ManualAltitudeSmoothVel
ManualPosition
ManualPositionSmoothVel
Transition
)

4
src/modules/flight_mode_manager/FlightModeManager.cpp
View File

@ -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) {

39
src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt
View File

@ -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})

185
src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
View File

@ -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 <mathlib/mathlib.h>
#include <float.h>
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;
}

92
src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
View File

@ -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 <motion_planning/ManualVelocitySmoothingXY.hpp>
#include <motion_planning/ManualVelocitySmoothingZ.hpp>
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<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _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 */
};

1
src/modules/mc_pos_control/MulticopterPositionControl.hpp
View File

@ -167,7 +167,6 @@ private:
(ParamFloat<px4::params::MPC_VEL_MAN_SIDE>) _param_mpc_vel_man_side,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */
(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode,
(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */
(ParamFloat<px4::params::MPC_THR_MIN>) _param_mpc_thr_min,

7
src/modules/mc_pos_control/multicopter_position_mode_params.c
View File

@ -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