dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mpc: add sideways and backward speed for manual position modes

This commit is contained in:
bresch 2022-02-18 17:08:00 +01:00 committed by Mathieu Bresciani
parent 1de38c88d9
commit 3b26c611af
7 changed files with 81 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
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();

2
src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp
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
)

9
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
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);

2
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp
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

24
src/modules/mc_pos_control/MulticopterPositionControl.cpp
View File

@ -120,6 +120,8 @@ void MulticopterPositionControl::parameters_update(bool force)
if (_param_mpc_xy_vel_all.get() >= 0.f) {
float xy_vel = _param_mpc_xy_vel_all.get();
num_changed += _param_mpc_vel_manual.commit_no_notification(xy_vel);
num_changed += _param_mpc_vel_man_back.commit_no_notification(-1.f);
num_changed += _param_mpc_vel_man_side.commit_no_notification(-1.f);
num_changed += _param_mpc_xy_cruise.commit_no_notification(xy_vel);
num_changed += _param_mpc_xy_vel_max.commit_no_notification(xy_vel);
}
@ -190,6 +192,28 @@ void MulticopterPositionControl::parameters_update(bool force)
"Manual speed has been constrained by maximum speed", _param_mpc_xy_vel_max.get());
}
if (_param_mpc_vel_man_back.get() > _param_mpc_vel_manual.get()) {
_param_mpc_vel_man_back.set(_param_mpc_vel_manual.get());
_param_mpc_vel_man_back.commit();
mavlink_log_critical(&_mavlink_log_pub, "Manual backward speed has been constrained by forward speed\t");
/* EVENT
* @description <param>MPC_VEL_MAN_BACK</param> is set to {1:.0}.
*/
events::send<float>(events::ID("mc_pos_ctrl_man_vel_back_set"), events::Log::Warning,
"Manual backward speed has been constrained by forward speed", _param_mpc_vel_manual.get());
}
if (_param_mpc_vel_man_side.get() > _param_mpc_vel_manual.get()) {
_param_mpc_vel_man_side.set(_param_mpc_vel_manual.get());
_param_mpc_vel_man_side.commit();
mavlink_log_critical(&_mavlink_log_pub, "Manual sideways speed has been constrained by forward speed\t");
/* EVENT
* @description <param>MPC_VEL_MAN_SIDE</param> is set to {1:.0}.
*/
events::send<float>(events::ID("mc_pos_ctrl_man_vel_side_set"), events::Log::Warning,
"Manual sideways speed has been constrained by forward speed", _param_mpc_vel_manual.get());
}
if (_param_mpc_z_v_auto_up.get() > _param_mpc_z_vel_max_up.get()) {
_param_mpc_z_v_auto_up.set(_param_mpc_z_vel_max_up.get());
_param_mpc_z_v_auto_up.commit();

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

@ -153,6 +153,8 @@ private:
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
(ParamFloat<px4::params::MPC_VEL_MAN_BACK>) _param_mpc_vel_man_back,
(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,

35
src/modules/mc_pos_control/mc_pos_control_params.c
View File

@ -349,11 +349,14 @@ PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.5f);
PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.0f);
/**
* Maximum horizontal velocity setpoint for manual controlled mode
* Maximum horizontal velocity setpoint in Position mode
*
* If velocity setpoint larger than MPC_XY_VEL_MAX is set, then
* the setpoint will be capped to MPC_XY_VEL_MAX
*
* The maximum sideways and backward speed can be set differently
* using MPC_VEL_MAN_SIDE and MPC_VEL_MAN_BACK, respectively.
*
* @unit m/s
* @min 3.0
* @max 20.0
@ -363,6 +366,36 @@ PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.0f);
*/
PARAM_DEFINE_FLOAT(MPC_VEL_MANUAL, 10.0f);
/**
* Maximum sideways velocity in Position mode
*
* If set to a negative value or larger than
* MPC_VEL_MANUAL then MPC_VEL_MANUAL is used.
*
* @unit m/s
* @min -1.0
* @max 20.0
* @increment 0.1
* @decimal 2
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_VEL_MAN_SIDE, -1.0f);
/**
* Maximum backward velocity in Position mode
*
* If set to a negative value or larger than
* MPC_VEL_MANUAL then MPC_VEL_MANUAL is used.
*
* @unit m/s
* @min -1.0
* @max 20.0
* @increment 0.1
* @decimal 2
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_VEL_MAN_BACK, -1.0f);
/**
* Maximum horizontal velocity
*