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 9971a598b3..243237a3a5 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -121,8 +121,8 @@ public: int start(); private: - const float alt_ctl_dz = 0.1f; - const float alt_ctl_dy = 0.05f; + const float alt_ctl_dz = 0.05f; + const float alt_ctl_dy = 0.025f; bool _task_should_exit; /**< if true, task should exit */ int _control_task; /**< task handle for task */ @@ -167,6 +167,7 @@ private: struct { param_t thr_min; param_t thr_max; + param_t thr_hover; param_t z_p; param_t z_vel_p; param_t z_vel_i; @@ -199,6 +200,7 @@ private: struct { float thr_min; float thr_max; + float thr_hover; float tilt_max_air; float land_speed; float tko_speed; @@ -269,7 +271,8 @@ private: */ void poll_subscriptions(); - static float scale_control(float ctl, float end, float dz, float dy); + static float scale_control(float ctl, float end, float dz, float dy); + static float throttle_curve(float ctl, float ctr); /** * Update reference for local position projection @@ -419,6 +422,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.thr_min = param_find("MPC_THR_MIN"); _params_handles.thr_max = param_find("MPC_THR_MAX"); + _params_handles.thr_hover = param_find("MPC_THR_HOVER"); _params_handles.z_p = param_find("MPC_Z_P"); _params_handles.z_vel_p = param_find("MPC_Z_VEL_P"); _params_handles.z_vel_i = param_find("MPC_Z_VEL_I"); @@ -493,6 +497,7 @@ MulticopterPositionControl::parameters_update(bool force) /* update legacy C interface params */ param_get(_params_handles.thr_min, &_params.thr_min); param_get(_params_handles.thr_max, &_params.thr_max); + param_get(_params_handles.thr_hover, &_params.thr_hover); 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); @@ -639,13 +644,26 @@ float MulticopterPositionControl::scale_control(float ctl, float end, float dz, float dy) { if (ctl > dz) { - return dy + (ctl - dz) * (1.0f - dy) / (end - dz); + return dy + (ctl - dz) * (1.0f - dy) / (end - dz); } else if (ctl < -dz) { - return -dy + (ctl + dz) * (1.0f - dy) / (end - dz); + return -dy + (ctl + dz) * (1.0f - dy) / (end - dz); } else { - return ctl * (dy / dz); + return ctl * (dy / dz); + } +} + +float +MulticopterPositionControl::throttle_curve(float ctl, float ctr) +{ + /* piecewise linear mapping: 0:ctr -> 0:0.5 + * and ctr:1 -> 0.5:1 */ + if (ctl < 0.5f) { + return 2 * ctl * ctr; + + } else { + return ctr + 2 * (ctl - 0.5f) * (1.0f - ctr); } } @@ -738,8 +756,8 @@ MulticopterPositionControl::control_manual(float dt) if (_control_mode.flag_control_altitude_enabled) { /* set vertical velocity setpoint with throttle stick */ - req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz, alt_ctl_dy); // D - } + req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz, alt_ctl_dy); // D + } if (_control_mode.flag_control_position_enabled) { /* set horizontal velocity setpoint with roll/pitch stick */ @@ -1462,7 +1480,7 @@ MulticopterPositionControl::task_main() float i = _params.thr_min; if (reset_int_z_manual) { - i = _manual.z; + i = _params.thr_hover; if (i < _params.thr_min) { i = _params.thr_min; @@ -1840,7 +1858,8 @@ MulticopterPositionControl::task_main() /* control throttle directly if no climb rate controller is active */ if (!_control_mode.flag_control_climb_rate_enabled) { - _att_sp.thrust = math::min(_manual.z, _manual_thr_max.get()); + float thr_val = throttle_curve(_manual.z, _params.thr_hover); + _att_sp.thrust = math::min(thr_val, _manual_thr_max.get()); /* enforce minimum throttle if not landed */ if (!_vehicle_status.condition_landed) { 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 1cb4d36ba3..9be0c5af4c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -50,6 +50,17 @@ */ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); +/** + * Hover thrust + * + * Vertical thrust required to hover. + * + * @min 0.2 + * @max 0.8 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_HOVER, 0.5f); + /** * Maximum thrust in auto thrust control *