From 7434bcc693018d0e27505c5b180ce568ef6a9404 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 21 Mar 2017 18:06:15 +0100 Subject: [PATCH] mc_pos_control: fixed rebase and refactor errors --- .../mc_pos_control/mc_pos_control_main.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 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 8be7234ae7..417e608fcb 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -169,13 +169,12 @@ private: control::BlockParamFloat _hold_dz; /**< deadzone around the center for the sticks when flying in position mode */ control::BlockParamFloat _acceleration_hor_max; /**< maximum velocity setpoint slewrate while decelerating */ 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::BlockDerivative _vel_x_deriv; control::BlockDerivative _vel_y_deriv; control::BlockDerivative _vel_z_deriv; - control::BlockParamFloat _target_threshold_xy; - struct { param_t thr_min; param_t thr_max; @@ -232,7 +231,6 @@ private: float vel_cruise_xy; float vel_max_up; float vel_max_down; - float xy_vel_man_expo; uint32_t alt_mode; int opt_recover; @@ -444,10 +442,10 @@ MulticopterPositionControl::MulticopterPositionControl() : _hold_dz(this, "HOLD_DZ"), _acceleration_hor_max(this, "ACC_HOR_MAX", true), _deceleration_hor_max(this, "DEC_HOR_MAX", true), + _target_threshold_xy(this, "MPC_TARGET_THRE"), _vel_x_deriv(this, "VELD"), _vel_y_deriv(this, "VELD"), _vel_z_deriv(this, "VELD"), - _target_threshold_xy(this, "MPC_TARGET_THRE"), _ref_alt(0.0f), _ref_timestamp(0), _reset_pos_sp(true), @@ -628,8 +626,9 @@ MulticopterPositionControl::parameters_update(bool force) v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(2) = v; param_get(_params_handles.hold_max_xy, &v); + _params.hold_max_xy = math::max(0.f, v); param_get(_params_handles.hold_max_z, &v); - _params.hold_max_z = (v < 0.0f ? 0.0f : v); + _params.hold_max_z = math::max(0.f, v); param_get(_params_handles.acc_up_max, &v); _params.acc_up_max = v; param_get(_params_handles.acc_down_max, &v); @@ -911,8 +910,9 @@ MulticopterPositionControl::limit_vel_xy_gradually() * the max velocity is defined by the linear line * with x= (curr_sp - pos) and y = _vel_sp */ + math::Vector<3> dist = _curr_pos_sp - _pos; float slope = get_cruising_speed_xy() / _target_threshold_xy.get(); - float vel_limit = slope * (_curr_pos_sp - _pos).length(); + float vel_limit = slope * sqrtf(dist(0) * dist(0) + dist(1) * dist(1)); float vel_mag_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1)); float vel_mag_valid = math::min(vel_mag_xy, vel_limit); _vel_sp(0) = _vel_sp(0) / vel_mag_xy * vel_mag_valid; @@ -1462,9 +1462,7 @@ void MulticopterPositionControl::control_auto(float dt) } } - /* set velocity limit if close to current setpoint and - * no next setpoint available: we only consider updated if xy is updated - */ + /* set velocity limit if close to current setpoint and no next setpoint available */ _limit_vel_xy = (!next_setpoint_valid || (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) && ((_curr_pos_sp - _pos).length() <= _target_threshold_xy.get());