From 7adab8f65e42bb83aff1fac325ff562377de7db3 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 24 May 2021 15:35:38 -0400
Subject: [PATCH] multicopter limit max throttle during clipping

---
 msg/vehicle_local_position.msg                  |  2 ++
 src/modules/ekf2/EKF2.cpp                       |  2 ++
 .../MulticopterPositionControl.cpp              | 17 ++++++++++++++++-
 .../MulticopterPositionControl.hpp              |  3 +++
 .../PositionControl/PositionControl.hpp         |  1 +
 5 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg
index 6f42547e43..62b714710a 100644
--- a/msg/vehicle_local_position.msg
+++ b/msg/vehicle_local_position.msg
@@ -68,5 +68,7 @@ float32 vz_max				# maximum vertical speed - set to 0 when limiting not required
 float32 hagl_min			# minimum height above ground level - set to 0 when limiting not required (meters)
 float32 hagl_max			# maximum height above ground level - set to 0 when limiting not required (meters)
 
+bool accelerometer_clipping
+
 # TOPICS vehicle_local_position vehicle_local_position_groundtruth
 # TOPICS estimator_local_position
diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index e9131a0ece..fb03bf9bcb 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -848,6 +848,8 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 		lpos.hagl_max = INFINITY;
 	}
 
+	lpos.accelerometer_clipping = _ekf.fault_status_flags().bad_acc_clipping;
+
 	// publish vehicle local position data
 	lpos.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_local_position_pub.publish(lpos);
diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp
index 68c037b30a..8c2cd22931 100644
--- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp
+++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp
@@ -180,6 +180,12 @@ int MulticopterPositionControl::parameters_update(bool force)
 			_hover_thrust_initialized = true;
 		}
 
+		_maximum_thrust_slew.setSlewRate(_param_mpc_thr_max.get() - _param_mpc_thr_min.get());
+
+		if (_maximum_thrust_slew.getState() <= FLT_EPSILON) {
+			_maximum_thrust_slew.setForcedValue(_param_mpc_thr_max.get());
+		}
+
 		// initialize vectors from params and enforce constraints
 		_param_mpc_tko_speed.set(math::min(_param_mpc_tko_speed.get(), _param_mpc_z_vel_max_up.get()));
 		_param_mpc_land_speed.set(math::min(_param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get()));
@@ -394,7 +400,16 @@ void MulticopterPositionControl::Run()
 			// Allow ramping from zero thrust on takeoff
 			const float minimum_thrust = flying ? _param_mpc_thr_min.get() : 0.f;
 
-			_control.setThrustLimits(minimum_thrust, _param_mpc_thr_max.get());
+			if (local_pos.accelerometer_clipping) {
+				// if accelerometer is currently clipping limit maximum thrust to hover + minimal margin
+				float maximum_thrust = _control.getHoverThrust() + 0.1f * (_param_mpc_thr_max.get() - _control.getHoverThrust());
+				_maximum_thrust_slew.update(maximum_thrust, dt);
+
+			} else {
+				_maximum_thrust_slew.update(_param_mpc_thr_max.get(), dt);
+			}
+
+			_control.setThrustLimits(minimum_thrust, _maximum_thrust_slew.getState());
 
 			_control.setVelocityLimits(
 				math::constrain(speed_horizontal, 0.f, _param_mpc_xy_vel_max.get()),
diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp
index 3b0f13228a..ac14d217c3 100644
--- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp
+++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp
@@ -44,6 +44,7 @@
 #include <lib/controllib/blocks.hpp>
 #include <lib/hysteresis/hysteresis.h>
 #include <lib/perf/perf_counter.h>
+#include <lib/slew_rate/SlewRate.hpp>
 #include <lib/slew_rate/SlewRateYaw.hpp>
 #include <lib/systemlib/mavlink_log.h>
 #include <px4_platform_common/px4_config.h>
@@ -182,6 +183,8 @@ private:
 
 	PositionControl _control; /**< class for core PID position control */
 
+	SlewRate<float> _maximum_thrust_slew;
+
 	hrt_abstime _last_warn{0}; /**< timer when the last warn message was sent out */
 
 	bool _in_failsafe{false}; /**< true if failsafe was entered within current cycle */
diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
index c486499e54..a80129adb9 100644
--- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
@@ -118,6 +118,7 @@ public:
 	 * @param thrust [0.1, 0.9] with which the vehicle hovers not acelerating down or up with level orientation
 	 */
 	void setHoverThrust(const float hover_thrust) { _hover_thrust = math::constrain(hover_thrust, 0.1f, 0.9f); }
+	float getHoverThrust() const { return _hover_thrust; }
 
 	/**
 	 * Update the hover thrust without immediately affecting the output