feat(mpc): add MPC_VEL_ONLY to only apply horizontal velocity control

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@gmail.com>

src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -127,7 +127,12 @@ void FlightTaskManualPosition::_updateSetpoints()
 	FlightTaskManualAltitude::_updateSetpoints(); // needed to get yaw and setpoints in z-direction
 	_acceleration_setpoint.setNaN(); // don't use the horizontal setpoints from FlightTaskAltitude
 
-	_updateXYlock(); // check for position lock
+	if (_param_mpc_vel_only.get()) {
+		_position_setpoint(0) = NAN;
+		_position_setpoint(1) = NAN;
+	} else {
+		_updateXYlock(); // check for position lock
+	}
 
 	_weathervane.update();
 

src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp

@@ -62,7 +62,8 @@ protected:
 					(ParamFloat<px4::params::MPC_VEL_MAN_SIDE>) _param_mpc_vel_man_side,
 					(ParamFloat<px4::params::MPC_VEL_MAN_BACK>) _param_mpc_vel_man_back,
 					(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max,
-					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy
+					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy,
+					(ParamBool<px4::params::MPC_VEL_ONLY>) _param_mpc_vel_only
 				       )
 private:
 	uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */

src/modules/mc_pos_control/multicopter_position_control_params.c

@@ -130,3 +130,14 @@ PARAM_DEFINE_FLOAT(MPC_VEL_NF_BW, 5.0f);
  * @group Multicopter Position Control
  */
 PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f);
+
+/**
+ * Disable horizontal position lock in manual position mode
+ *
+ * Disable horizontal position lock when the sticks are centered in position controlled modes
+ * and when the vehicle is not moving.
+ *
+ * @boolean
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_INT32(MPC_VEL_ONLY, 0);