From 9a162a24bb31c8d7f5e89507a2099f984a68f6a4 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 24 Apr 2017 20:23:02 +0200 Subject: [PATCH] mc_pos_control: improved smooth takeoff and used it for manual takeoff as well 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 --- .../mc_pos_control/mc_pos_control_main.cpp | 55 ++++++++++++------- .../mc_pos_control/mc_pos_control_params.c | 12 ++++ 2 files changed, 47 insertions(+), 20 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index f02afccfae..94934baf05 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -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(); 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 153b3c4356..852b4e12be 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -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);