From ffee103ae05647a4e7936ec828905bcf037d1c6b Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Sat, 9 Nov 2019 14:20:57 +0100
Subject: [PATCH] FlightTask: add 1st order lpf on yawrate satepoint for smooth
 motion

---
 .../FlightTaskManualAltitude.cpp              | 51 ++++++++++++++-----
 .../FlightTaskManualAltitude.hpp              |  6 +++
 .../mc_pos_control/mc_pos_control_params.c    | 12 +++++
 3 files changed, 57 insertions(+), 12 deletions(-)

diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
index 3e13f0deba..d5e653dfa6 100644
--- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
@@ -85,13 +85,22 @@ bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s last_s
 
 void FlightTaskManualAltitude::_scaleSticks()
 {
-	// Use sticks input with deadzone and exponential curve for vertical velocity and yawspeed
-	_yawspeed_setpoint = _sticks_expo(3) * math::radians(_param_mpc_man_y_max.get());
+	// Use stick input with deadzone, exponential curve and first order lpf for yawspeed
+	const float yawspeed_target = _sticks_expo(3) * math::radians(_param_mpc_man_y_max.get());
+	_yawspeed_setpoint = _applyYawspeedFilter(yawspeed_target);
 
+	// Use sticks input with deadzone and exponential curve for vertical velocity
 	const float vel_max_z = (_sticks(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
 	_velocity_setpoint(2) = vel_max_z * _sticks_expo(2);
 }
 
+float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target)
+{
+	const float den = math::max(_param_mpc_man_y_tau.get() + _deltatime, 0.001f);
+	const float alpha = _deltatime / den;
+	return (1.f - alpha) * _yawspeed_setpoint + alpha * yawspeed_target;
+}
+
 void FlightTaskManualAltitude::_updateAltitudeLock()
 {
 	// Depending on stick inputs and velocity, position is locked.
@@ -289,18 +298,36 @@ void FlightTaskManualAltitude::_rotateIntoHeadingFrame(Vector2f &v)
 
 void FlightTaskManualAltitude::_updateHeadingSetpoints()
 {
-	/* Yaw-lock depends on stick input. If not locked,
-	 * yaw_sp is set to NAN.
-	 * TODO: add yawspeed to get threshold.*/
-	if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) {
-		// no fixed heading when rotating around yaw by stick
-		_yaw_setpoint = NAN;
+	if (_isYawInput()) {
+		_unlockYaw();
 
 	} else {
-		// hold the current heading when no more rotation commanded
-		if (!PX4_ISFINITE(_yaw_setpoint)) {
-			_yaw_setpoint = _yaw;
-		}
+		_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;
+		_yawspeed_setpoint = 0.f;
 	}
 }
 
diff --git a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
index e77687fb96..4b25bda933 100644
--- a/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
+++ b/src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
@@ -76,6 +76,7 @@ protected:
 					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy,
 					(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p, /**< position controller altitude propotional gain */
 					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */
+					(ParamFloat<px4::params::MPC_MAN_Y_TAU>) _param_mpc_man_y_tau,
 					(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */
 					(ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1, /**< altitude at which to start downwards slowdown */
 					(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< altitude below wich to land with land speed */
@@ -85,6 +86,11 @@ protected:
 					_param_mpc_tko_speed /**< desired upwards speed when still close to the ground */
 				       )
 private:
+	bool _isYawInput();
+	void _unlockYaw();
+	void _lockYaw();
+	float _applyYawspeedFilter(float yawspeed_target);
+
 	/**
 	 * Terrain following.
 	 * During terrain following, the position setpoint is adjusted
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 7a9871f433..92ba63de3f 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -395,6 +395,18 @@ PARAM_DEFINE_FLOAT(MPC_MAN_TILT_MAX, 35.0f);
  */
 PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 150.0f);
 
+/**
+ * Manual yaw rate input filter time constant
+ * Setting this parameter to 0 disables the filter
+ *
+ * @unit s
+ * @min 0.0
+ * @max 5.0
+ * @decimal 2
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_MAN_Y_TAU, 0.08f);
+
 /**
  * Deadzone of sticks where position hold is enabled
  *