PositionControl: acceleration based control strategy

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -40,6 +40,7 @@
 #include <float.h>
 #include <mathlib/mathlib.h>
 #include <px4_platform_common/defines.h>
+#include <ecl/geo/geo.h>
 
 using namespace matrix;
 
@@ -83,7 +84,6 @@ void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &
 	_pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
 	_vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);
 	_acc_sp = Vector3f(setpoint.acceleration);
-	_thr_sp = Vector3f(setpoint.thrust);
 	_yaw_sp = setpoint.yaw;
 	_yawspeed_sp = setpoint.yawspeed;
 }
@@ -114,7 +114,7 @@ bool PositionControl::update(const float dt)
 	// x and y input setpoints always have to come in pairs
 	const bool valid = (PX4_ISFINITE(_pos_sp(0)) == PX4_ISFINITE(_pos_sp(1)))
 			   && (PX4_ISFINITE(_vel_sp(0)) == PX4_ISFINITE(_vel_sp(1)))
-			   && (PX4_ISFINITE(_thr_sp(0)) == PX4_ISFINITE(_thr_sp(1)));
+			   && (PX4_ISFINITE(_acc_sp(0)) == PX4_ISFINITE(_acc_sp(1)));
 
 	_positionControl();
 	_velocityControl(dt);
@@ -143,42 +143,16 @@ void PositionControl::_positionControl()
 
 void PositionControl::_velocityControl(const float dt)
 {
-	// Generate desired thrust setpoint.
-	// PID
-	// u_des = P(vel_err) + D(vel_err_dot) + I(vel_integral)
-	// Umin <= u_des <= Umax
-	//
-	// Anti-Windup:
-	// u_des = _thr_sp; r = _vel_sp; y = _vel
-	// u_des >= Umax and r - y >= 0 => Saturation = true
-	// u_des >= Umax and r - y <= 0 => Saturation = false
-	// u_des <= Umin and r - y <= 0 => Saturation = true
-	// u_des <= Umin and r - y >= 0 => Saturation = false
-	//
-	// 	Notes:
-	// - PID implementation is in NED-frame
-	// - control output in D-direction has priority over NE-direction
-	// - the equilibrium point for the PID is at hover-thrust
-	// - the maximum tilt cannot exceed 90 degrees. This means that it is
-	// 	 not possible to have a desired thrust direction pointing in the positive
-	// 	 D-direction (= downward)
-	// - the desired thrust in D-direction is limited by the thrust limits
-	// - the desired thrust in NE-direction is limited by the thrust excess after
-	// 	 consideration of the desired thrust in D-direction. In addition, the thrust in
-	// 	 NE-direction is also limited by the maximum tilt.
-
 	// PID velocity control
 	Vector3f vel_error = _vel_sp - _vel;
-	Vector3f thr_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int + _vel_dot.emult(_gain_vel_d);
+	Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int + _vel_dot.emult(_gain_vel_d);
-	thr_sp_velocity -= Vector3f(0.f, 0.f, _hover_thrust);
 
-	if (PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(thr_sp_velocity(2))) {
+	// For backwards compatibility of the gains to non-acceleration based control, needs to be overcome with configuration conversion
-		// Thrust set-point in NE-direction from FlightTaskManualAltitude is provided. Scaling by the maximum tilt is required.
+	acc_sp_velocity *= CONSTANTS_ONE_G / _hover_thrust;
-		_thr_sp.xy() = Vector2f(_thr_sp) * fabsf(thr_sp_velocity(2)) * tanf(_constraints.tilt);
+	// No control input from setpoints or corresponding states which are NAN
-	}
+	ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity);
 
-	// Velocity and feed-forward thrust setpoints or velocity states being NAN results in them not having an influence
+	_accelerationControl();
-	ControlMath::addIfNotNanVector3f(_thr_sp, thr_sp_velocity);
 
 	// Integrator anti-windup in vertical direction
 	if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.0f) ||
@@ -186,23 +160,23 @@ void PositionControl::_velocityControl(const float dt)
 		vel_error(2) = 0.f;
 	}
 
-	// Saturate thrust setpoint in D-direction.
+	// Saturate maximal vertical thrust
-	_thr_sp(2) = math::constrain(_thr_sp(2), -_lim_thr_max, -_lim_thr_min);
+	_thr_sp(2) = math::max(_thr_sp(2), -_lim_thr_max);
 
-	// Get maximum allowed thrust in NE based on tilt and excess thrust.
+	// Get allowed horizontal thrust after prioritizing vertical control
-	float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
+	const float thrust_max_squared = _lim_thr_max * _lim_thr_max;
-	float thrust_max_NE = sqrtf(_lim_thr_max * _lim_thr_max - _thr_sp(2) * _thr_sp(2));
+	const float thrust_z_squared = _thr_sp(2) * _thr_sp(2);
-	thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);
+	float thrust_max_xy = sqrtf(thrust_max_squared - thrust_z_squared);
 
-	// Saturate thrust in NE-direction.
+	// Saturate thrust in horizontal direction
 	const Vector2f thrust_sp_xy(_thr_sp);
 	const float thrust_sp_xy_norm = thrust_sp_xy.norm();
 
-	if (thrust_sp_xy_norm > thrust_max_NE) {
+	if (thrust_sp_xy_norm > thrust_max_xy) {
-		_thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_NE;
+		_thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy;
 	}
 
-	// Use tracking Anti-Windup for NE-direction: during saturation, the integrator is used to unsaturate the output
+	// Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output
 	// see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
 	const float arw_gain = 2.f / _gain_vel_p(0);
 	vel_error.xy() = Vector2f(vel_error) - (arw_gain * (thrust_sp_xy - Vector2f(_thr_sp)));
@@ -216,6 +190,19 @@ void PositionControl::_velocityControl(const float dt)
 	_vel_int(2) = math::min(fabsf(_vel_int(2)), _lim_thr_max) * sign(_vel_int(2));
 }
 
+void PositionControl::_accelerationControl()
+{
+	// Assume standard acceleration due to gravity in vertical direction for attitude generation
+	Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized();
+	ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _constraints.tilt);
+	// Scale thrust assuming hover thrust produces standard gravity
+	float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;
+	// Project thrust to planned body attitude
+	collective_thrust /= (Vector3f(0, 0, 1).dot(body_z));
+	collective_thrust = math::min(collective_thrust, -_lim_thr_min);
+	_thr_sp = body_z * collective_thrust;
+}
+
 bool PositionControl::_updateSuccessful()
 {
 	bool valid = true;
@@ -231,8 +218,9 @@ bool PositionControl::_updateSuccessful()
 		}
 	}
 
-	// There has to be a valid output thrust setpoint otherwise there was no
+	// There has to be a valid output accleration and thrust setpoint otherwise there was no
 	// setpoint-state pair for each axis that can get controlled
+	valid = valid && PX4_ISFINITE(_acc_sp(0)) && PX4_ISFINITE(_acc_sp(1)) && PX4_ISFINITE(_acc_sp(2));
 	valid = valid && PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(_thr_sp(2));
 	return valid;
 }

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -189,6 +189,7 @@ private:
 
 	void _positionControl(); ///< Position proportional control
 	void _velocityControl(const float dt); ///< Velocity PID control
+	void _accelerationControl(); ///< Acceleration setpoint processing
 
 	// Gains
 	matrix::Vector3f _gain_pos_p; ///< Position control proportional gain

src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp

@@ -188,17 +188,40 @@ TEST_F(PositionControlBasicTest, VelocityLimit)
 	EXPECT_LE(abs(_output_setpoint.vz), 1.f);
 }
 
-TEST_F(PositionControlBasicTest, ThrustLimit)
+TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit)
 {
 	_input_setpoint.x = 10.f;
 	_input_setpoint.y = 10.f;
 	_input_setpoint.z = -10.f;
 
-	EXPECT_TRUE(runController());
+	runController();
-	EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f);
+	Vector3f thrust(_output_setpoint.thrust);
-	EXPECT_FLOAT_EQ(_attitude.thrust_body[1], 0.f);
+	EXPECT_FLOAT_EQ(thrust(0), 0.f);
-	EXPECT_LT(_attitude.thrust_body[2], -.1f);
+	EXPECT_FLOAT_EQ(thrust(1), 0.f);
-	EXPECT_GE(_attitude.thrust_body[2], -0.9f);
+	EXPECT_FLOAT_EQ(thrust(2), -0.9f);
+
+	EXPECT_EQ(_attitude.thrust_body[0], 0.f);
+	EXPECT_EQ(_attitude.thrust_body[1], 0.f);
+	EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.9f);
+
+	EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f);
+	EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f);
+}
+
+TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit)
+{
+	_input_setpoint.x = 10.f;
+	_input_setpoint.y = 0.f;
+	_input_setpoint.z = 10.f;
+
+	runController();
+	Vector3f thrust(_output_setpoint.thrust);
+	EXPECT_FLOAT_EQ(thrust.length(), 0.1f);
+
+	EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.1f);
+
+	EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f);
+	EXPECT_FLOAT_EQ(_attitude.pitch_body, -1.f);
 }
 
 TEST_F(PositionControlBasicTest, FailsafeInput)
@@ -216,6 +239,17 @@ TEST_F(PositionControlBasicTest, FailsafeInput)
 	EXPECT_LE(_attitude.thrust_body[2], -.1f);
 }
 
+TEST_F(PositionControlBasicTest, IdleThrustInput)
+{
+	// High downwards acceleration to make sure there's no thrust
+	Vector3f(0.f, 0.f, 100.f).copyTo(_input_setpoint.acceleration);
+
+	EXPECT_TRUE(runController());
+	EXPECT_FLOAT_EQ(_output_setpoint.thrust[0], 0.f);
+	EXPECT_FLOAT_EQ(_output_setpoint.thrust[1], 0.f);
+	EXPECT_FLOAT_EQ(_output_setpoint.thrust[2], -.1f);
+}
+
 TEST_F(PositionControlBasicTest, InputCombinationsPosition)
 {
 	_input_setpoint.x = .1f;
@@ -259,16 +293,16 @@ TEST_F(PositionControlBasicTest, SetpointValiditySimple)
 	EXPECT_FALSE(runController());
 	_input_setpoint.y = .2f;
 	EXPECT_FALSE(runController());
-	_input_setpoint.thrust[2] = .3f;
+	_input_setpoint.acceleration[2] = .3f;
 	EXPECT_TRUE(runController());
 }
 
 TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations)
 {
-	// This test runs any position, velocity, thrust setpoint combination and checks if it gets accepted or rejected correctly
+	// This test runs any combination of set and unset (NAN) setpoints and checks if it gets accepted or rejected correctly
-	float *const setpoint_loop_access_map[] = {&_input_setpoint.x, &_input_setpoint.vx, &_input_setpoint.thrust[0],
+	float *const setpoint_loop_access_map[] = {&_input_setpoint.x, &_input_setpoint.vx, &_input_setpoint.acceleration[0],
-						   &_input_setpoint.y, &_input_setpoint.vy, &_input_setpoint.thrust[1],
+						   &_input_setpoint.y, &_input_setpoint.vy, &_input_setpoint.acceleration[1],
-						   &_input_setpoint.z, &_input_setpoint.vz, &_input_setpoint.thrust[2]
+						   &_input_setpoint.z, &_input_setpoint.vz, &_input_setpoint.acceleration[2]
 						  };
 
 	for (int combination = 0; combination < 512; combination++) {
@@ -289,16 +323,17 @@ TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations)
 		const bool has_xy_pairs = (combination & 7) == ((combination >> 3) & 7);
 		const bool expected_result = has_x_setpoint && has_y_setpoint && has_z_setpoint && has_xy_pairs;
 
-		EXPECT_EQ(runController(), expected_result) << "combination " << combination
+		EXPECT_EQ(runController(), expected_result) << "combination " << combination << std::endl
-				<< std::endl << "input" << std::endl
+				<< "input" << std::endl
-				<< "position " << _input_setpoint.x << ", " << _input_setpoint.y << ", " << _input_setpoint.z << std::endl
+				<< "position     " << _input_setpoint.x << ", " << _input_setpoint.y << ", " << _input_setpoint.z << std::endl
-				<< "velocity " << _input_setpoint.vx << ", " << _input_setpoint.vy << ", " << _input_setpoint.vz << std::endl
+				<< "velocity     " << _input_setpoint.vx << ", " << _input_setpoint.vy << ", " << _input_setpoint.vz << std::endl
-				<< "thrust   " << _input_setpoint.thrust[0] << ", " << _input_setpoint.thrust[1] << ", " << _input_setpoint.thrust[2]
+				<< "acceleration " << _input_setpoint.acceleration[0] << ", "
-				<< std::endl << "output" << std::endl
+				<< _input_setpoint.acceleration[1] << ", " << _input_setpoint.acceleration[2] << std::endl
-				<< "position " << _output_setpoint.x << ", " << _output_setpoint.y << ", " << _output_setpoint.z << std::endl
+				<< "output" << std::endl
-				<< "velocity " << _output_setpoint.vx << ", " << _output_setpoint.vy << ", " << _output_setpoint.vz << std::endl
+				<< "position     " << _output_setpoint.x << ", " << _output_setpoint.y << ", " << _output_setpoint.z << std::endl
-				<< "thrust   " << _output_setpoint.thrust[0] << ", " << _output_setpoint.thrust[1] << ", " << _output_setpoint.thrust[2]
+				<< "velocity     " << _output_setpoint.vx << ", " << _output_setpoint.vy << ", " << _output_setpoint.vz << std::endl
-				<< std::endl;
+				<< "acceleration " << _output_setpoint.acceleration[0] << ", "
+				<< _output_setpoint.acceleration[1] << ", " << _output_setpoint.acceleration[2] << std::endl;
 	}
 }
 

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -637,7 +637,7 @@ MulticopterPositionControl::Run()
 			if (not_taken_off || flying_but_ground_contact) {
 				// we are not flying yet and need to avoid any corrections
 				reset_setpoint_to_nan(setpoint);
-				setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f;
+				Vector3f(0.f, 0.f, 100.f).copyTo(setpoint.acceleration); // High downwards acceleration to make sure there's no thrust
 				// set yaw-sp to current yaw
 				// TODO: we need a clean way to disable yaw control
 				setpoint.yaw = _states.yaw;
@@ -684,7 +684,7 @@ MulticopterPositionControl::Run()
 			// Inform FlightTask about the input and output of the velocity controller
 			// This is used to properly initialize the velocity setpoint when onpening the position loop (position unlock)
 			_flight_tasks.updateVelocityControllerIO(Vector3f(local_pos_sp.vx, local_pos_sp.vy, local_pos_sp.vz),
-					Vector3f(local_pos_sp.thrust));
+					Vector3f(local_pos_sp.acceleration));
 
 			vehicle_attitude_setpoint_s attitude_setpoint{};
 			attitude_setpoint.timestamp = time_stamp_now;
@@ -949,7 +949,7 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint
 
 		if (PX4_ISFINITE(_states.velocity(0)) && PX4_ISFINITE(_states.velocity(1))) {
 			// don't move along xy
-			setpoint.vx = setpoint.vy = 0.0f;
+			setpoint.vx = setpoint.vy = 0.f;
 
 			if (warn) {
 				PX4_WARN("Failsafe: stop and wait");
@@ -957,7 +957,7 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint
 
 		} else {
 			// descend with land speed since we can't stop
-			setpoint.thrust[0] = setpoint.thrust[1] = 0.f;
+			setpoint.acceleration[0] = setpoint.acceleration[1] = 0.f;
 			setpoint.vz = _param_mpc_land_speed.get();
 
 			if (warn) {
@@ -974,7 +974,7 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint
 		} else {
 			// emergency descend with a bit below hover thrust
 			setpoint.vz = NAN;
-			setpoint.thrust[2] = _param_mpc_thr_hover.get() * .8f;
+			setpoint.acceleration[2] = .3f;
 
 			if (warn) {
 				PX4_WARN("Failsafe: blind descend");