fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 03:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mpc: add sideways and backward speed for manual position modes

Browse Source
This commit is contained in:
bresch
2022-07-08 14:03:44 +02:00
committed by Mathieu Bresciani
parent 1de38c88d9
commit 3b26c611af
7 changed files with 81 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
+8 -1
View File
@@ -70,11 +70,18 @@ void FlightTaskManualPosition::_scaleSticks()
/* Use same scaling as for FlightTaskManualAltitude */
FlightTaskManualAltitude::_scaleSticks();
/* Constrain length of stick inputs to 1 for xy*/
Vector2f stick_xy = _sticks.getPositionExpo().slice<2, 1>(0, 0);
Sticks::limitStickUnitLengthXY(stick_xy);
if (_param_mpc_vel_man_side.get() >= 0.f) {
stick_xy(1) *= _param_mpc_vel_man_side.get() / _param_mpc_vel_manual.get();
}
if ((_param_mpc_vel_man_back.get() >= 0.f) && (stick_xy(0) < 0.f)) {
stick_xy(0) *= _param_mpc_vel_man_back.get() / _param_mpc_vel_manual.get();
}
const float max_speed_from_estimator = _sub_vehicle_local_position.get().vxy_max;
float velocity_scale = _param_mpc_vel_manual.get();
src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp
+2
View File
@@ -59,6 +59,8 @@ protected:
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
(ParamFloat<px4::params::MPC_VEL_MAN_SIDE>) _param_mpc_vel_man_side,
(ParamFloat<px4::params::MPC_VEL_MAN_BACK>) _param_mpc_vel_man_back,
(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max,
(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy
)
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
+9
View File
@@ -81,6 +81,15 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw,
// Map stick input to acceleration
Sticks::limitStickUnitLengthXY(stick_xy);
if (_param_mpc_vel_man_side.get() >= 0.f) {
stick_xy(1) *= _param_mpc_vel_man_side.get() / _param_mpc_vel_manual.get();
}
if ((_param_mpc_vel_man_back.get() >= 0.f) && (stick_xy(0) < 0.f)) {
stick_xy(0) *= _param_mpc_vel_man_back.get() / _param_mpc_vel_manual.get();
}
Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_sp);
_acceleration_setpoint = stick_xy.emult(acceleration_scale);
applyJerkLimit(dt);
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp
+2
View File
@@ -81,6 +81,8 @@ private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
(ParamFloat<px4::params::MPC_VEL_MAN_SIDE>) _param_mpc_vel_man_side,
(ParamFloat<px4::params::MPC_VEL_MAN_BACK>) _param_mpc_vel_man_back,
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor,
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
(ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air