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

mpc: add parameter for ascent/descent speed in auto modes

This commit is contained in:
bresch 2021-12-17 15:17:46 +01:00 committed by Mathieu Bresciani
parent 463513f31f
commit ea7d2334c9
11 changed files with 97 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View File

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

2
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
View File

@ -176,6 +176,8 @@ protected:
_param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed
(ParamFloat<px4::params::MPC_LAND_ALT2>)
_param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed
(ParamFloat<px4::params::MPC_Z_V_AUTO_UP>) _param_mpc_z_v_auto_up,
(ParamFloat<px4::params::MPC_Z_V_AUTO_DN>) _param_mpc_z_v_auto_dn,
(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
(ParamFloat<px4::params::MPC_TKO_RAMP_T>)
_param_mpc_tko_ramp_t, // time constant for smooth takeoff ramp

15
src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
View File

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

2
src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
View File

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

13
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
View File

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

4
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
View File

@ -140,6 +140,8 @@ private:
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight
(ParamFloat<px4::params::MPC_JERK_AUTO>) _param_mpc_jerk_auto,
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,
(ParamFloat<px4::params::MPC_Z_V_AUTO_UP>) _param_mpc_z_v_auto_up,
(ParamFloat<px4::params::MPC_Z_V_AUTO_DN>) _param_mpc_z_v_auto_dn
)
};

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

@ -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 <param>MPC_Z_V_AUTO_UP</param> is set to {1:.0}.
*/
events::send<float>(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 <param>MPC_Z_V_AUTO_DN</param> is set to {1:.0}.
*/
events::send<float>(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(),

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

@ -137,7 +137,9 @@ private:
(ParamFloat<px4::params::MPC_Z_VEL_I_ACC>) _param_mpc_z_vel_i_acc,
(ParamFloat<px4::params::MPC_Z_VEL_D_ACC>) _param_mpc_z_vel_d_acc,
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
(ParamFloat<px4::params::MPC_Z_V_AUTO_UP>) _param_mpc_z_v_auto_up,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
(ParamFloat<px4::params::MPC_Z_V_AUTO_DN>) _param_mpc_z_v_auto_dn,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
(ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air,
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,

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

@ -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_VEL_MAX_DN, 1.0f);
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.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

8
src/modules/navigator/rtl.cpp
View File

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

4
src/modules/navigator/rtl.h
View File

@ -177,8 +177,8 @@ private:
(ParamInt<px4::params::RTL_TIME_MARGIN>) _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};