move takeoff state machine flight_mode_manager -> mc_pos_control

src/modules/mc_pos_control/PositionControl/CMakeLists.txt

@@ -32,13 +32,12 @@
 ############################################################################
 
 px4_add_library(PositionControl
-	PositionControl.cpp
 	ControlMath.cpp
+	ControlMath.hpp
+	PositionControl.cpp
+	PositionControl.hpp
 )
-target_include_directories(PositionControl
-	PUBLIC
-	${CMAKE_CURRENT_SOURCE_DIR}
-)
+target_include_directories(PositionControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} )
 
 px4_add_unit_gtest(SRC ControlMathTest.cpp LINKLIBS PositionControl)
 px4_add_unit_gtest(SRC PositionControlTest.cpp LINKLIBS PositionControl)

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -88,27 +88,6 @@ void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &
 	_yawspeed_sp = setpoint.yawspeed;
 }
 
-void PositionControl::setConstraints(const vehicle_constraints_s &constraints)
-{
-	_constraints = constraints;
-
-	// For safety check if adjustable constraints are below global constraints. If they are not stricter than global
-	// constraints, then just use global constraints for the limits.
-	if (!PX4_ISFINITE(constraints.tilt) || (constraints.tilt > _lim_tilt)) {
-		_constraints.tilt = _lim_tilt;
-	}
-
-	if (!PX4_ISFINITE(constraints.speed_up) || (constraints.speed_up > _lim_vel_up)) {
-		_constraints.speed_up = _lim_vel_up;
-	}
-
-	if (!PX4_ISFINITE(constraints.speed_down) || (constraints.speed_down > _lim_vel_down)) {
-		_constraints.speed_down = _lim_vel_down;
-	}
-
-	// ignore _constraints.speed_xy TODO: remove it completely as soon as no task uses it anymore to avoid confusion
-}
-
 bool PositionControl::update(const float dt)
 {
 	// x and y input setpoints always have to come in pairs
@@ -138,7 +117,7 @@ void PositionControl::_positionControl()
 	// the desired position setpoint over the feed-forward term.
 	_vel_sp.xy() = ControlMath::constrainXY(vel_sp_position.xy(), (_vel_sp - vel_sp_position).xy(), _lim_vel_horizontal);
 	// Constrain velocity in z-direction.
-	_vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down);
+	_vel_sp(2) = math::constrain(_vel_sp(2), -_lim_vel_up, _lim_vel_down);
 }
 
 void PositionControl::_velocityControl(const float dt)
@@ -198,7 +177,7 @@ 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);
+	ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_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

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -41,7 +41,6 @@
 
 #include <matrix/matrix/math.hpp>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_constraints.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 
 struct PositionControlStates {
@@ -139,13 +138,6 @@ public:
 	 */
 	void setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint);
 
-	/**
-	 * Pass constraints that are stricter than the global limits
-	 * Note: NAN value means no constraint, take maximum limit of controller.
-	 * @param constraints a PositionControl structure with supported constraints
-	 */
-	void setConstraints(const vehicle_constraints_s &constraints);
-
 	/**
 	 * Apply P-position and PID-velocity controller that updates the member
 	 * thrust, yaw- and yawspeed-setpoints.
@@ -209,8 +201,6 @@ private:
 	matrix::Vector3f _vel_int; /**< integral term of the velocity controller */
 	float _yaw{}; /**< current heading */
 
-	vehicle_constraints_s _constraints{}; /**< variable constraints */
-
 	// Setpoints
 	matrix::Vector3f _pos_sp; /**< desired position */
 	matrix::Vector3f _vel_sp; /**< desired velocity */

src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp

@@ -82,11 +82,6 @@ public:
 		_position_control.setTiltLimit(1.f);
 		_position_control.setHoverThrust(.5f);
 
-		_contraints.tilt = 1.f;
-		_contraints.speed_xy = NAN;
-		_contraints.speed_up = NAN;
-		_contraints.speed_down = NAN;
-
 		resetInputSetpoint();
 	}
 
@@ -106,7 +101,6 @@ public:
 
 	bool runController()
 	{
-		_position_control.setConstraints(_contraints);
 		_position_control.setInputSetpoint(_input_setpoint);
 		const bool ret = _position_control.update(.1f);
 		_position_control.getLocalPositionSetpoint(_output_setpoint);
@@ -115,7 +109,6 @@ public:
 	}
 
 	PositionControl _position_control;
-	vehicle_constraints_s _contraints{};
 	vehicle_local_position_setpoint_s _input_setpoint{};
 	vehicle_local_position_setpoint_s _output_setpoint{};
 	vehicle_attitude_setpoint_s _attitude{};
@@ -168,12 +161,14 @@ TEST_F(PositionControlBasicTest, TiltLimit)
 	EXPECT_GT(angle, 0.f);
 	EXPECT_LE(angle, 1.f);
 
-	_contraints.tilt = .5f;
+	_position_control.setTiltLimit(0.5f);
 	EXPECT_TRUE(runController());
 	body_z = Quatf(_attitude.q_d).dcm_z();
 	angle = acosf(body_z.dot(Vector3f(0.f, 0.f, 1.f)));
 	EXPECT_GT(angle, 0.f);
 	EXPECT_LE(angle, .50001f);
+
+	_position_control.setTiltLimit(1.f);  // restore original
 }
 
 TEST_F(PositionControlBasicTest, VelocityLimit)