From 05a73d2821246557e6ae44639210fbd93a73f4b8 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 20 Dec 2015 17:33:12 +0100 Subject: [PATCH] added takeoff logic for position controller to get the uav off the ground fast and transition smoothly to poctl after takeoff, added landing logic to reduce thrust to zero once on the ground --- .../mc_pos_control/mc_pos_control_main.cpp | 71 +++++++++++++++++-- .../mc_pos_control/mc_pos_control_params.c | 18 +++++ 2 files changed, 83 insertions(+), 6 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 00e6df0655..e98a4fcb58 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -180,6 +180,8 @@ private: param_t xy_ff; param_t tilt_max_air; param_t land_speed; + param_t tko_jmpspd; + param_t tko_speed; param_t tilt_max_land; param_t man_roll_max; param_t man_pitch_max; @@ -197,6 +199,8 @@ private: float thr_max; float tilt_max_air; float land_speed; + float tko_jmpspd; + float tko_speed; float tilt_max_land; float man_roll_max; float man_pitch_max; @@ -243,6 +247,8 @@ private: float _yaw; /**< yaw angle (euler) */ float _landing_thrust; hrt_abstime _landing_started; + int _takeoff_jumped; + float _vel_z_lp; /** * Update our local parameter cache. @@ -374,7 +380,9 @@ MulticopterPositionControl::MulticopterPositionControl() : _run_alt_control(true), _yaw(0.0f), _landing_thrust(1.0f), - _landing_started(0) + _landing_started(0), + _takeoff_jumped(0), + _vel_z_lp(0) { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_ctrl_state, 0, sizeof(_ctrl_state)); @@ -425,6 +433,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.xy_ff = param_find("MPC_XY_FF"); _params_handles.tilt_max_air = param_find("MPC_TILTMAX_AIR"); _params_handles.land_speed = param_find("MPC_LAND_SPEED"); + _params_handles.tko_jmpspd = param_find("MPC_TKO_JMPSPD"); + _params_handles.tko_speed = param_find("MPC_TKO_SPEED"); _params_handles.tilt_max_land = param_find("MPC_TILTMAX_LND"); _params_handles.man_roll_max = param_find("MPC_MAN_R_MAX"); _params_handles.man_pitch_max = param_find("MPC_MAN_P_MAX"); @@ -487,6 +497,8 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.tilt_max_air, &_params.tilt_max_air); _params.tilt_max_air = math::radians(_params.tilt_max_air); param_get(_params_handles.land_speed, &_params.land_speed); + param_get(_params_handles.tko_jmpspd, &_params.tko_jmpspd); + param_get(_params_handles.tko_speed, &_params.tko_speed); param_get(_params_handles.tilt_max_land, &_params.tilt_max_land); _params.tilt_max_land = math::radians(_params.tilt_max_land); @@ -1344,6 +1356,31 @@ MulticopterPositionControl::task_main() _vel_sp(2) = _params.land_speed; } + /* velocity handling during takeoff */ + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid + && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + + switch (_takeoff_jumped) { + case 0: + /* do a quick jump until we shoot over takeoff speed */ + _vel_sp(2) = -_params.tko_jmpspd; + + if (_vel(2) < -_params.tko_speed) { + _takeoff_jumped = 1; + } + break; + + case 1: + /* slowly reduce forced takeoff speed to set climb rate */ + float tmp = 0.3f * dt + _vel_sp_prev(2); + _vel_sp(2) = math::min(tmp, -_params.tko_speed); + break; + } + + } else { + _takeoff_jumped = 0; + } + // limit total horizontal acceleration math::Vector<2> acc_hor; acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt; @@ -1356,6 +1393,8 @@ MulticopterPositionControl::task_main() math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev; _vel_sp(0) = vel_sp_hor(0); _vel_sp(1) = vel_sp_hor(1); + + /*PX4_WARN("vel limited %.4f, %.4f", (double)_vel_sp(0), (double)_vel_sp(1));*/ } _vel_sp_prev = _vel_sp; @@ -1439,29 +1478,49 @@ MulticopterPositionControl::task_main() float thrust_abs = thrust_sp.length(); float tilt_max = _params.tilt_max_air; float thr_max = _params.thr_max; + _vel_z_lp = _vel_z_lp * 0.9f + 0.1f * _vel(2); /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.tilt_max_land; - if (_landing_started == 0) { - _landing_started = hrt_absolute_time(); - } if (thr_min < 0.0f) { thr_min = 0.0f; } + if (_landing_started == 0) { + _landing_started = hrt_absolute_time(); + } + float vel_z_change = (_vel(2) - _vel_prev(2)) / dt; + /* don't let it throttle up again during landing */ if (thrust_sp(2) < 0.0f && thrust_abs < _landing_thrust + /* fix landing thrust after a certain time when velocity change is minimal */ + && (float)fabs(vel_z_change) < 0.05f + && _vel_z_lp > 0.5f * _params.land_speed && hrt_elapsed_time(&_landing_started) > 15e5) { _landing_thrust = thrust_abs; } + + /* assume ground, reduce thrust */ + if (hrt_elapsed_time(&_landing_started) > 15e5 + && _landing_thrust > FLT_EPSILON + && _vel_z_lp < 0.1f) { + _landing_thrust = 0.0f; + } + + /* if we suddenly fall, reset landing logic and remove thrust limit */ + if (hrt_elapsed_time(&_landing_started) > 15e5 + && _landing_thrust < FLT_EPSILON + && vel_z_change > 4.0f) { + _landing_thrust = _params.thr_max; + _landing_started = 0; + } + /* add 5% to give it some margin to compensate external influences */ thr_max = _landing_thrust + 0.05f * _landing_thrust; - /*PX4_WARN("thrust: abs %.4f, sp(2) %.4f, _landing_thrust %.4f, vel_err(2) %.4f, vel_sp(2) %.4f, vel(2) %.4f", - (double) thrust_abs, (double) thrust_sp(2), (double) _landing_thrust, (double) vel_err(2), (double) _vel_sp(2), (double) _vel(2));*/ } else { _landing_started = 0; 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 65673a8284..7ef530c049 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -234,6 +234,24 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 15.0f); */ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); +/** + * Takeoff initial speed to jump off the ground + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TKO_JMPSPD, 2.5f); + +/** + * Takeoff climb rate + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TKO_SPEED, 1.5f); + /** * Max manual roll *