diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index ee0a0aab71..d7ad0ed872 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -231,7 +231,7 @@ void FlightTaskAuto::_prepareLandSetpoints() // Slow down automatic descend close to ground float land_speed = math::gradual(_dist_to_ground, _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(), - _param_mpc_land_speed.get(), _constraints.speed_down); + _param_mpc_land_speed.get(), _param_mpc_z_v_auto_dn.get()); if (_type_previous != WaypointType::land) { // initialize xy-position and yaw to waypoint such that home is reached exactly without user input @@ -802,12 +802,12 @@ void FlightTaskAuto::_updateTrajConstraints() // If the current velocity is beyond the usual constraints, tell // the controller to exceptionally increase its saturations to avoid // cutting out the feedforward - _constraints.speed_down = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), _param_mpc_z_vel_max_dn.get()); - _constraints.speed_up = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), _param_mpc_z_vel_max_up.get()); + _constraints.speed_down = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), _constraints.speed_down); + _constraints.speed_up = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), _constraints.speed_up); } else if (_unsmoothed_velocity_setpoint(2) < 0.f) { // up float z_accel_constraint = _param_mpc_acc_up_max.get(); - float z_vel_constraint = _param_mpc_z_vel_max_up.get(); + float z_vel_constraint = _param_mpc_z_v_auto_up.get(); // The constraints are broken because they are used as hard limits by the position controller, so put this here // until the constraints don't do things like cause controller integrators to saturate. Once the controller @@ -828,7 +828,7 @@ void FlightTaskAuto::_updateTrajConstraints() } else { // down _position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get()); - _position_smoothing.setMaxVelocityZ(_param_mpc_z_vel_max_dn.get()); + _position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get()); } } diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index fe902b8025..50042232e3 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -176,6 +176,8 @@ protected: _param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed (ParamFloat) _param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed + (ParamFloat) _param_mpc_z_v_auto_up, + (ParamFloat) _param_mpc_z_v_auto_dn, (ParamFloat) _param_mpc_tko_speed, (ParamFloat) _param_mpc_tko_ramp_t, // time constant for smooth takeoff ramp diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 168dfd0177..3764c1d948 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -72,9 +72,6 @@ bool FlightTaskManualAltitude::activate(const vehicle_local_position_setpoint_s _updateConstraintsFromEstimator(); - _max_speed_up = _constraints.speed_up; - _max_speed_down = _constraints.speed_down; - return ret; } @@ -102,7 +99,8 @@ void FlightTaskManualAltitude::_scaleSticks() _yawspeed_setpoint = _applyYawspeedFilter(yawspeed_target); // Use sticks input with deadzone and exponential curve for vertical velocity - const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up; + const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _param_mpc_z_vel_max_dn.get() : + _param_mpc_z_vel_max_up.get(); _velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2); } @@ -249,12 +247,13 @@ void FlightTaskManualAltitude::_respectMaxAltitude() // if there is a valid maximum distance to ground, linearly increase speed limit with distance // below the maximum, preserving control loop vertical position error gain. + // TODO: manipulate the velocity setpoint instead of tweaking the saturation of the controller if (PX4_ISFINITE(_max_distance_to_ground)) { _constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom), - -_max_speed_down, _max_speed_up); + -_param_mpc_z_vel_max_dn.get(), _param_mpc_z_vel_max_up.get()); } else { - _constraints.speed_up = _max_speed_up; + _constraints.speed_up = _param_mpc_z_vel_max_up.get(); } // if distance to bottom exceeded maximum distance, slowly approach maximum distance @@ -264,10 +263,10 @@ void FlightTaskManualAltitude::_respectMaxAltitude() // set position setpoint to maximum distance to ground _position_setpoint(2) = _position(2) + delta_distance_to_max; // limit speed downwards to 0.7m/s - _constraints.speed_down = math::min(_max_speed_down, 0.7f); + _constraints.speed_down = math::min(_param_mpc_z_vel_max_dn.get(), 0.7f); } else { - _constraints.speed_down = _max_speed_down; + _constraints.speed_down = _param_mpc_z_vel_max_dn.get(); } } } diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index d2868aae0e..affbf6f185 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -134,8 +134,6 @@ private: float _yawspeed_filter_state{}; /**< state of low-pass filter in rad/s */ uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ - float _max_speed_up = 10.0f; - float _max_speed_down = 1.0f; bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */ bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */ diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index a614fb2c87..6d86cdc8be 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -248,19 +248,18 @@ void FlightTaskOrbit::_updateTrajectoryBoundaries() _altitude_velocity_smoothing.setMaxJerk(max_jerk); if (_unsmoothed_velocity_setpoint(2) < 0.f) { // up - float z_accel_constraint = _param_mpc_acc_up_max.get(); - float z_vel_constraint = _param_mpc_z_vel_max_up.get(); + const float z_accel_constraint = _param_mpc_acc_up_max.get(); - _position_smoothing.setMaxVelocityZ(z_vel_constraint); + _position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_up.get()); _position_smoothing.setMaxAccelerationZ(z_accel_constraint); - _altitude_velocity_smoothing.setMaxVel(z_vel_constraint); + _altitude_velocity_smoothing.setMaxVel(_param_mpc_z_vel_max_up.get()); _altitude_velocity_smoothing.setMaxAccel(z_accel_constraint); } else { // down _position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get()); - _position_smoothing.setMaxVelocityZ(_param_mpc_z_vel_max_dn.get()); - _altitude_velocity_smoothing.setMaxVel(_param_mpc_acc_down_max.get()); - _altitude_velocity_smoothing.setMaxAccel(_param_mpc_z_vel_max_dn.get()); + _position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get()); + _altitude_velocity_smoothing.setMaxAccel(_param_mpc_acc_down_max.get()); + _altitude_velocity_smoothing.setMaxVel(_param_mpc_z_vel_max_dn.get()); } } diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp index 7749eb0d84..96fa315913 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp @@ -140,6 +140,8 @@ private: (ParamFloat) _param_mpc_acc_hor, // acceleration in flight (ParamFloat) _param_mpc_jerk_auto, (ParamFloat) _param_mpc_acc_up_max, - (ParamFloat) _param_mpc_acc_down_max + (ParamFloat) _param_mpc_acc_down_max, + (ParamFloat) _param_mpc_z_v_auto_up, + (ParamFloat) _param_mpc_z_v_auto_dn ) }; diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 7f754e86bc..69266c46fc 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -127,7 +127,9 @@ int MulticopterPositionControl::parameters_update(bool force) if (_param_mpc_z_vel_all.get() >= 0.f) { float z_vel = _param_mpc_z_vel_all.get(); + num_changed += _param_mpc_z_v_auto_up.commit_no_notification(z_vel); num_changed += _param_mpc_z_vel_max_up.commit_no_notification(z_vel); + num_changed += _param_mpc_z_v_auto_dn.commit_no_notification(z_vel * 0.75f); num_changed += _param_mpc_z_vel_max_dn.commit_no_notification(z_vel * 0.75f); num_changed += _param_mpc_tko_speed.commit_no_notification(z_vel * 0.6f); num_changed += _param_mpc_land_speed.commit_no_notification(z_vel * 0.5f); @@ -189,6 +191,28 @@ int MulticopterPositionControl::parameters_update(bool force) "Manual speed has been constrained by maximum speed", _param_mpc_xy_vel_max.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(); + mavlink_log_critical(&_mavlink_log_pub, "Ascent speed has been constrained by max speed\t"); + /* EVENT + * @description MPC_Z_V_AUTO_UP is set to {1:.0}. + */ + events::send(events::ID("mc_pos_ctrl_up_vel_set"), events::Log::Warning, + "Ascent speed has been constrained by max speed", _param_mpc_z_vel_max_up.get()); + } + + if (_param_mpc_z_v_auto_dn.get() > _param_mpc_z_vel_max_dn.get()) { + _param_mpc_z_v_auto_dn.set(_param_mpc_z_vel_max_dn.get()); + _param_mpc_z_v_auto_dn.commit(); + mavlink_log_critical(&_mavlink_log_pub, "Descent speed has been constrained by max speed\t"); + /* EVENT + * @description MPC_Z_V_AUTO_DN is set to {1:.0}. + */ + events::send(events::ID("mc_pos_ctrl_down_vel_set"), events::Log::Warning, + "Descent speed has been constrained by max speed", _param_mpc_z_vel_max_dn.get()); + } + if (_param_mpc_thr_hover.get() > _param_mpc_thr_max.get() || _param_mpc_thr_hover.get() < _param_mpc_thr_min.get()) { _param_mpc_thr_hover.set(math::constrain(_param_mpc_thr_hover.get(), _param_mpc_thr_min.get(), diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index dd183b07d4..2f43e629cf 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -137,7 +137,9 @@ private: (ParamFloat) _param_mpc_z_vel_i_acc, (ParamFloat) _param_mpc_z_vel_d_acc, (ParamFloat) _param_mpc_xy_vel_max, + (ParamFloat) _param_mpc_z_v_auto_up, (ParamFloat) _param_mpc_z_vel_max_up, + (ParamFloat) _param_mpc_z_v_auto_dn, (ParamFloat) _param_mpc_z_vel_max_dn, (ParamFloat) _param_mpc_tiltmax_air, (ParamFloat) _param_mpc_thr_hover, 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 8a6b25cd3f..fe361d0f5c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -200,29 +200,64 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_I_ACC, 2.0f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_D_ACC, 0.0f); /** - * Maximum vertical ascent velocity + * Automatic ascent velocity * - * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL). + * Ascent velocity in auto modes. + * For manual modes and offboard, see MPC_Z_VEL_MAX_UP * * @unit m/s * @min 0.5 * @max 8.0 + * @increment 0.1 * @decimal 1 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_UP, 3.0f); +PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_UP, 3.f); /** - * Maximum vertical descent velocity + * Maximum ascent velocity * - * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL). + * Ascent velocity in manual modes and offboard. + * For auto modes, see MPC_Z_V_AUTO_UP + * + * @unit m/s + * @min 0.5 + * @max 8.0 + * @increment 0.1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_UP, 3.f); + +/** + * Automatic descent velocity + * + * Descent velocity in auto modes. + * For manual modes and offboard, see MPC_Z_VEL_MAX_DN * * @unit m/s * @min 0.5 * @max 4.0 + * @increment 0.1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_DN, 1.f); + +/** + * Maximum descent velocity + * + * Descent velocity in manual modes and offboard. + * For auto modes, see MPC_Z_V_AUTO_DN + * + * @unit m/s + * @min 0.5 + * @max 4.0 + * @increment 0.1 + * @decimal 1 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_DN, 1.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_DN, 1.f); /** * Proportional gain for horizontal position error @@ -664,7 +699,7 @@ PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.0f); * Altitude for 1. step of slow landing (descend) * * Below this altitude descending velocity gets limited to a value - * between "MPC_Z_VEL_MAX_DN" and "MPC_LAND_SPEED" + * between "MPC_Z_VEL_MAX_DN" (or "MPC_Z_V_AUTO_DN") and "MPC_LAND_SPEED" * Value needs to be higher than "MPC_LAND_ALT2" * * @unit m diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 80ecaa646e..05e3ed3e1e 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -57,8 +57,8 @@ RTL::RTL(Navigator *navigator) : MissionBlock(navigator), ModuleParams(navigator) { - _param_mpc_z_vel_max_up = param_find("MPC_Z_VEL_MAX_UP"); - _param_mpc_z_vel_max_down = param_find("MPC_Z_VEL_MAX_DN"); + _param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP"); + _param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN"); _param_mpc_land_speed = param_find("MPC_LAND_SPEED"); _param_fw_climb_rate = param_find("FW_T_CLMB_R_SP"); _param_fw_sink_rate = param_find("FW_T_SINK_R_SP"); @@ -886,7 +886,7 @@ float RTL::getClimbRate() float ret = 1e6f; if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (_param_mpc_z_vel_max_up == PARAM_INVALID || param_get(_param_mpc_z_vel_max_up, &ret) != PX4_OK) { + if (_param_mpc_z_v_auto_up == PARAM_INVALID || param_get(_param_mpc_z_v_auto_up, &ret) != PX4_OK) { ret = 1e6f; } @@ -905,7 +905,7 @@ float RTL::getDescendRate() float ret = 1e6f; if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (_param_mpc_z_vel_max_down == PARAM_INVALID || param_get(_param_mpc_z_vel_max_down, &ret) != PX4_OK) { + if (_param_mpc_z_v_auto_dn == PARAM_INVALID || param_get(_param_mpc_z_v_auto_dn, &ret) != PX4_OK) { ret = 1e6f; } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 0dfc3afc7e..02570dca84 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -177,8 +177,8 @@ private: (ParamInt) _param_rtl_time_margin ) - param_t _param_mpc_z_vel_max_up{PARAM_INVALID}; - param_t _param_mpc_z_vel_max_down{PARAM_INVALID}; + param_t _param_mpc_z_v_auto_up{PARAM_INVALID}; + param_t _param_mpc_z_v_auto_dn{PARAM_INVALID}; param_t _param_mpc_land_speed{PARAM_INVALID}; param_t _param_fw_climb_rate{PARAM_INVALID}; param_t _param_fw_sink_rate{PARAM_INVALID};