multicopter limit max throttle during clipping

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

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

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

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 */

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