From 3b26c611af3f001614cf83af77c68615e9661b92 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 18 Feb 2022 17:08:00 +0100 Subject: [PATCH] mpc: add sideways and backward speed for manual position modes --- .../FlightTaskManualPosition.cpp | 9 ++++- .../FlightTaskManualPosition.hpp | 2 ++ .../tasks/Utility/StickAccelerationXY.cpp | 9 +++++ .../tasks/Utility/StickAccelerationXY.hpp | 2 ++ .../MulticopterPositionControl.cpp | 24 +++++++++++++ .../MulticopterPositionControl.hpp | 2 ++ .../mc_pos_control/mc_pos_control_params.c | 35 ++++++++++++++++++- 7 files changed, 81 insertions(+), 2 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp index ba3d541ba5..f0b5eb31e7 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -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(); diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp index b730fd5466..fb7f5dd4e5 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -59,6 +59,8 @@ protected: DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude, (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_vel_man_side, + (ParamFloat) _param_mpc_vel_man_back, (ParamFloat) _param_mpc_acc_hor_max, (ParamFloat) _param_mpc_hold_max_xy ) diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index b6a33e9bce..5c0d8f9ca7 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -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); diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp index 94637217d6..fe0aee7acb 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp @@ -81,6 +81,8 @@ private: DEFINE_PARAMETERS( (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_vel_man_side, + (ParamFloat) _param_mpc_vel_man_back, (ParamFloat) _param_mpc_acc_hor, (ParamFloat) _param_mpc_jerk_max, (ParamFloat) _param_mpc_tiltmax_air diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 8dfb9e3f34..fa097c7cdf 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -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 MPC_VEL_MAN_BACK is set to {1:.0}. + */ + events::send(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 MPC_VEL_MAN_SIDE is set to {1:.0}. + */ + events::send(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(); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index d3103dd584..7a70ae0b13 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -153,6 +153,8 @@ private: (ParamFloat) _param_mpc_land_speed, (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_vel_man_back, + (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, diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 69d910b475..25d094289c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -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 *