fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 15:27:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: improved smooth takeoff and used it for manual takeoff as well

Browse Source 
adresses:
there were setpoint twitches at the beginning and end of my smooth takeoff routine
it was to slow and not configurable
it was only available for automatic takeoff
This commit is contained in:
Matthias Grob
2017-04-24 20:23:02 +02:00
parent 75872b0713
commit 9a162a24bb
2 changed files with 47 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+35 -20
View File
@@ -147,6 +147,7 @@ private:
control::BlockParamFloat _deceleration_hor_max; /**< maximum velocity setpoint slewrate while decelerating */
control::BlockParamFloat _target_threshold_xy; /**< distance threshold for slowdown close to target during mission */
control::BlockParamFloat _velocity_hor_manual; /**< target velocity in manual controlled mode at full speed*/
control::BlockParamFloat _takeoff_ramp_time; /**< time contant for smooth takeoff ramp */
control::BlockDerivative _vel_x_deriv;
control::BlockDerivative _vel_y_deriv;
@@ -262,9 +263,11 @@ private:
bool _lnd_reached_ground; /**< controller assumes the vehicle has reached the ground after landing */
float _vel_z_lp;
float _acc_z_lp;
float _takeoff_vel_limit;
float _vel_max_xy; /**< equal to vel_max except in auto mode when close to target */
bool _in_takeoff; /**< flag for smooth velocity setpoint takeoff ramp */
float _takeoff_vel_limit; /**< velocity limit value which gets ramped up */
// counters for reset events on position and velocity states
// they are used to identify a reset event
uint8_t _z_reset_counter;
@@ -414,6 +417,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_deceleration_hor_max(this, "DEC_HOR_MAX", true),
_target_threshold_xy(this, "TARGET_THRE"),
_velocity_hor_manual(this, "VEL_MAN_MAX", true),
_takeoff_ramp_time(this, "TKO_RAMP_T", true),
_vel_x_deriv(this, "VELD"),
_vel_y_deriv(this, "VELD"),
_vel_z_deriv(this, "VELD"),
@@ -433,8 +437,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_lnd_reached_ground(false),
_vel_z_lp(0),
_acc_z_lp(0),
_takeoff_vel_limit(0.0f),
_vel_max_xy(0.0f),
_takeoff_vel_limit(0.0f),
_z_reset_counter(0),
_xy_reset_counter(0),
_vz_reset_counter(0),
@@ -1709,21 +1713,16 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
_vel_sp(2) = 0.0f;
}
/* special velocity setpoint limitation for smooth takeoff from ground */
/* limit vertical takeoff speed if we are in auto takeoff */
if (_pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
&& _control_mode.flag_armed) {
/* ramp vertical velocity limit up to takeoff speed */
_takeoff_vel_limit += _params.tko_speed * dt / 10.0f;
_takeoff_vel_limit = math::min(_takeoff_vel_limit, _params.tko_speed);
/* limit vertical velocity to the current ramp value */
_vel_sp(2) = math::max(_vel_sp(2), -_takeoff_vel_limit);
} else {
_takeoff_vel_limit = 0.0f;
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
_vel_sp(2) = math::max(_vel_sp(2), -_params.tko_speed);
}
/* apply slewrate (aka acceleration limit) for smooth flying */
vel_sp_slewrate(dt);
_vel_sp_prev = _vel_sp;
/* make sure velocity setpoint is constrained in all directions*/
if (vel_norm_xy > _vel_max_xy) {
_vel_sp(0) = _vel_sp(0) * _vel_max_xy / vel_norm_xy;
@@ -1732,9 +1731,18 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
_vel_sp(2) = math::max(_vel_sp(2), -_params.vel_max_up);
/* apply slewrate (aka acceleration limit) for smooth flying */
vel_sp_slewrate(dt);
_vel_sp_prev = _vel_sp;
/* special velocity setpoint limitation for smooth takeoff */
if (_in_takeoff) {
_in_takeoff = _takeoff_vel_limit < -_vel_sp(2);
/* ramp vertical velocity limit up to takeoff speed */
_takeoff_vel_limit += -_vel_sp(2) * dt / _takeoff_ramp_time.get();
/* limit vertical velocity to the current ramp value */
_vel_sp(2) = math::max(_vel_sp(2), -_takeoff_vel_limit);
//printf("ramping: %f %f\n", (double)_takeoff_vel_limit, (double)_vel_sp(2));
} else {
_takeoff_vel_limit = -0.5f;
}
/* publish velocity setpoint */
_global_vel_sp.timestamp = hrt_absolute_time();
@@ -2222,6 +2230,7 @@ MulticopterPositionControl::task_main()
orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected);
bool was_armed = false;
bool was_landed = true;
hrt_abstime t_prev = 0;
@@ -2257,7 +2266,7 @@ MulticopterPositionControl::task_main()
float dt = t_prev != 0 ? (t - t_prev) / 1e6f : 0.004f;
t_prev = t;
// set dt for control blocks
/* set dt for control blocks */
setDt(dt);
/* set default max velocity in xy to vel_max */
@@ -2275,6 +2284,8 @@ MulticopterPositionControl::task_main()
_yaw_takeoff = _yaw;
}
was_armed = _control_mode.flag_armed;
/* reset setpoints and integrators VTOL in FW mode */
if (_vehicle_status.is_vtol && !_vehicle_status.is_rotary_wing) {
_reset_alt_sp = true;
@@ -2285,8 +2296,12 @@ MulticopterPositionControl::task_main()
_vel_sp_prev = _vel;
}
//Update previous arming state
was_armed = _control_mode.flag_armed;
/* switch to smooth takeoff if we got out of landed state */
if (!_vehicle_land_detected.landed && was_landed) {
_in_takeoff = true;
}
was_landed = _vehicle_land_detected.landed;
update_ref();
src/modules/mc_pos_control/mc_pos_control_params.c
+12
View File
@@ -543,3 +543,15 @@ PARAM_DEFINE_FLOAT(MPC_LAND_ALT1, 10.0f);
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.0f);
/**
* Position control smooth takeoff ramp time constant
*
* Increasing this value will make automatic and manual takeoff slower.
* If it's too slow the drone might scratch the ground and tip over.
*
* @min 0.1
* @max 1
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 0.4f);