From 14bf9cf7533fa588bd06c0e60bb44ad0c2c6db07 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 3 Feb 2021 13:25:37 +0100 Subject: [PATCH] mc: add SYS_VEHICLE_RESP param to configure vehicle responsiveness --- src/lib/mathlib/math/Functions.hpp | 12 +++++ .../MulticopterPositionControl.cpp | 51 +++++++++++++++++++ .../MulticopterPositionControl.hpp | 16 +++++- .../mc_pos_control/mc_pos_control_params.c | 48 +++++++++++++++++ 4 files changed, 126 insertions(+), 1 deletion(-) diff --git a/src/lib/mathlib/math/Functions.hpp b/src/lib/mathlib/math/Functions.hpp index 79c7dd56ce..610900581a 100644 --- a/src/lib/mathlib/math/Functions.hpp +++ b/src/lib/mathlib/math/Functions.hpp @@ -198,4 +198,16 @@ const T sqrt_linear(const T &value) } } +/* + * Linear interpolation between 2 points a, and b. + * s=0 return a + * s=1 returns b + * Any value for s is valid. + */ +template +const T lerp(const T &a, const T &b, const T &s) +{ + return (static_cast(1) - s) * a + s * b; +} + } /* namespace math */ diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index d2c4f6b47e..88f4a82eb0 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -88,6 +88,57 @@ int MulticopterPositionControl::parameters_update(bool force) ModuleParams::updateParams(); SuperBlock::updateParams(); + int num_changed = 0; + + if (_param_sys_vehicle_resp.get() >= 0.f) { + // make it less sensitive at the lower end + float responsiveness = _param_sys_vehicle_resp.get() * _param_sys_vehicle_resp.get(); + + num_changed += _param_mpc_acc_hor.commit_no_notification(math::lerp(1.f, 15.f, responsiveness)); + num_changed += _param_mpc_acc_hor_max.commit_no_notification(math::lerp(2.f, 15.f, responsiveness)); + num_changed += _param_mpc_man_y_max.commit_no_notification(math::lerp(80.f, 450.f, responsiveness)); + + if (responsiveness > 0.6f) { + num_changed += _param_mpc_man_y_tau.commit_no_notification(0.f); + + } else { + num_changed += _param_mpc_man_y_tau.commit_no_notification(math::lerp(0.5f, 0.f, responsiveness / 0.6f)); + } + + if (responsiveness < 0.5f) { + num_changed += _param_mpc_tiltmax_air.commit_no_notification(45.f); + + } else { + num_changed += _param_mpc_tiltmax_air.commit_no_notification(math::min(MAX_SAFE_TILT_DEG, math::lerp(45.f, 70.f, + (responsiveness - 0.5f) * 2.f))); + } + + num_changed += _param_mpc_acc_down_max.commit_no_notification(math::lerp(0.8f, 15.f, responsiveness)); + num_changed += _param_mpc_acc_up_max.commit_no_notification(math::lerp(1.f, 15.f, responsiveness)); + num_changed += _param_mpc_jerk_max.commit_no_notification(math::lerp(2.f, 50.f, responsiveness)); + num_changed += _param_mpc_jerk_auto.commit_no_notification(math::lerp(1.f, 25.f, responsiveness)); + } + + if (_param_mpc_xy_vel_all.get() >= 0.f) { + float xy_vel = _param_mpc_xy_vel_all.get(); + num_changed += _param_mpc_vel_manual.commit_no_notification(xy_vel); + num_changed += _param_mpc_xy_cruise.commit_no_notification(xy_vel); + num_changed += _param_mpc_xy_vel_max.commit_no_notification(xy_vel); + num_changed += _param_mpc_land_vel_xy.commit_no_notification(xy_vel * 0.75f); + } + + if (_param_mpc_z_vel_all.get() >= 0.f) { + float z_vel = _param_mpc_z_vel_all.get(); + num_changed += _param_mpc_z_vel_max_up.commit_no_notification(z_vel); + num_changed += _param_mpc_z_vel_max_dn.commit_no_notification(z_vel * 0.75f); + num_changed += _param_mpc_tko_speed.commit_no_notification(z_vel * 0.6f); + num_changed += _param_mpc_land_speed.commit_no_notification(z_vel * 0.5f); + } + + if (num_changed > 0) { + param_notify_changes(); + } + if (_param_mpc_tiltmax_air.get() > MAX_SAFE_TILT_DEG) { _param_mpc_tiltmax_air.set(MAX_SAFE_TILT_DEG); _param_mpc_tiltmax_air.commit(); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 4a3ebba521..2f5313b7d1 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -135,7 +135,21 @@ private: (ParamInt) _param_mpc_alt_mode, (ParamFloat) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */ (ParamFloat) _param_mpc_thr_min, - (ParamFloat) _param_mpc_thr_max + (ParamFloat) _param_mpc_thr_max, + + (ParamFloat) _param_sys_vehicle_resp, + (ParamFloat) _param_mpc_acc_hor, + (ParamFloat) _param_mpc_acc_down_max, + (ParamFloat) _param_mpc_acc_up_max, + (ParamFloat) _param_mpc_acc_hor_max, + (ParamFloat) _param_mpc_jerk_auto, + (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_man_y_max, + (ParamFloat) _param_mpc_man_y_tau, + + (ParamFloat) _param_mpc_xy_vel_all, + (ParamFloat) _param_mpc_z_vel_all, + (ParamFloat) _param_mpc_land_vel_xy ); control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ 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 332490345a..5ce5dd50d6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -789,3 +789,51 @@ PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 1.0f); * @group Mission */ PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); + +/** + * Responsiveness + * + * Changes the overall responsiveness of the vehicle. + * The higher the value, the faster the vehicle will react. + * + * If set to a value greater than zero, other parameters are automatically set (such as + * the acceleration or jerk limits). + * If set to -1, the existing individual parameters are used. + * + * @min -1 + * @max 1 + * @decimal 2 + * @increment 0.05 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(SYS_VEHICLE_RESP, -0.4f); + +/** + * Overall Horizonal Velocity Limit + * + * If set to a value greater than zero, other parameters are automatically set (such as + * MPC_XY_VEL_MAX or MPC_VEL_MANUAL). + * If set to -1, the existing individual parameters are used. + * + * @min -20 + * @max 20 + * @decimal 1 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_ALL, -10.0f); + +/** + * Overall Vertical Velocity Limit + * + * If set to a value greater than zero, other parameters are automatically set (such as + * MPC_Z_VEL_MAX_UP or MPC_LAND_SPEED). + * If set to -1, the existing individual parameters are used. + * + * @min -3 + * @max 8 + * @decimal 1 + * @increment 0.5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_ALL, -3.0f);