Do not handle yaw stick multiple times in Altitude and Position mode

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp

@@ -61,9 +61,6 @@ bool FlightTaskManualAcceleration::update()
 {
 	bool ret = FlightTaskManualAltitudeSmoothVel::update();
 
-	_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _is_yaw_good_for_control,
-				       _deltatime);
-
 	_stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _yaw_setpoint, _position,
 			_velocity_setpoint_feedback.xy(), _deltatime);
 	_stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint);

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp

@@ -58,7 +58,5 @@ class FlightTaskManualAcceleration : public FlightTaskManualAltitudeSmoothVel
 	void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override;
 
 	StickAccelerationXY _stick_acceleration_xy{this};
-	StickYaw _stick_yaw{this};
-
 	WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
 };

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -90,10 +90,6 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
 
 void FlightTaskManualAltitude::_scaleSticks()
 {
-	// Use stick input with deadzone, exponential curve and first order lpf for yawspeed
-	_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw,
-				       _is_yaw_good_for_control, _deltatime);
-
 	// Use sticks input with deadzone and exponential curve for vertical velocity
 	const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _param_mpc_z_vel_max_dn.get() :
 				_param_mpc_z_vel_max_up.get();
@@ -273,40 +269,6 @@ void FlightTaskManualAltitude::_respectGroundSlowdown()
 	}
 }
 
-void FlightTaskManualAltitude::_updateHeadingSetpoints()
-{
-	if (_isYawInput() || !_is_yaw_good_for_control) {
-		_unlockYaw();
-
-	} else {
-		_lockYaw();
-	}
-}
-
-bool FlightTaskManualAltitude::_isYawInput()
-{
-	/*
-	 * A threshold larger than FLT_EPSILON is required because the
-	 * _yawspeed_setpoint comes from an IIR filter and takes too much
-	 * time to reach zero.
-	 */
-	return fabsf(_yawspeed_setpoint) > 0.001f;
-}
-
-void FlightTaskManualAltitude::_unlockYaw()
-{
-	// no fixed heading when rotating around yaw by stick
-	_yaw_setpoint = NAN;
-}
-
-void FlightTaskManualAltitude::_lockYaw()
-{
-	// hold the current heading when no more rotation commanded
-	if (!PX4_ISFINITE(_yaw_setpoint)) {
-		_yaw_setpoint = _yaw;
-	}
-}
-
 void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi)
 {
 	// Only reset the yaw setpoint when the heading is locked
@@ -317,7 +279,8 @@ void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi)
 
 void FlightTaskManualAltitude::_updateSetpoints()
 {
-	_updateHeadingSetpoints(); // get yaw setpoint
+	_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw,
+				       _is_yaw_good_for_control, _deltatime);
 	_acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw,
 				      _yaw_setpoint);
 	_updateAltitudeLock();

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -56,7 +56,6 @@ class FlightTaskManualAltitude : public FlightTask
 	bool update() override;
 
 protected:
-	void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */
 	void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */
 	virtual void _updateSetpoints(); /**< updates all setpoints */
 	virtual void _scaleSticks(); /**< scales sticks to velocity in z */
@@ -89,10 +88,6 @@ class FlightTaskManualAltitude : public FlightTask
 					_param_mpc_tko_speed /**< desired upwards speed when still close to the ground */
 				       )
 private:
-	bool _isYawInput();
-	void _unlockYaw();
-	void _lockYaw();
-
 	/**
 	 * Terrain following.
 	 * During terrain following, the position setpoint is adjusted

src/modules/mc_pos_control/multicopter_altitude_mode_params.c

@@ -35,25 +35,25 @@
  * Maximum upwards acceleration in climb rate controlled modes
  *
  * @unit m/s^2
- * @min 2.0
- * @max 15.0
+ * @min 2
+ * @max 15
  * @decimal 1
  * @increment 1
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_ACC_UP_MAX, 4.0f);
+PARAM_DEFINE_FLOAT(MPC_ACC_UP_MAX, 4.f);
 
 /**
  * Maximum downwards acceleration in climb rate controlled modes
  *
  * @unit m/s^2
- * @min 2.0
- * @max 15.0
+ * @min 2
+ * @max 15
  * @decimal 1
  * @increment 1
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_ACC_DOWN_MAX, 3.0f);
+PARAM_DEFINE_FLOAT(MPC_ACC_DOWN_MAX, 3.f);
 
 /**
  * Manual yaw rate input filter time constant
@@ -62,8 +62,8 @@ PARAM_DEFINE_FLOAT(MPC_ACC_DOWN_MAX, 3.0f);
  * Setting this parameter to 0 disables the filter
  *
  * @unit s
- * @min 0.0
- * @max 5.0
+ * @min 0
+ * @max 5
  * @decimal 2
  * @increment 0.01
  * @group Multicopter Position Control

src/modules/mc_pos_control/multicopter_position_control_params.c

@@ -66,7 +66,7 @@ PARAM_DEFINE_INT32(MPC_USE_HTE, 1);
  * To avoid completely starving horizontal control with high vertical error.
  *
  * @unit norm
- * @min 0.0
+ * @min 0
  * @max 0.5
  * @decimal 2
  * @increment 0.01
@@ -78,7 +78,7 @@ PARAM_DEFINE_FLOAT(MPC_THR_XY_MARG, 0.3f);
  * Numerical velocity derivative low pass cutoff frequency
  *
  * @unit Hz
- * @min 0.0
+ * @min 0
  * @max 10
  * @decimal 1
  * @increment 0.5

src/modules/mc_pos_control/multicopter_stabilized_mode_params.c

@@ -64,8 +64,8 @@ PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 150.f);
  * Airmode is used to keep torque authority with zero thrust (see MC_AIRMODE).
  *
  * @unit norm
- * @min 0.0
- * @max 1.0
+ * @min 0
+ * @max 1
  * @decimal 2
  * @increment 0.01
  * @group Multicopter Position Control