mc: add SYS_VEHICLE_RESP param to configure vehicle responsiveness

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<typename T>
+const T lerp(const T &a, const T &b, const T &s)
+{
+	return (static_cast<T>(1) - s) * a + s * b;
+}
+
 } /* namespace math */

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();

src/modules/mc_pos_control/MulticopterPositionControl.hpp

@@ -135,7 +135,21 @@ private:
 		(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
 		(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */
 		(ParamFloat<px4::params::MPC_THR_MIN>) _param_mpc_thr_min,
-		(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max
+		(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max,
+
+		(ParamFloat<px4::params::SYS_VEHICLE_RESP>) _param_sys_vehicle_resp,
+		(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor,
+		(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,
+		(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
+		(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max,
+		(ParamFloat<px4::params::MPC_JERK_AUTO>) _param_mpc_jerk_auto,
+		(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
+		(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max,
+		(ParamFloat<px4::params::MPC_MAN_Y_TAU>) _param_mpc_man_y_tau,
+
+		(ParamFloat<px4::params::MPC_XY_VEL_ALL>) _param_mpc_xy_vel_all,
+		(ParamFloat<px4::params::MPC_Z_VEL_ALL>) _param_mpc_z_vel_all,
+		(ParamFloat<px4::params::MPC_LAND_VEL_XY>) _param_mpc_land_vel_xy
 	);
 
 	control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */

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);